Robot Collision Checking for ROS

A ROS package that wraps around FCL, a popular library in robotics for collision detection and proximity computation. This repository is inspired by MoveIt’s approach 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 for more information). This package can handle objects represented as shape_msgs, OctoMaps, and VoxelGrids.

Implementations for the following ROS distros are available on different git branches of this repository:

• ROS 1 Noetic on the noetic-devel branch,
• ROS 2 Foxy on the foxy branch, and
• ROS 2 Humble on the default humble branch.

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
• libccd
• OctoMap
• Eigen

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 section of this repository.

Installation

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).

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

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
cmake -G "Unix Makefiles" -DENABLE_DOUBLE_PRECISION=ON ..
make
sudo make install

FCL

Important: Before installing FCL, make sure to have liboctomap-dev installed, e.g.,

sudo apt install liboctomap-dev

as FCL will ignore building OcTree collision geometries otherwise (see Issue #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) from source:

git clone https://github.com/flexible-collision-library/fcl.git
cd fcl && mkdir build && cd build
cmake ..
make
sudo make install

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 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 <branch-name> https://github.com/philip-long/robot_collision_checking.git

Where <branch-name> 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. After installing 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. First compile the tests:

colcon test --ctest-args tests

And then examine the results:

colcon test-result --all --verbose

There are seven tests implemented in interface_test.cpp:

1. TransformToFCL: To validate the Eigen::Affine3d to fcl::Transform3d transformation method.
2. AddRemove: Asserts that a variety of shape_msgs and OctoMaps can be added/removed to a collision world.
3. AddRemoveVoxelGrid: Same as “AddRemove”, except focused on VoxelGrids.
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).

Examples

A toy example is provided in the examples directory and can be run as follows:

ros2 run robot_collision_checking fcl_interface_example

In a separate terminal, run an instance of RViz 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:

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<robot_collision_checking::FCLObjectPtr> fcl_objects;
        std::vector<int> 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);
    } &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp; 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 messages for visualization in RViz.

• Finally, the example shows how the created collision world can be used to check for collisions between its constituent objects.

    std::vector<robot_collision_checking::FCLInterfaceCollisionObjectPtr> 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 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. 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).

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 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
• I Have a Question
  • I Want To Contribute
  • Reporting Bugs
  • Suggesting Enhancements <!– - Your First Code Contribution
  • Improving The Documentation –> <!– - Styleguides
  • Commit Messages
• Join The Project Team –>

Code of Conduct

This project and everyone participating in it is governed by the robot_collision_checking Code of Conduct. 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 and API Documentation.

Before you ask a question, it is best to search for existing 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.
• 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. If you are looking for support, you might want to check this section).
• 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.
• 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. (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 and API Documentation carefully, and find out if the functionality is already covered, maybe by an individual configuration.
• Perform a search 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.

• 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 to record GIFs on macOS and Windows, and the built-in screen recorder in GNOME or SimpleScreenRecorder 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!