/ROG-Map

Primary LanguageC++GNU General Public License v3.0GPL-3.0

ROG-Map: An Efficient Robocentric Occupancy Grid Map for Large-scene and High-resolution LiDAR-based Motion Planning

IROS 2024
Yunfan REN, Yixi Cai Fangcheng Zhu, Siqi Liang, and Fu Zhang

HKU MaRS Lab

arxiv Bilibili Youtube

Updates

If our repository supports your academic projects, please cite our paper. Thank you!

@article{ren2023rogmap,
    title={ROG-Map: An Efficient Robocentric Occupancy Grid Map for Large-scene and High-resolution LiDAR-based Motion Planning},
    author={Yunfan Ren and Yixi Cai and Fangcheng Zhu and Siqi Liang and Fu Zhang},
    journal={arXiv preprint arXiv:2302.14819},
    year={2023}
}

Click for the video demo.

Video Demo

Build Instructions

# install dependencies
sudo apt-get install ros-noetic-rosfmt
# for MARSIM example
sudo apt-get install libglfw3-dev libglew-dev
# Eigen and soft link
sudo apt-get install libeigen3-dev       
sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen
# dw for backward cpp
sudo apt-get install libdw-dev


mkdir -p rog_ws/src && cd rog_ws/src
git clone https://github.com/hku-mars/ROG-Map.git
cd ..
catkin_make -DBUILD_TYPE=Release

Known Build Issues

  • Disable the conda environment with conda deactivate to avoid linking issues. If you have try catkin_make in conda environment, please delete the build and devel and deactivate conda, and try catkin_make again.
  • If VizCfg fails to generate, try building with catkin_make -DCATKIN_DEVEL_PREFIX:PATH=${change-to-your-path-to-rog_ws}/devel.

Overview

ROG-Map's three main features are zero-copy map sliding, incremental map expansion, and a counter-based multi-resolution map. All sub-maps and functionalities are built upon the SlidingMap structure. The currently open-source version includes:

  • Multi-resolution inflation maps and incremental obstacle inflation:
    • Example: ProbMap resolution 0.1 m (yellow) with InfMap resolution 0.2 m (gray)

image-20240830181904520

  • Incremental Frontier generation
    • Example: Frontiers with a sensing range of 5m

image-20240830183455023

  • Sliding ESDF map generation

    image-20240830183740886

Applications

1. Running with MARSIM

First, launch the MARSIM environment:

source devel/setup.bash # or source devel/setup.zsh
roslaunch test_interface single_drone_os128.launch

Then, launch the ROG-Map test node and the keyboard controller:

sudo chmod +x -R src
roslaunch rog_map_example marsim_example.launch

After launching, click on the terminal running the second launch file, use the keyboard to control the drone, and observe the local sliding map:

Use W A S D on your keyboard to control the drone's velocity, press the spacebar to stop, and press Q or Ctrl + C to exit.

2 Running A* Search Example

source devel/setup.bash # or source devel/setup.zsh
roslaunch rog_map_example astar_example.launch

Then, you can press G to enable 3D Nav Goal in RViz and click to select a point. Each time you select two points, ROG-Map will perform path planning between them.

You can also enable the visualize_process_en param at ./examples/rog_map_example/config/astar_example.yaml to visualize the search process:

astar:
  visualize_process_en: true

3 Run RRT* example

source devel/setup.bash # or source devel/setup.zsh
roslaunch rog_map_example rrt_example.launch

Then, you can press G to enable 3D Nav Goal in RViz and click to select a point. Each time you select two points, ROG-Map will perform path planning between them.

You can also enable the visualize_process_en param at ./examples/rog_map_example/config/rrt_example.yaml to visualize the sampling process:

rrt_star:
  visualize_process_en: true

TODO

  • Add example for safe flight corridor generation.
  • Add example for trajectory optimization.

Using ROG-Map in Your Own ROS Project

To use ROG-Map, refer to the rog_map_example package. Here’s a basic guide:

  1. Copy the rog_map package to your workspace and add the following dependencies in your package.xml:
  <build_depend>rog_map</build_depend>
  <exec_depend>rog_map</exec_depend>
  1. Include rog_map in your CMakeLists.txt:
find_package(catkin REQUIRED COMPONENTS
        roscpp
        std_msgs
        pcl_ros
        geometry_msgs
        nav_msgs
        rog_map # here!
)
  1. Include rog_map in your source file as demonstrated in marsim_example_node.cpp
#include "rog_map/rog_map.h"

int main(int argc, char** argv) {
    ros::init(argc, argv, "rm_node");
    ros::NodeHandle nh("~");

    pcl::console::setVerbosityLevel(pcl::console::L_ALWAYS);

    /* 1. Creat a ROGMap ptr*/
    rog_map::ROGMap::Ptr rog_map_ptr = std::make_shared<rog_map::ROGMap>(nh);

    /* Publisher and subcriber */
    ros::AsyncSpinner spinner(0);
    spinner.start();
    ros::Duration(1.0).sleep();


    ros::waitForShutdown();
    return 0;
}
  1. ROG-Map automatically reads parameters from the ROS parameter server. Ensure you load parameters in your launch file:
<launch>
    <node name="rm_node" pkg="rog_map_example" type="marsim_example_node" output="log">
        <!--        remember to load the parameters like here!! -->
        <rosparam command="load" file="$(find rog_map_example)/config/marsim_example.yaml"/>
    </node>

    <node name="keyboard_control" pkg="rog_map_example" type="keyboard_control.py" output="screen">

    </node>
</launch>
  1. Update ROG-Map by either:
  • Using ROS topics:
    • Specify odom and point cloud topic name and ROG-Map will automatically update.
rog_map:
  ros_callback:
    enable: true
    cloud_topic: "/cloud_registered"
    odom_topic: "/lidar_slam/odom"
    odom_timeout: 2.0
  • Manually updating: Disable ROS topic updates in the configuration YAML:
rog_map:
  ros_callback:
    enable: false
    cloud_topic: "/cloud_registered"
    odom_topic: "/lidar_slam/odom"
    odom_timeout: 2.0

Then actively update ROG-Map by calling:

void ROGMap::updateMap(const PointCloud& cloud, const Pose& pose);

Parameters

We provide preset parameter files in ./examples/rog_map_example/config for your convenience. You can select and modify them as needed:

  • No raycasting, only occupied and inflated maps: no_raycast.yaml
  • Basic occupancy grid map with frontier generation and ESDF update disabled: pure_ogm.yaml
  • ...

Acknowledgements

Special thanks to ZJU-FAST-Lab and HKUST Aerial Robotics Group for their great works.