Skip to content

Commit

Permalink
Adding better nullptr error handling and a new test, plus correspondi…
Browse files Browse the repository at this point in the history 
…ng readme/api doc changes
  • Loading branch information
@mazrk7
mazrk7 committed Jan 1, 2025
1 parent 0a2b7d2 commit 6e5ab17
Show file tree
Hide file tree
Showing 5 changed files with 196 additions and 18 deletions.
11 changes: 6 additions & 5 deletions README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down
33 changes: 26 additions & 7 deletions docs/api.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -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<FCLInterfaceCollisionObjectPtr> 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<FCLInterfaceCollisionObjectPtr> fcl_collision_objects_;
// Counter for object IDs
unsigned int obj_counter_;
```
There are getter methods to expose these members.

Expand Down Expand Up @@ -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.
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.

81 changes: 81 additions & 0 deletions src/fcl_interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}

Expand Down Expand Up @@ -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);
Expand All @@ -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<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -184,6 +217,12 @@ void getObjectDistancesWorld(const FCLObjectPtr& obj,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& 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();
Expand All @@ -206,6 +245,12 @@ void getObjectDistancesWorld(const FCLCollisionObjectPtr& co,
std::vector<Eigen::Vector3d>& closest_pt_obj,
std::vector<Eigen::Vector3d>& 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();
Expand All @@ -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);
Expand All @@ -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;
Expand All @@ -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);
Expand Down Expand Up @@ -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;
Expand All @@ -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<double>::max();
std::vector<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
for (const auto& other : world_objects)
Expand All @@ -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<double>::max();
std::vector<FCLInterfaceCollisionObjectPtr> world_objects = world.getCollisionObjects();
for (const auto& other : world_objects)
Expand Down
Loading

0 comments on commit 6e5ab17

Please sign in to comment.