This repo contains 3D occupancy map, dynamic obstacle detection and osbatcle trajectory prediction for autonomous navigation. The dynamic obstacles will be detected, tracked and merged into the static occupancy map with a corresponding trajectory prediction (ICRA2023)
Youtube Demo Video Link: https://www.youtube.com/watch?v=u5zblVx8KRc
You may want to link Eigen to defalt location if Eigen is not found
sudo ln -s /usr/include/eigen3/Eigen/ /usr/include/Eigen
prerequisite: Eigen, OpenCV
git clone https://github.com/Shawn207/map_manager_pub.git
cd ~/catkin_ws
catkin_make
Please specify the camera parameters and the depth image/pose topic in map_manager_pub/map_manager/cfg/dynamic_map_param
, then run the dynamic map launch:
roslaunch map_manager dynamic_map.launch
roslaunch map_manager rviz.launch
Please find parameters in map_manager_pub/map_manager/cfg/dynamic_map_param.yaml
files. The parameters are explained by comments.
Subsribe the following topics for occupancy and ESDF map:
robot/odometry
or robot/pose
(please enter the name of your topic in the parameter files)camera/depth
(defined in the config file)Publish the following topics:
dynamic_map/inflated_voxel_map
dynamic_map/box_visualization_marker
dynamic_map/traj_marker
Example code:
#include <map_manager/dynamicMap.h>
int main(){
...
map_manager::dynamicMap m;
m.initMap(nh);
// collision checking with static obstacles given a point
Eigen::Vector3d pos (1.0, 1.0, 1.0)
bool hasCollision = m.isOccupied(pos);
// get dynamic obstacles information(poseition, velocity, size)
std::vector<Eigen::Vector3d> obstaclesPos;
std::vector<Eigen::Vector3d> obstaclesVel;
std::vector<Eigen::Vector3d> obstaclesSize;
m.getDynamicObstacles(obstaclesPos, obstaclesVel, obstaclesSize);
// get predicted trajectory
std::vector<std::vector<std::vector<geometry_msgs::Point>>> predTraj;
m.getTPredTraj(predTraj);
...
}
If you find this work useful, please cite the paper:
@article{xu2022real,
title={A real-time dynamic obstacle tracking and mapping system for UAV navigation and collision avoidance with an RGB-D camera},
author={Xu, Zhefan and Zhan, Xiaoyang and Chen, Baihan and Xiu, Yumeng and Yang, Chenhao and Shimada, Kenji},
journal={arXiv preprint arXiv:2209.08258},
year={2022}
}