This is the code for IDMP, implementing a gaussian process based distance and gradient field algorithm.
This codebase is implemented using ROS in both C++ and Python.
We present an interactive distance field mapping and planning (IDMP) framework that handles dynamic objects and collision avoidance through an efficient representation. Given depth sensor data, our framework builds a continuous field that allows to query the distance and gradient to the closest obstacle at any required position. The key aspect of this work is an efficient Gaussian Process field that performs incremental updates and implicitly handles dynamic objects with a simple and elegant formulation based on a temporary latent model.
- Python3
- C++14
- ROS Noetic
- Eigen
- OpenMP (for multithreading. optional but strongly recommended)
- sensor-filters (
sudo apt install ros-noetic-sensor-filters) - point-cloud2-filters (
sudo apt install ros-noetic-point-cloud2-filters) - robot-body-filter (optional for removing the robot from the pointcloud.) ()
sudo apt install ros-noetic-point-cloud2-filters) - Camera specific ros driver
- For Python tools:
- numpy
- OpenCV 4.9.0
- Open3D (for evaluation only)
IDMP is provided as catkin package
- clone the package into the src folder of your workspace
- run a
catkin build - resource your workspace
We provide a few launch files to get started:
- realsense.launch: Launches the realsense driver, publishes the camera pose as TF and runs IDMP
- zed.launch: Launches the zed driver, publishes the camera pose as TF and runs IDMP
- kinect_UR5e.launch: Launches the Azure Kinect driver, an inoffical UR5e Ros driver, a filter node that filters the robot points and IDMP. Note that we use our own custom Universal Robots Ros Driver and not the official one.
You can run these with roslaunch idmp_ros {name}.launch.
The camera pose has to be published as a TF transform. This is already done in the launch files. To change the pose, adjust the corresponding parameters in the static transform publisher args
If you do not want to use a launch file, you can also run the IDMP node standalone
rosrun idmp_ros idmp
Keep in mind that it needs the parameters of the config file in the parameter server at launch. You can lode these with rosparam load {file}.yaml.
These config also contains the topics of the input pointcloud, the camera intrinsics and the reference frame. Make sure to adjust these to your setup.
The Datasets can be found here:
Running the dataset with IDMP:
- Launch the corresponding launch file (run_cow_and_lady.launch or run_dynamic_ball.launch)-
- Play the rosbag
- To visualize the distance field and gradients, run the query tool
rosrun idmp_ros queryTool.py
Idmp supports the use of multiple cameras to capture a bigger scene. For this, each camera has to publish its pose as TF and its intrinsic parameters as CameraInfo message. The pointclouds of the cameras need to be merged into one pointcloud (preferably in the base frame), before being fed into IDMP.
- Merge pointcloud of all cameras. (For example using something like this)
- set the merged pointcloud topic as input for IDMP
- for each camera, add the cameraInfo topic and the TF frame name to the list in idmp_caminfo_topic.
- idmp_caminfo_topic for two cameras might look like this:
idmp_caminfo_topic:
- ["/camera_1/depth/camera_info", "camera_1_depth_optical_frame"]
- ["/camera_2/depth/camera_info", "camera_2_depth_optical_frame"]
We provide a set of tools to interact with IDMP for testing and demonstration purposes. You can run these with `rosrun idmp_ros {tool}.py
- calibPose: This can be used to calibrate the camera pose using an aruco marker. Adjust the topics and marker parameters for your setting and run the script. Make sure the determined pose is aligned properly and copy the output into your launchfile to replace the transform.
- queryTool: This creates an interactive marker and queries a distance field slice. It is then published and can be visualized in RVIZ. You can adjust the slice properties and make it move by setting the corresponding variables in the file.
- cowAndLadyInfoPub: This publishes the pose topic found in the cow and lady dataset as TF and the cameraInfo for the kinect
- evalTool: This is similar to the queryTool but it can load a groundtruth pointcloud and calculate real time metrics like RMSE
- speedMove: This is a basic implementation of obstacle avoidance with repulsive vectors by querying the distance field. This needs a robot driver to run and sends velocity commands to control the robot.
- The eval folder contains some tools used to evaluate our results for the publication
All IDMP parameters can be found as yaml files in the config folder. These need to be loaded into the parameter server before launching IDMP.
| Parameter | Description | Recommended Value |
|---|---|---|
| idmp_rleng | Cluster overlap factor for GP training | 1.8 |
| idmp_tree_hl_min | Octree half-length min | 0.025 |
| idmp_tree_hl_max | Octree half-length max | 3.2 |
| idmp_tree_hl_clust | Octree half-length of cluster | 0.05 |
| idmp_tree_hl_init | Initial octree half-length | 1.6 |
| idmp_map_scale | Kernel map scale | 1024 |
| idmp_filt_outl | Enables outlier filtering for input | True |
| idmp_pub_pcl | Publish the pointcloud of the octree for visualization | True |
| idmp_world_frame | Reference frame (needs to be published as TF) | "base_link" |
| idmp_pcl_topic | Topic of input pointcloud | "/points_filt" |
| idmp_caminfo_topic | List of cameraInfo and tf pose pairs for every sensor | " - ["/depth/camera_info", "depth_camera_link"]" |
| idmp_dynamic | Enables dynamic object mode | True |
| idmp_fusion | Enables local fusion | False |
| idmp_dyn_tresh | Dynamic movement treshold | 0.08 |
| idmp_fus_min | Fusion minimum treshold | 0.0005 |
| idmp_fus_max | Fusion maximum treshold | 0.01 |