diff --git a/CODE_OF_CONDUCT.md b/CODE_OF_CONDUCT.md new file mode 100644 index 0000000..25861ff --- /dev/null +++ b/CODE_OF_CONDUCT.md @@ -0,0 +1,69 @@ +# Code of Conduct - robot_collision_checking + +## Our Pledge + +In the interest of fostering an open and welcoming environment, we as +contributors and maintainers pledge to make participation in our project and +our community a harassment-free experience for everyone, regardless of age, body +size, disability, ethnicity, sex characteristics, gender identity and expression, +level of experience, education, socio-economic status, nationality, personal +appearance, race, religion, or sexual identity and orientation. + +## Our Standards + +Examples of behaviour that contributes to a positive environment for our +community include: + +* Demonstrating empathy and kindness toward other people +* Being respectful of differing opinions, viewpoints, and experiences +* Giving and gracefully accepting constructive feedback +* Accepting responsibility and apologising to those affected by our mistakes, + and learning from the experience +* Focusing on what is best not just for us as individuals, but for the + overall community + +Examples of unacceptable behaviour include: + +* The use of sexualised language or imagery, and sexual attention or advances +* Trolling, insulting or derogatory comments, and personal or political attacks +* Public or private harassment +* Publishing others' private information, such as a physical or email + address, without their explicit permission +* Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Our Responsibilities + +Project maintainers are responsible for clarifying and enforcing our standards of +acceptable behaviour and will take appropriate and fair corrective action in +response to any instances of unacceptable behaviour. + +Project maintainers have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, or to ban +temporarily or permanently any contributor for other behaviours that they deem +inappropriate, threatening, offensive, or harmful. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official e-mail address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behaviour may be +reported to the community leaders responsible for enforcement at <>. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant](https://contributor-covenant.org/), version +[1.4](https://www.contributor-covenant.org/version/1/4/code-of-conduct/code_of_conduct.md) and +[2.0](https://www.contributor-covenant.org/version/2/0/code_of_conduct/code_of_conduct.md), +and was generated by [contributing-gen](https://github.com/bttger/contributing-gen). diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md new file mode 100644 index 0000000..1f4ccd9 --- /dev/null +++ b/CONTRIBUTING.md @@ -0,0 +1,159 @@ + +# Contributing to robot_collision_checking + +First off, thanks for taking the time to contribute! ❤️ + +All types of contributions are encouraged and valued. See the [Table of Contents](#table-of-contents) for different ways to help and details about how this project handles them. Please make sure to read the relevant section before making your contribution. It will make it a lot easier for us maintainers and smooth out the experience for all involved. The community looks forward to your contributions. 🎉 + +> And if you like the project, but just don't have time to contribute, that's fine. There are other easy ways to support the project and show your appreciation, which we would also be very happy about: +> - Star the project +> - Tweet about it +> - Refer this project in your project's readme +> - Mention the project at local meetups and tell your friends/colleagues + + +## Table of Contents + +- [Code of Conduct](#code-of-conduct) +- [I Have a Question](#i-have-a-question) + - [I Want To Contribute](#i-want-to-contribute) + - [Reporting Bugs](#reporting-bugs) + - [Suggesting Enhancements](#suggesting-enhancements) + + + + +## Code of Conduct + +This project and everyone participating in it is governed by the +[robot_collision_checking Code of Conduct](https://github.com/philip-long/robot_collision_checking/blob/humble/CODE_OF_CONDUCT.md). +By participating, you are expected to uphold this code. Please report unacceptable behavior +to the developers of this project. + + +## I Have a Question + +> If you want to ask a question, we assume that you have read the [README](README.md) and [API Documentation](docs/api.md). + +Before you ask a question, it is best to search for existing [Issues](https://github.com/philip-long/robot_collision_checking/issues) that might help you. In case you have found a suitable issue and still need clarification, you can write your question in this issue. It is also advisable to search the internet for answers first. + +If you then still feel the need to ask a question and need clarification, we recommend the following: + +- Open an [Issue](https://github.com/philip-long/robot_collision_checking/issues/new). +- Provide as much context as you can about what you're running into. +- Provide project and platform versions (nodejs, npm, etc), depending on what seems relevant. + +We will then take care of the issue as soon as possible. + + + +## I Want To Contribute + +> ### Legal Notice +> When contributing to this project, you must agree that you have authored 100% of the content, that you have the necessary rights to the content and that the content you contribute may be provided under the project licence. + +### Reporting Bugs + + +#### Before Submitting a Bug Report + +A good bug report shouldn't leave others needing to chase you up for more information. Therefore, we ask you to investigate carefully, collect information and describe the issue in detail in your report. Please complete the following steps in advance to help us fix any potential bug as fast as possible. + +- Make sure that you are using the latest version. +- Determine if your bug is really a bug and not an error on your side e.g. using incompatible environment components/versions (Make sure that you have read the [README](README.md). If you are looking for support, you might want to check [this section](#i-have-a-question)). +- To see if other users have experienced (and potentially already solved) the same issue you are having, check if there is not already a bug report existing for your bug or error in the [bug tracker](https://github.com/philip-long/robot_collision_checking/issues?q=label%3Abug). +- Also make sure to search the internet (including Stack Overflow) to see if users outside of the GitHub community have discussed the issue. +- Collect information about the bug: + - Stack trace (Traceback) + - OS, Platform and Version (Windows, Linux, macOS, x86, ARM) + - Version of the interpreter, compiler, SDK, runtime environment, package manager, depending on what seems relevant. + - Possibly your input and the output + - Can you reliably reproduce the issue? And can you also reproduce it with older versions? + + +#### How Do I Submit a Good Bug Report? + +> You must never report security related issues, vulnerabilities or bugs including sensitive information to the issue tracker, or elsewhere in public. Instead sensitive bugs must be alerted by contacting the developers of this repository. + + +We use GitHub issues to track bugs and errors. If you run into an issue with the project: + +- Open an [Issue](https://github.com/philip-long/robot_collision_checking/issues/new). (Since we can't be sure at this point whether it is a bug or not, we ask you not to talk about a bug yet and not to label the issue.) +- Explain the behavior you would expect and the actual behavior. +- Please provide as much context as possible and describe the *reproduction steps* that someone else can follow to recreate the issue on their own. This usually includes your code. For good bug reports you should isolate the problem and create a reduced test case. +- Provide the information you collected in the previous section. + +Once it's filed: + +- The project team will label the issue accordingly. +- A team member will try to reproduce the issue with your provided steps. If there are no reproduction steps or no obvious way to reproduce the issue, the team will ask you for those steps and mark the issue as `needs-repro`. Bugs with the `needs-repro` tag will not be addressed until they are reproduced. +- If the team is able to reproduce the issue, it will be marked `needs-fix`, as well as possibly other tags (such as `critical`), and the issue will be left to be implemented by someone. + + + + +### Suggesting Enhancements + +This section guides you through submitting an enhancement suggestion for `robot_collision_checking`, **including completely new features and minor improvements to existing functionality**. Following these guidelines will help maintainers and the community to understand your suggestion and find related suggestions. + + +#### Before Submitting an Enhancement + +- Make sure that you are using the latest version. +- Read the [README](README.md) and [API Documentation](docs/api.md) carefully, and find out if the functionality is already covered, maybe by an individual configuration. +- Perform a [search](https://github.com/philip-long/robot_collision_checking/issues) to see if the enhancement has already been suggested. If it has, add a comment to the existing issue instead of opening a new one. +- Find out whether your idea fits with the scope and aims of the project. It's up to you to make a strong case to convince the project's developers of the merits of this feature. Keep in mind that we want features that will be useful to the majority of our users and not just a small subset. If you're just targeting a minority of users, consider writing an add-on/plugin library. + + +#### How Do I Submit a Good Enhancement Suggestion? + +Enhancement suggestions are tracked as [GitHub issues](https://github.com/philip-long/robot_collision_checking/issues). + +- Use a **clear and descriptive title** for the issue to identify the suggestion. +- Provide a **step-by-step description of the suggested enhancement** in as many details as possible. +- **Describe the current behavior** and **explain which behavior you expected to see instead** and why. At this point you can also tell which alternatives do not work for you. +- You may want to **include screenshots or screen recordings** which help you demonstrate the steps or point out the part which the suggestion is related to. You can use [LICEcap](https://www.cockos.com/licecap/) to record GIFs on macOS and Windows, and the built-in [screen recorder in GNOME](https://help.gnome.org/users/gnome-help/stable/screen-shot-record.html.en) or [SimpleScreenRecorder](https://github.com/MaartenBaert/ssr) on Linux. +- **Explain why this enhancement would be useful** to most `robot_collision_checking` users. You may also want to point out the other projects that solved it better and which could serve as inspiration. + + + + + + + + + + + + + + + + +## Attribution +This guide is based on the **contributing-gen**. [Make your own](https://github.com/bttger/contributing-gen)! diff --git a/README.md b/README.md index 9cea604..755a6ab 100644 --- a/README.md +++ b/README.md @@ -1,32 +1,36 @@ -# Robot Collision Checking +# Robot Collision Checking for ROS -A lightweight package to use FCL with ROS messages that is heavily inspired by [MoveIt's version](https://moveit.ros.org/documentation/concepts/developer_concepts/). +A ROS package that wraps around [FCL](https://github.com/flexible-collision-library/fcl), a popular library in robotics for collision detection and proximity computation. This repository is inspired by [MoveIt's approach](https://moveit.ros.org/documentation/concepts/developer_concepts/) to interfacing ROS with FCL. However, `robot_collision_checking` is a lightweight alternative that does not require the entirety of a motion planning framework, like MoveIt, to expose FCL's collision and distance checking capabilities to ROS messages/types. The `robot_collision_checking` package can be utilised to perform distance and collision checking of objects by creating and maintaining a collision world and/or by using utility functions (see the [API Documentation](docs/api.md) for more information). This package can handle objects represented as [shape_msgs](http://wiki.ros.org/shape_msgs), [OctoMaps](https://github.com/OctoMap/octomap_msgs), and [VoxelGrids](https://github.com/ros-planning/navigation2/blob/main/nav2_msgs/msg/VoxelGrid.msg). -Depending on which git branch is used, implementations for the following ROS distros are available: -- ROS 1 Noetic on the `noetic-devel` branch, -- ROS 2 Foxy on the `foxy` branch, and -- ROS 2 Humble on the default `humble` branch. +Implementations for the following ROS distros are available on different git branches of this repository: +- [ROS 1 Noetic](https://wiki.ros.org/noetic) on the `noetic-devel` branch, +- [ROS 2 Foxy](https://docs.ros.org/en/foxy/index.html) on the `foxy` branch, and +- [ROS 2 Humble](https://docs.ros.org/en/humble/index.html) on the default `humble` branch. -The package was developed and tested on Ubuntu 20.04 for Noetic/Foxy and Ubuntu 22.04 for Humble. However, any operating systems supported by the ROS distros available +The package was developed and tested on Ubuntu 20.04 for Noetic/Foxy and Ubuntu 22.04 for Humble. Nevertheless, any operating systems supported by the ROS distros available to this package should also work. We recommend using the default ROS 2 Humble implementation, as this continues to have ongoing support. In terms of third-party software, this package requires: - * [FCL](https://github.com/flexible-collision-library/fcl) version **0.7.0** + * [FCL](https://github.com/flexible-collision-library/fcl) * [libccd](https://github.com/danfis/libccd) - * [Octomap](https://octomap.github.io/) + * [OctoMap](https://octomap.github.io/) + * [Eigen](https://eigen.tuxfamily.org/dox/GettingStarted.html) + +**Warning:** Older versions of FCL and `libccd` may already be installed on your system when installing ROS. These versions are incompatible with `robot_collision_checking`, so please ensure that these libraries are installed according to the [Installation](#installation) section of this repository. ## Installation -FCL and `libccd-dev` may already be installed on your machine via your ROS distro, but these versions are likely outdated for the current repository's use. -The following instructions will enable you to build the `robot_collision_checking` package within a ROS 2 workspace using `colcon build` (or `catkin build` if using ROS 1). +The following instructions will enable you to build the `robot_collision_checking` package within a [ROS 2 workspace](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html) using `colcon build` (or `catkin build` if using ROS 1). + +FCL and `libccd` may already be installed on your machine via your ROS distro, but these versions are likely outdated for the current repository's use. You will need to build these libraries from source, as described below. ### libccd -Run the following commands to build `libccd` from source: +In a directory of your choice, run the following commands to build `libccd` from source: ``` git clone https://github.com/danfis/libccd.git cd libccd && mkdir build && cd build @@ -41,8 +45,9 @@ sudo make install ``` sudo apt install liboctomap-dev ``` -as FCL will ignore building `OcTree` collision geometries otherwise. -Once Octomap is installed, run the following commands to build `fcl` from source: +as FCL will ignore building `OcTree` collision geometries otherwise (see [Issue #4](https://github.com/philip-long/robot_collision_checking/issues/4) for more information). + +Once Octomap is installed, run the following commands in a directory of your choice to build an up-to-date version of FCL (i.e., later than [version 0.7.0](https://github.com/flexible-collision-library/fcl/releases/tag/0.7.0)) from source: ``` git clone https://github.com/flexible-collision-library/fcl.git @@ -52,24 +57,34 @@ make sudo make install ``` -If there are errors, such as constants not being found, then you are probably still using the older version of FCL. +If there are errors when building `robot_collision_checking`, such as constants not being found, then you are probably still using an older version of FCL (prior to version 0.7.0). ### Workspace Setup -You can now clone the `robot_collision_checking` repository in your ROS workspace (set to the appropriate `$ROS_DISTRO` branch). Don't forget to install -any other system dependencies through `rosdep` after installing the above libraries, e.g., in the root directory of your ROS workspace run: +You can now clone the `robot_collision_checking` package into your ROS workspace's `src` directory and set it to the appropriate ROS distro implementation, e.g., by cloning as follows: +``` +git clone --branch https://github.com/philip-long/robot_collision_checking.git +``` +Where `` is either `humble`, `foxy`, or `noetic-devel`. + +Don't forget to install any other system dependencies through `rosdep` after installing the above libraries, e.g., in the root directory of your ROS workspace run: ``` rosdep install --from-paths src --ignore-src -y ``` ## Alternative - Docker Image -If you instead wish to explore the package in a Docker image, there is a `Dockerfile` available. Simply clone the repository or download the `Dockerfile` and +If you instead wish to explore the package in a Docker image, there is a `Dockerfile` available. After [installing docker](https://docs.ros.org/en/humble/How-To-Guides/Setup-ROS-2-with-VSCode-and-Docker-Container.html#install-docker), simply clone the repository or download the `Dockerfile` and then run: ``` docker build --tag 'robot_collision_checking' . && docker run -it 'robot_collision_checking' bash ``` +The Docker image is preconfigured with all the core libraries for `robot_collision_checking` (e.g., FCL, `libccd`, ROS, etc.). After building the image and starting the container, a ROS workspace `ros2_ws` will have been created and built with all the necessary dependencies. The final step is to source `ros2_ws` before testing out the package: +``` +source /ros2_ws/install/setup.bash +``` + ## Testing You can run tests for the `robot_collision_checking` package as described in [this ROS 2 tutorial](https://docs.ros.org/en/humble/Tutorials/Intermediate/Testing/CLI.html). First compile the tests: @@ -82,32 +97,86 @@ And then examine the results: colcon test-result --all --verbose ``` +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. **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 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 A toy example is provided in the `examples` directory and can be run as follows: ``` ros2 run robot_collision_checking fcl_interface_example ``` -Separately run an instance of `rviz2` and set the global fixed frame to "world" to visualize the collision world. You can install `rviz2` on Debian systems by running: +In a separate terminal, run an instance of [RViz](https://docs.ros.org/en/humble/Tutorials/Intermediate/RViz/RViz-User-Guide/RViz-User-Guide.html) and set the global fixed frame to "world" to visualize the collision world. You can install `rviz2` on Debian systems by running: ``` sudo apt install ros-$ROS_DISTRO-rviz2 ``` +If everything is set up correctly, you should see a view similar to: + +![RViz view collision world](docs/fcl_interface_example_rviz.png) -Within this ROS node, a few key pieces of functionality are provided: -- First, the `initCollisionWorld()` method demonstrates how a collision world composed of different geometric shapes and types (meshes, planes, voxel grids, etc.) can be constructed + +Within the fcl_interface_example node, a few key pieces of functionality are provided: +- First, a collision world composed of different geometric shapes and types (meshes, planes, voxel grids, etc.) is constructed and maintained using the package's interface. + + // Initialize the FCL collision world + robot_collision_checking::FCLInterfaceCollisionWorld collision_world("world"); + bool success = initCollisionWorld(collision_world); + + bool initCollisionWorld(robot_collision_checking::FCLInterfaceCollisionWorld& world) + { + ... + // Collection of objects to be added to the world + std::vector fcl_objects; + std::vector object_ids = {0, 1, 2, 3, 4}; + ... + // Adds the collection of FCL objects to the collision world + // And returns whether this operation succeeded or failed + return world.addCollisionObjects(fcl_objects, object_ids); + } +            where in the `initCollisionWorld` method, a collection of five FCL objects are added to the world: - Second, the main publishing loop indicates how these different geometric types can be translated into [visualization_msgs/Marker](https://wiki.ros.org/rviz/DisplayTypes/Marker) messages for visualization in RViz. -- Finally, the example shows how the created collision world can be used to check for collisions and distances between its constituent objects. -The output of the example node prints information about any objects currently in collision. - -While this example only contains static objects, the package also works with dynamic objects. - -A more extensive use-case of this package is provided in [constrained_manipulability](https://github.com/philip-long/constrained_manipulability). +- Finally, the example shows how the created collision world can be used to check for collisions between its constituent objects. + + std::vector world_objects = + collision_world.getCollisionObjects(); + for (int i = 0; i < num_objects; /*i++*/) + { + auto world_obj = world_objects[i]; + std::string obj_type = world_obj->object->getTypeString(); + ... + bool is_collision = collision_world.checkCollisionObject( + world_obj->collision_id, collision_object_ids); + if (is_collision) + { + for (int obj_id : collision_object_ids) + { + RCLCPP_INFO(node->get_logger(), "%s with ID %d in collision with object with ID %d", + obj_type.c_str(), world_obj->collision_id, obj_id); + } + } + ... + } + +            The output of the example node prints information about any objects currently in collision (as shown in the code snippet above). + +Please refer to the [package's API documentation](docs/api.md) for more information about the code. + +While this example only contains static objects, the package also works with dynamic objects. A more extensive use-case of this package that includes dynamic scenarios is provided in [constrained_manipulability](https://github.com/philip-long/constrained_manipulability). Here, the `robot_collision_checking` interface checks for collisions and distances between environmental objects and a robot manipulator (based on the geometric shapes -present in its URDF model). +present in its [URDF](https://docs.ros.org/en/humble/Tutorials/Intermediate/URDF/URDF-Main.html) model). -## Contributing + diff --git a/docs/api.md b/docs/api.md index 6e74118..44796e0 100644 --- a/docs/api.md +++ b/docs/api.md @@ -1,10 +1,12 @@ # C++ API Documentation The following describes the `robot_collision_checking` ROS package's C++ API for managing and checking collisions -between various object types in a robot environment, using the Flexible Collision Library (FCL). We break this documentation down into +between various object types in a robot environment, using the Flexible Collision Library (FCL). We break this documentation down into: 1) [Types](#types) 2) [Core Functionality](#core-functionality) +3) [Error Handling](#error-handling) +4) [Testing](#testing) ## Types @@ -108,7 +110,21 @@ struct FCLInterfaceCollisionObject typedef std::shared_ptr FCLInterfaceCollisionObjectPtr; ``` -This struct provides the glue between our container for different ROS message types, `FCLObject`, and object types suitable collision and distance checking using FCL, `fcl::CollisionObjectd`. +This struct provides the glue between our container for different ROS message types, `FCLObject`, and object types suitable collision and distance checking using FCL, `fcl::CollisionObjectd`. + +To create an `FCLInterfaceCollisionObject`, an `FCLObjectPtr`, an `FCLCollisionObjectPtr`, and an object ID are required. For instance, given a pointer variable `obj` of type `FCLObjectPtr`, an integer `object_id`, and the utility method `fcl_interface::createCollisionGeometry` described in the [fcl_interface section](#fcl_interface), an `FCLInterfaceCollisionObject` can be constructed as follows: +```c++ +// Obtain the collision geometry for FCLObjectPtr `obj` +FCLCollisionGeometryPtr cg = fcl_interface::createCollisionGeometry(obj); +// Convert the Eigen Affine3D transform to an FCL coordinate transform +fcl::Transform3d world_to_fcl; +fcl_interface::transform2fcl(obj->object_transform, world_to_fcl); +// Construct an FCLCollisionObjectPtr from the object's corresponding collision geometry and FCL transform +FCLCollisionObjectPtr co = std::make_shared(cg, world_to_fcl); + +// Create an FCLInterfaceCollisionObjectPtr from the FCLObjectPtr, FCLCollisionObjectPtr, and a given `object_id` +FCLInterfaceCollisionObjectPtr new_col_object = std::make_shared(obj, co, object_id); +``` ## Core Functionality @@ -117,12 +133,15 @@ The package can be used by instantiating a collision world, `FCLInterfaceCollisi ### FCLInterfaceCollisionWorld -The `FCLInterfaceCollisionWorld` class is a container and manager for the objects in a collision world, providing functionality to +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. @@ -183,11 +202,12 @@ See [interface_test.cpp](../test/interface_test.cpp) and [fcl_interface_example. ### fcl_interface -The `robot_collision_checking::fcl_interface` methods declared in [fcl_interface.hpp](../include/robot_collision_checking/fcl_interface.hpp) expose much of the same functionality as `FCLInterfaceCollisionWorld` in terms of checking for collisions, calculating distances between objects, and identifying the closest points on objects. However, no collision world is maintained. Instead these calculations are performed on FCL objects without a specified world, assuming the objects are in the same reference frame, e.g.,: +The `robot_collision_checking::fcl_interface` methods declared in [fcl_interface.hpp](../include/robot_collision_checking/fcl_interface.hpp) expose much of the same functionality as `FCLInterfaceCollisionWorld` in terms of checking for collisions, calculating distances between objects, and identifying the closest points on objects. However, no collision world is maintained. Instead these calculations are performed on FCL objects without a specified world, e.g.,: ```c++ bool checkCollisionObjects(const FCLObjectPtr& obj1, const FCLObjectPtr& obj2); -bool checkCollisionObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2); +double getDistanceObjects(const FCLCollisionObjectPtr& co1, const FCLCollisionObjectPtr& co2); ``` +An assumption when performing these calculations on objects detached from a collision world is that the objects are posed in a common frame of reference. This common frame is typically the "world" frame. Likewise, an FCL object can also be compared against a given world: ```c++ @@ -209,5 +229,57 @@ There are also other convenience functions to convert ROS or standard types to a // Create collision FCL geometries from specific ROS msgs FCLCollisionGeometryPtr createCollisionGeometry(const FCLObjectPtr& obj); ``` +For instance, if the `obj` type is a `shape_msgs::msg::SolidPrimitive` then the above method is implemented as: +```c++ +FCLCollisionGeometryPtr createCollisionGeometry(const shape_msgs::msg::SolidPrimitive& solid) +{ + if (solid.type == shape_msgs::msg::SolidPrimitive::SPHERE) + { + return std::make_shared(solid.dimensions[shape_msgs::msg::SolidPrimitive::SPHERE_RADIUS]); + } + else if (solid.type == shape_msgs::msg::SolidPrimitive::BOX) + { + return std::make_shared(solid.dimensions[shape_msgs::msg::SolidPrimitive::BOX_X], + solid.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Y], + solid.dimensions[shape_msgs::msg::SolidPrimitive::BOX_Z]); + } + else if (solid.type == shape_msgs::msg::SolidPrimitive::CONE) + { + return std::make_shared(solid.dimensions[shape_msgs::msg::SolidPrimitive::CONE_RADIUS], + solid.dimensions[shape_msgs::msg::SolidPrimitive::CONE_HEIGHT]); + } + else if (solid.type == shape_msgs::msg::SolidPrimitive::CYLINDER) + { + return std::make_shared(solid.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_RADIUS], + solid.dimensions[shape_msgs::msg::SolidPrimitive::CYLINDER_HEIGHT]); + } + else + { + RCLCPP_ERROR(getLogger(), "NOT a valid solid primitive type"); + return nullptr; + } +} +``` + +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. **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. -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. \ No newline at end of file diff --git a/docs/fcl_interface_example_rviz.png b/docs/fcl_interface_example_rviz.png new file mode 100644 index 0000000..15c9322 Binary files /dev/null and b/docs/fcl_interface_example_rviz.png differ 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