ntnu-arl / mbplanner_ros

Motion-primitives Based Planner for Fast & Agile Exploration

mbplanner_ros

Install and run:

• Follow the instructions in mbplanner_ws for installation.
• Launch the planner:

roslaunch mbplanner mbplanner_m100_sim.launch

This will launch the planner with the rviz visualizer with the UI which can be used to trigger the planner.
Alternately, the planner can be triggered by calling the respective rosservices as follows:
Start planner: rosservice call /planner_control_interface/std_srvs/automatic_planning "{}"
Stop planner: rosservice call /planner_control_interface/std_srvs/stop

• We provide 4 different gazebo simulation environments which can be found in the planner_gazebo_sim package. The world file for the desired environment needs to be specified in the mbplanner_m100_sim.launch file here:

<arg name="world_file" default="$(find planner_gazebo_sim)/worlds/pittsburgh_mine.world"/>

We also support the environments published by DARPA for the Subterranean Challenge - Cave Circuit. In order to use those environments, the models and worlds need to be downloaded from another package: subt_cave_sim. To compile this package:

catkin build subt_cave_sim

And use the mbplanner/launch/mbplanner_m100_sim_cave.launch file:

roslaunch mbplanner mbplanner_m100_sim_cave.launch

Select the mapping framework

The planner supports two mapping frameworks: Voxblox and Octomap

Voxblox

By default, the planner is compiled with Voxblox. Voxblox can be used with two mapping modes, Euclidean Signed Distance Fields (ESDF) and Truncated Signed Distance Fields (TSDF). By default TSDF is used in which the distance (to the closest occupied voxel) of a voxel is truncated to a fixed value.
In order to switch to ESDF make the following changes:
In the file planner_common/include/planner_common/map_manager_voxblox_impl.h comment the following line:

#define TSDF

In the file mbplanner/launch/mbplanner_m100_sim.launch comment the following line:

<remap from="global_planner_node/tsdf_map_in" to="mbplanner_node/tsdf_map_out"/>

Uncomment the following line:

<!-- <remap from="global_planner_node/esdf_map_in" to="mbplanner_node/esdf_map_out"/> -->

Octomap

To compile with Octomap, set the flag USE_OCTOMAP to 1:

catkin build -DCMAKE_BUILD_TYPE=Release -DUSE_OCTOMAP=1 mbplanner

Also change the config to Octomap in the mbplanner/launch/mbplanner_m100_sim.launch file Replace:

<arg name="map_config_file" default="$(arg voxblox_config_file)"/>

With:

<arg name="map_config_file" default="$(arg octomap_config_file)"/>

Tutorial:

You could find a short tutorial on the plannning algorithm and the overall architecture on our website: Link

Reference:

If you use this work in your research, please cite the following publication.

@inproceedings{mbplanner2020,
  title={Motion Primitives-based Agile Exploration Path Planning for Aerial Robotics},
  author={Dharmadhikari, Mihir and Dang, Tung and Solanka, Lukas and Loje, Johannes and Nguyen, Huan and Khedekar, Nikhil and Alexis, Kostas},
  booktitle={2020 IEEE International Conference on Robotics and Automation (ICRA)},
  year={2020},
  organization={IEEE}
}

Please contact us for any question:

• Mihir Dharmadhikari
• Tung Dang
• Kostas Alexis

mbplanner_ros's Issues

Planner service fails in Ubuntu 20.04

Hey @MihirDharmadhikari @tungdanganh @engcang ,
I read the paper and checked out the algorithm in Ubuntu 18.04 (Where it works). Congratulations on a great job.

Due to hardware constraints, I need to run this algorithm in Ubuntu 20.04.
I have successfully built mbplanner_ws and mbplanner_ros on Ubuntu 20.04.

When I run,
roslaunch mbplanner mbplanner_m100_sim.launch

This is the result with a blank rviz screen (The voxblox map is not created and no data from the lidar point cloud)

I checked this issue: #12 (comment)
According to this @MihirDharmadhikari suggested checking the motion primitives tree as well as the Voxblox map. I do not know how to check the motion primitives tree. All I can say is, that there is no tree formed in Rviz like in Ubuntu 18.04 when I start the planner(blue tree-like structure).

So I focused on the voxblox map. As I mentioned before, the voxblox map hasn't been created and there is no data from the lidar. I downloaded different versions of the sim and mapping packages to solve this issue. My current combination is :

Voxblox: master branch
Volumetric mapping: noetic/gbp_dev branch
RotorS_sim : /dev/arl_planner branch
Lidar_sim: noetic-devel branch

Now I can get the point cloud as well as the Voxlblox mesh. But the planner still fails. Same output in the terminal but Rviz now has Voxblox mesh (& lidar point-cloud).

What exactly is the issue? Is there anything else I can do?
Any suggestions would be helpful.

Thank you and congrats once again.

m100.gazebo

I can not find m100.gazebo，but the launch file include m100.gazebo.Could you tell me why and what should I do？

Using RGB-D camera instead of 3D Lidar

Hey @MihirDharmadhikari @engcang @ShreyanshDarshan,
I want to use a front facing depth camera instead of a 3D Lidar. I want to evaluate the working of the algorithm with limited FoV and range.

I tried to follow @engcang
changes in
#4 (comment)

I changed the SensorParams in global_planner_config.yaml and mbplanner_config.yaml as:

type: kCamera
max_range: 6.0
center_offset: [0.0, 0.0, 0.0]
rotations: [0.0, 0.0, 0.0]
fov: [rad(pi/2), rad(pi/3)]
resolution: [rad(5.0pi/180), rad(3.0pi/180)]

The simulation runs as usual and there is no change in FoV or range of the pointcloud

Please let me know if I missed something?
I am actively working on this problem, so any suggestion would be helpful

Thank you,

error occured when start planner

Input pointcloud queue getting too long! Dropping some pointclouds. Either unable to look up transform timestamps or the processing is taking too long.

something error with /mbplanner_node/FreeFrustParams load

Hi, first thanks for you open source code. And I am testing the proposed launch file mbplanner_m100_sim.launch with melodic branch, and countered the following problem.Could you tell me how to deal with this situation? Thanks a lot

Loading: /mbplanner_node/FreeFrustumParams
namespace_name:/mbplanner_node/FreeFrustumParams
[ERROR][File: /home/tina1994/CM_PHD_CODE/src/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/params.cpp] [Line: 235]
Param is not set: /mbplanner_node/FreeFrustumParams/sensor_list

Loading: /mbplanner_node/BoundedSpaceParams/Local
[WARN][File: /home/tina1994/CM_PHD_CODE/src/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/params.cpp] [Line: 431]
Param is not set: /global_planner_node/BoundedSpaceParams/Global/min_extension. Setting to default value: {0,0,0}

[WARN][File: /home/tina1994/CM_PHD_CODE/src/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/params.cpp] [Line: 442]
Param is not set: /global_planner_node/BoundedSpaceParams/Global/max_extension. Setting to default value: {0,0,0}

HELP！roslaunch mbplanner mbplanner_m100_sim.launch is wrong

when i launch roslaunch mbplanner mbplanner_m100_sim.launch，i can not find rotors_simulator/rotors_description/urdf/m100.gazebo，please help me

something error happend in catkin duild mbplanner_ros

first,thank you and your team for providing such a good algorithm
but when i catkin build this workspace,some error happend.
the error are as follows:

/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp: In member function ‘virtual explorer::MapManager::VoxelStatus explorer::MapManagerOctomap::getPathStatus(const Vector3d&, const Vector3d&, const Vector3d&, bool) const’:
/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp:59:25: error: ‘class volumetric_mapping::OctomapManager’ has no member named ‘getDirectionalLineStatusBoundingBox’; did you mean ‘getLineStatusBoundingBox’?
octomap_manager_->getDirectionalLineStatusBoundingBox(start, end, box_size, true);
^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
getLineStatusBoundingBox
In file included from /home/jojo/mbplanner_ws/src/mapping/voxblox/voxblox_ros/include/voxblox_ros/tsdf_server.h:12:0,
from /home/jojo/mbplanner_ws/src/mapping/voxblox/voxblox_ros/include/voxblox_ros/esdf_server.h:11,
from /home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/include/planner_common/map_manager_voxblox_impl.h:5,
from /home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_voxblox_impl.cpp:1:
/opt/ros/melodic/include/pcl_ros/point_cloud.h:303:27: warning: variable templates only available with -std=c++14 or -std=gnu++14
constexpr static bool pcl_uses_boost = true;
^~~~~~~~~~~~~~
/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp: In member function ‘virtual void explorer::MapManagerOctomap::getScanStatus(Eigen::Vector3d&, std::vector<Eigen::Matrix<double, 3, 1> >&, std::tuple<int, int, int>&, std::vector<std::pair<Eigen::Matrix<double, 3, 1>, explorer::MapManager::VoxelStatus> >&)’:
/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp:77:21: error: ‘class volumetric_mapping::OctomapManager’ has no member named ‘getScanStatus’; did you mean ‘getLineStatus’?
octomap_manager_->getScanStatus(pos, multiray_endpoints, gain_log_vec, voxel_log_temp);
^~~~~~~~~~~~~
getLineStatus
/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp: In member function ‘virtual void explorer::MapManagerOctomap::augmentFreeFrustum()’:
/home/jojo/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_octomap_impl.cpp:91:66: error: ‘class volumetric_mapping::OctomapManager’ has no member named ‘augmentFreeFrustum’
void MapManagerOctomap::augmentFreeFrustum() { octomap_manager_->augmentFreeFrustum(); }

is my version of octomap wrong?

experimental results

I can't find where the experimental results are stored. Can you help me?

Large Planning Times

Thanks for sharing this repository with the open-source community!
I just ran the simulated example. I started roslaunch mbplanner mbplanner_m100_sim.launch and then clicked on Start Planner in the UI.
Things looked awesome other than it would take quite a bit of time to compute the trajectories, as shows below. Are there any compiling/execution optimizations that I am missing, or is this expected? Are there GPU optimizations (I am running on a machine without an external GPU, if that matters)?

         Build tree     : 34.130 (s)
         Compute gain   : 35.360 (s)
         Evaluate graph : 6.000 (s)
         Total          : 75.490 (s)

How to run with custom data?

Hello there,
Thank you for sharing this amazing work! I'd like to modify this work and have it run on LIDAR pointclouds and odometry data generated from LOAM module.
I intend to use TSDF mode, so I looked up the tsdf_server.h file, and found both pointcloud_sub and freespace_pointcloud_sub. Could you kindly advise me what to do next?

• I am only able to build mbplanner_ws for now. During build of mbplanner_ros I had an error: cmake could not find package voxblox_ros. Also, there's no .rosinstall files in this repository, so I couldn't strictly followed the instructions in mbplanner_ws.

Thank you,

can't get path

Hi, thanks for your project. I have compiled it successfully, but when I run the launch file and clicked "start planer" in the rviz, the planner can't get an available path. The outputs from terminal are as follow:

How to use RGB-D camera?

Hi. @MihirDharmadhikari ,
I really appreciate sharing this great repo.

I tried to use RGB-D camera by changing all of

• exploration/mbplanner_ros/global_planner/config/global_planner_config.yaml
• exploration/mbplanner_ros/mbplanner/config/mbplanner_config.yaml
• exploration/mbplanner_ros/mbplanner/config/octomap_sim_config.yaml
• exploration/mbplanner_ros/mbplanner/config/planner_control_interface_sim_config.yaml
• exploration/mbplanner_ros/mbplanner/mbplanner.launch

especially, global_planner_config.yaml and mbplanner_config.yaml.

I changed the SensorParams as
type: kCamera
max_range: 15.0
center_offset: [0.0, 0.0, 0.0]
rotations: [0.0, 0.0, 0.0]
fov: [rad(pi/2), rad(pi/3)]
resolution: [rad(5.0pi/180), rad(3.0pi/180)]

But right after I changed the sensorparams, I can get only

[ WARN] [1607363547.506713317, 30.048000000]: No positive gain for any vertex
[ WARN] [1607363547.506726017, 30.048000000]: Zero gain
[ WARN] [1607363547.506776290, 30.048000000]: Planner service failed

Zero gain continuously.

Did I set any configuration wrong to use RGB-D Camera?

thank you in advance.