diff --git a/README.md b/README.md index 8689820..755a6ab 100644 --- a/README.md +++ b/README.md @@ -97,16 +97,17 @@ And then examine the results: colcon test-result --all --verbose ``` -There are six tests implemented in [interface_test.cpp](test/interface_test.cpp): +There are seven tests implemented in [interface_test.cpp](test/interface_test.cpp): 1. **TransformToFCL:** To validate the `Eigen::Affine3d` to `fcl::Transform3d` transformation method. 2. **AddRemove:** Asserts that a variety of [shape_msgs](http://wiki.ros.org/shape_msgs) and [OctoMaps](https://github.com/OctoMap/octomap_msgs) can be added/removed to a collision world. 3. **AddRemoveVoxelGrid:** Same as "AddRemove", except focused on [VoxelGrids](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/VoxelGrid.msg). -4. **CollisionCheck**: Validates that FCL collision-checking capabilities operate correctly for ROS types exposed through this package (e.g., `shape_msgs`). -5. **DistanceCheck**: Validates that FCL distance computations are correct for ROS types exposed through this package (e.g., `shape_msgs`). -6. **OctomapCollDistCheck**: A combination of tests for FCL collision and distance checking when using `robot_collision_checking` as an interface to handle OctoMaps as collision geometries. +4. **NullPtrCheck**: Tests whether the library handles `nullptr` function arguments correctly. +5. **CollisionCheck**: Validates that FCL collision-checking capabilities operate correctly for ROS types exposed through this package (e.g., `shape_msgs`). +6. **DistanceCheck**: Validates that FCL distance computations are correct for ROS types exposed through this package (e.g., `shape_msgs`). +7. **OctomapCollDistCheck**: A combination of tests for FCL collision and distance checking when using `robot_collision_checking` as an interface to handle OctoMaps as collision geometries. -If all six tests pass, you can expect minimal output with a summary of "0 failures". If there are any errors, `test-result` will provide a detailed breakdown of the test(s) that failed and reason why in the terminal output. A test results file will also be stored in the `build/robot_collision_checking` directory of your ROS workspace (unless configured otherwise -- see [ROS tutorial docs](https://docs.ros.org/en/humble/Tutorials/Intermediate/Testing/CLI.html)). +If all seven tests pass, you can expect minimal output with a summary of "0 failures". If there are any errors, `test-result` will provide a detailed breakdown of the test(s) that failed and reason why in the terminal output. A test results file will also be stored in the `build/robot_collision_checking` directory of your ROS workspace (unless configured otherwise -- see [ROS tutorial docs](https://docs.ros.org/en/humble/Tutorials/Intermediate/Testing/CLI.html)). ## Examples diff --git a/docs/api.md b/docs/api.md index 43442d0..44796e0 100644 --- a/docs/api.md +++ b/docs/api.md @@ -5,6 +5,8 @@ between various object types in a robot environment, using the Flexible Collisio 1) [Types](#types) 2) [Core Functionality](#core-functionality) +3) [Error Handling](#error-handling) +4) [Testing](#testing) ## Types @@ -134,9 +136,12 @@ The package can be used by instantiating a collision world, `FCLInterfaceCollisi The `FCLInterfaceCollisionWorld` class is a container and manager for objects in a collision world, providing functionality to add, remove, and check collisions between objects. Its state is encapsulated by the following private members: ```c++ -std::string frame_; // Collision world reference frame -std::vector fcl_collision_objects_; // List of collision objects in the world -unsigned int obj_counter_; // Counter for object IDs +// Collision world reference frame +std::string frame_; +// List of collision objects in the world +std::vector fcl_collision_objects_; +// Counter for object IDs +unsigned int obj_counter_; ``` There are getter methods to expose these members. @@ -256,11 +261,25 @@ FCLCollisionGeometryPtr createCollisionGeometry(const shape_msgs::msg::SolidPrim } ``` -Refer to the [interface_test.cpp](../test/interface_test.cpp) test code for example usage of the methods defined in the `robot_collision_checking::fcl_interface` namespace. There are six tests implemented: +Refer to the [interface_test.cpp](../test/interface_test.cpp) test code for example usage of the methods defined in the `robot_collision_checking::fcl_interface` namespace. + +## Error Handling + +Errors in `robot_collision_checking` are handled by outputting an `RCLCPP_ERROR` message in the format: **"function_name: error_message"**. Depending on the function where the error occurs, there will also be specific return values, e.g.,: +- Functions with `bool` return types (e.g., `addCollisionObject`), will return `false` on an error. +- Functions with `int` or `double` as return types (e.g., `getMinimumObjectDistance`), will return `-1` or `-1.0`, respectively. +- `void` functions (e.g., `getObjectDistances`) with an error detected will immediately return from the function. +- Functions with pointer return types, like `FCLCollisionGeometryPtr` (e.g., for `createCollisionGeometry`), will return `nullptr` values on an error. + +## Testing + +There are seven tests implemented in [interface_test.cpp](../test/interface_test.cpp): 1. **TransformToFCL:** To validate the `Eigen::Affine3d` to `fcl::Transform3d` transformation method. 2. **AddRemove:** Asserts that a variety of [shape_msgs](http://wiki.ros.org/shape_msgs) and [OctoMaps](https://github.com/OctoMap/octomap_msgs) can be added/removed to a collision world (e.g., adding a sphere shape to a collision world). 3. **AddRemoveVoxelGrid:** Same as "AddRemove", except focused on [VoxelGrids](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/VoxelGrid.msg). -4. **CollisionCheck**: Validates that FCL collision-checking capabilities operate correctly for ROS types exposed through this package (e.g., testing a collision between a box and sphere in the same coordinate frame). -5. **DistanceCheck**: Validates that FCL distance computations are correct for ROS types exposed through this package (e.g., testing the distance calculations between different object geometries are correct). -6. **OctomapCollDistCheck**: A combination of tests for FCL collision and distance checking when using `robot_collision_checking` as an interface to handle OctoMaps as collision geometries. \ No newline at end of file +4. **NullPtrCheck**: Validates that the library outputs error messages for `nullptr` function arguments (e.g., if `nullptr` is passed to `addCollisionObject`). +5. **CollisionCheck**: Validates that FCL collision-checking capabilities operate correctly for ROS types exposed through this package (e.g., testing a collision between a box and sphere in the same coordinate frame). +6. **DistanceCheck**: Validates that FCL distance computations are correct for ROS types exposed through this package (e.g., testing the distance calculations between different object geometries are correct). +7. **OctomapCollDistCheck**: A combination of tests for FCL collision and distance checking when using `robot_collision_checking` as an interface to handle OctoMaps as collision geometries. + diff --git a/src/fcl_interface.cpp b/src/fcl_interface.cpp index 7d3bc24..1742151 100644 --- a/src/fcl_interface.cpp +++ b/src/fcl_interface.cpp @@ -19,8 +19,17 @@ namespace FCLCollisionGeometryPtr createCollisionGeometry(const FCLObjectPtr& obj) { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "createCollisionGeometry: called with a NULL FCLObjectPtr."); + return nullptr; + } + + // TODO: Find a better representation for voxel grid collision geometry if (obj->object_type == VOXEL_GRID) { + RCLCPP_ERROR(getLogger(), "createCollisionGeometry: cannot create collision geometry " + "for voxel grid. Need to break the grid up into cells."); return nullptr; } @@ -113,6 +122,12 @@ FCLCollisionGeometryPtr createCollisionGeometry(const octomap_msgs::msg::Octomap int checkCollisionObjectWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisionWorld& world) { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObjectWorld: called with a NULL FCLObjectPtr."); + return -1; + } + FCLCollisionGeometryPtr cg = createCollisionGeometry(obj); fcl::Transform3d world_to_fcl; transform2fcl(obj->object_transform, world_to_fcl); @@ -135,6 +150,12 @@ int checkCollisionObjectWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisi int checkCollisionObjectWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world) { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObjectWorld: called with a NULL FCLCollisionObjectPtr."); + return -1; + } + fcl::CollisionRequestd col_req; fcl::CollisionResultd col_result; std::vector world_objects = world.getCollisionObjects(); @@ -152,6 +173,12 @@ int checkCollisionObjectWorld(const FCLCollisionObjectPtr& co, const FCLInterfac bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2) { + if (obj1 == nullptr || obj2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObjects: called with a NULL FCLObjectPtr."); + return false; + } + // Create the collision geometries FCLCollisionGeometryPtr cg1 = createCollisionGeometry(obj1); FCLCollisionGeometryPtr cg2 = createCollisionGeometry(obj2); @@ -171,6 +198,12 @@ bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2) bool checkCollisionObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2) { + if (co1 == nullptr || co2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObjects: called with a NULL FCLCollisionObjectPtr."); + return false; + } + // Use FCL to check for collision fcl::CollisionRequestd col_req; fcl::CollisionResultd col_result; @@ -184,6 +217,12 @@ void getObjectDistancesWorld(const FCLObjectPtr& obj, std::vector& closest_pt_obj, std::vector& closest_pt_world) { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "getObjectDistancesWorld: called with a NULL FCLObjectPtr."); + return; + } + // Reset output vector variables int world_size = world.getNumObjects(); obj_distances.clear(); @@ -206,6 +245,12 @@ void getObjectDistancesWorld(const FCLCollisionObjectPtr& co, std::vector& closest_pt_obj, std::vector& closest_pt_world) { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "getObjectDistancesWorld: called with a NULL FCLCollisionObjectPtr."); + return; + } + // Reset output vector variables int world_size = world.getNumObjects(); obj_distances.clear(); @@ -224,6 +269,12 @@ void getObjectDistancesWorld(const FCLCollisionObjectPtr& co, double getDistanceObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2) { + if (obj1 == nullptr || obj2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "getDistanceObjects: called with a NULL FCLObjectPtr."); + return -1.0; + } + // Create the collision geometries FCLCollisionGeometryPtr cg1 = createCollisionGeometry(obj1); FCLCollisionGeometryPtr cg2 = createCollisionGeometry(obj2); @@ -247,6 +298,12 @@ double getDistanceObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2) double getDistanceObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2) { + if (co1 == nullptr || co2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "getDistanceObjects: called with a NULL FCLCollisionObjectPtr."); + return -1.0; + } + fcl::DistanceRequestd dist_req; dist_req.enable_nearest_points = false; dist_req.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD; @@ -263,6 +320,12 @@ double getDistanceObjects(const FCLObjectPtr& obj1, Eigen::Vector3d& closest_pt_obj1, Eigen::Vector3d& closest_pt_obj2) { + if (obj1 == nullptr || obj2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "getDistanceObjects: called with a NULL FCLObjectPtr."); + return -1.0; + } + // Create the collision geometries FCLCollisionGeometryPtr cg1 = createCollisionGeometry(obj1); FCLCollisionGeometryPtr cg2 = createCollisionGeometry(obj2); @@ -297,6 +360,12 @@ double getDistanceObjects(const FCLCollisionObjectPtr& co1, Eigen::Vector3d& closest_pt_obj1, Eigen::Vector3d& closest_pt_obj2) { + if (co1 == nullptr || co2 == nullptr) + { + RCLCPP_ERROR(getLogger(), "getDistanceObjects: called with a NULL FCLCollisionObjectPtr."); + return -1.0; + } + fcl::DistanceRequestd dist_req; dist_req.enable_nearest_points = true; dist_req.gjk_solver_type = fcl::GJKSolverType::GST_LIBCCD; @@ -317,6 +386,12 @@ double getDistanceObjects(const FCLCollisionObjectPtr& co1, double getMinimumObjectDistanceWorld(const FCLObjectPtr& obj, const FCLInterfaceCollisionWorld& world) { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "getMinimumObjectDistanceWorld: called with a NULL FCLObjectPtr."); + return -1.0; + } + double min_distance = std::numeric_limits::max(); std::vector world_objects = world.getCollisionObjects(); for (const auto& other : world_objects) @@ -333,6 +408,12 @@ double getMinimumObjectDistanceWorld(const FCLObjectPtr& obj, const FCLInterface double getMinimumObjectDistanceWorld(const FCLCollisionObjectPtr& co, const FCLInterfaceCollisionWorld& world) { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "getMinimumObjectDistanceWorld: called with a NULL FCLCollisionObjectPtr."); + return -1.0; + } + double min_distance = std::numeric_limits::max(); std::vector world_objects = world.getCollisionObjects(); for (const auto& other : world_objects) diff --git a/src/fcl_interface_collision_world.cpp b/src/fcl_interface_collision_world.cpp index 10b17ad..c556912 100644 --- a/src/fcl_interface_collision_world.cpp +++ b/src/fcl_interface_collision_world.cpp @@ -62,7 +62,7 @@ bool FCLInterfaceCollisionWorld::addCollisionObjects(const std::vectorcollision_object; @@ -224,6 +231,12 @@ bool FCLInterfaceCollisionWorld::checkCollisionObject(int obj_id, std::vector& collision_object_ids) const { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObject: called with a NULL FCLObjectPtr."); + return false; + } + // Create the collision object FCLCollisionGeometryPtr cg = fcl_interface::createCollisionGeometry(obj); fcl::Transform3d world_to_fcl; @@ -254,6 +267,12 @@ bool FCLInterfaceCollisionWorld::checkCollisionObject(const FCLObjectPtr& obj, s bool FCLInterfaceCollisionWorld::checkCollisionObject(const FCLCollisionObjectPtr& co, std::vector& collision_object_ids) const { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "checkCollisionObject: called with a NULL FCLCollisionObjectPtr."); + return false; + } + // Clear vector of any prior entries collision_object_ids.clear(); @@ -285,7 +304,8 @@ void FCLInterfaceCollisionWorld::getObjectDistances(int obj_id, int index; if (!collisionObjectExists(obj_id, index)) { - RCLCPP_ERROR(getLogger(), "No object with ID %d in the world", obj_id); + RCLCPP_ERROR(getLogger(), "getObjectDistances: No object with ID %d in the world", obj_id); + return; } FCLCollisionObjectPtr co = fcl_collision_objects_[index]->collision_object; @@ -332,6 +352,12 @@ void FCLInterfaceCollisionWorld::getObjectDistances(const FCLObjectPtr& obj, std::vector& closest_pt_obj, std::vector& closest_pt_world) const { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "getObjectDistances: called with a NULL FCLObjectPtr."); + return; + } + // Create the collision object FCLCollisionGeometryPtr cg = fcl_interface::createCollisionGeometry(obj); fcl::Transform3d world_to_fcl; @@ -378,6 +404,12 @@ void FCLInterfaceCollisionWorld::getObjectDistances(const FCLCollisionObjectPtr& std::vector& closest_pt_obj, std::vector& closest_pt_world) const { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "getObjectDistances: called with a NULL FCLCollisionObjectPtr."); + return; + } + // Reset output vector variables obj_distances.clear(); obj_distances.resize(obj_counter_); @@ -419,7 +451,8 @@ double FCLInterfaceCollisionWorld::getMinimumObjectDistance(int obj_id) const int index; if (!collisionObjectExists(obj_id, index)) { - RCLCPP_ERROR(getLogger(), "No object with ID %d in the world", obj_id); + RCLCPP_ERROR(getLogger(), "getMinimumObjectDistance: No object with ID %d in the world", obj_id); + return -1.0; } FCLCollisionObjectPtr co = fcl_collision_objects_[index]->collision_object; @@ -450,6 +483,12 @@ double FCLInterfaceCollisionWorld::getMinimumObjectDistance(int obj_id) const double FCLInterfaceCollisionWorld::getMinimumObjectDistance(const FCLObjectPtr& obj) const { + if (obj == nullptr) + { + RCLCPP_ERROR(getLogger(), "getMinimumObjectDistance: called with a NULL FCLObjectPtr."); + return -1.0; + } + // Create the collision object FCLCollisionGeometryPtr cg = fcl_interface::createCollisionGeometry(obj); fcl::Transform3d world_to_fcl; @@ -480,6 +519,12 @@ double FCLInterfaceCollisionWorld::getMinimumObjectDistance(const FCLObjectPtr& double FCLInterfaceCollisionWorld::getMinimumObjectDistance(const FCLCollisionObjectPtr& co) const { + if (co == nullptr) + { + RCLCPP_ERROR(getLogger(), "getMinimumObjectDistance: called with a NULL FCLCollisionObjectPtr."); + return -1.0; + } + double min_distance = std::numeric_limits::max(); for (const auto& collision_interface_obj : fcl_collision_objects_) { diff --git a/test/interface_test.cpp b/test/interface_test.cpp index fab7ade..68ce8b7 100644 --- a/test/interface_test.cpp +++ b/test/interface_test.cpp @@ -220,6 +220,38 @@ TEST(FCLInterface, AddRemoveVoxelGrid) ASSERT_TRUE(success); } +TEST(FCLInterface, NullPtrCheck) +{ + robot_collision_checking::FCLInterfaceCollisionWorld collision_world; + + robot_collision_checking::FCLObjectPtr null_obj = nullptr; + bool success = collision_world.addCollisionObject(null_obj, -1); + ASSERT_FALSE(success); + + std::vector collision_object_ids; + bool is_collision = collision_world.checkCollisionObject(null_obj, collision_object_ids); + ASSERT_FALSE(is_collision); + + std::vector obj_distances; + std::vector closest_pt_obj; + std::vector closest_pt_world; + collision_world.getObjectDistances(null_obj, obj_distances, closest_pt_obj, closest_pt_world); + ASSERT_EQ(obj_distances.size(), 0); + + double min_dist = collision_world.getMinimumObjectDistance(null_obj); + ASSERT_EQ(min_dist, -1.0); + + robot_collision_checking::FCLObjectPtr null_obj2 = nullptr; + is_collision = robot_collision_checking::fcl_interface::checkCollisionObjects(null_obj, null_obj2); + ASSERT_FALSE(is_collision); + + robot_collision_checking::FCLCollisionGeometryPtr cg = robot_collision_checking::fcl_interface::createCollisionGeometry(null_obj); + ASSERT_EQ(cg, nullptr); + + min_dist = robot_collision_checking::fcl_interface::getMinimumObjectDistanceWorld(null_obj, collision_world); + ASSERT_EQ(min_dist, -1.0); +} + TEST(FCLInterface, CollisionCheck) { // Create some Eigen transforms in the world frame