Planner service fails in Ubuntu 20.04

[ta-jetson]

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.

[drjsmith]

@ta-jetson I had the same experience and just got it running. I will post a link to updated instructions shortly

[drjsmith]

@ta-jetson Try following the instructions on this fork, assuming I didn't overlook anything it should get you the same set of packages that work for me (by work, I mean it starts off working correctly, though the planner often fails later).

[ta-jetson]

Hey @drjsmith ,
Thanks for helping out.
I followed the instructions on the fork (with some changes), and I got it to work. But yeah, the planner often fails.

Do you have any suggestions on how to solve this issue? I am actively working on it, so I could try and let you know.

[ta-jetson]

These are the changes I made :

• For cloning packages :

wstool init
wstool merge packages_https.rosinstall
wstool update 

catkin config -DCMAKE_BUILD_TYPE=Release
catkin build

• I added the DCMAKE_BUILD_TYPE=Release, to prevent long planning time.

• In the CMakelists.txt of ivaROS/src/sim/rotors_simulator/rotors_gazebo_plugins and ivaROS/src/mapping/voxblox/voxblox

• I changed this line from c++11 to c++17

add_definitions(-std=c++17)

• I cloned protobuf_catkin to src to prevent some other dependecy issue by voxblox_ros

https://github.com/ethz-asl/protobuf_catkin.git

[drjsmith]

@ta-jetson After a lot of debugging, I was able to solve the Planner service failed issue, though it's more of a workaround than a proper fix.

The planner service fails because the node providing the service segfaults.

[ WARN] [1713216085.897822147, 403.530000000]: ROOT INSIDE OBSTACLE

Thread 1 "mbplanner_node" received signal SIGSEGV, Segmentation fault.
std::__uninitialized_copy_a<__gnu_cxx::__normal_iterator<Eigen::Matrix<int, 3, 1, 0, 3, 1> const*, std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<int, 3, 1, 0, 3, 1>*, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > (
    __alloc=..., __result=0x0, __last=..., __first=...)
    at /usr/include/c++/9/bits/stl_uninitialized.h:293
293                 __traits::construct(__alloc, std::__addressof(*__cur), *__first);

Stack trace:

#0  std::__uninitialized_copy_a<__gnu_cxx::__normal_iterator<Eigen::Matrix<int, 3, 1, 0, 3, 1> const*, std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > >, Eigen::Matrix<int, 3, 1, 0, 3, 1>*, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > (
    __alloc=..., __result=0x0, __last=..., __first=...) at /usr/include/c++/9/bits/stl_uninitialized.h:293
#1  std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > >::vector (
    __x=<error reading variable: Cannot access memory at address 0x20>, this=<optimized out>) at /usr/include/c++/9/bits/stl_vector.h:555
#2  std::pair<Eigen::Matrix<int, 3, 1, 0, 3, 1>, std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > > >::pair<Eigen::Matrix<int, 3, 1, 0, 3, 1> const, std::vector<Eigen::Matrix<int, 3, 1, 0, 3, 1>, Eigen::aligned_allocator<Eigen::Matrix<int, 3, 1, 0, 3, 1> > >, true> (__p={...}, this=<optimized out>) at /usr/include/c++/9/bits/stl_pair.h:292
#3  explorer::MapManagerVoxblox<voxblox::TsdfServer, voxblox::TsdfVoxel>::augmentFreeBox (this=0x555555ae1a80, position=..., box_size=...)
    at /home/justin/workspaces/mbplanner_ws/src/exploration/mbplanner_ros/planner_common/src/map_manager_voxblox_impl.cpp:277
#4  0x00007ffff7d0541e in explorer::mbplanner::MBTree::buildTree (this=0x555555adf9b0, prev_point_plan=prev_point_plan@entry=false, 
    use_current_state=use_current_state@entry=true, use_given_vel=use_given_vel@entry=false)
    at /home/justin/workspaces/mbplanner_ws/src/exploration/mbplanner_ros/mbplanner/src/mbtree.cpp:221
#5  0x00007ffff7cef32f in explorer::MBPlanner::plannerServiceCallback (this=0x7fffffffc050, req=..., res=...)
    at /home/justin/workspaces/mbplanner_ws/src/exploration/mbplanner_ros/mbplanner/src/mbplanner.cpp:81
#6  0x00007ffff7cfc2eb in boost::function2<bool, planner_msgs::planner_srvRequest_<std::allocator<void> >&, planner_msgs::planner_srvResponse_<std::allocator<void> >&>::operator() (a1=..., a0=..., this=0x555555ae8b08) at /usr/include/boost/function/function_template.hpp:677
#7  ros::ServiceSpec<planner_msgs::planner_srvRequest_<std::allocator<void> >, planner_msgs::planner_srvResponse_<std::allocator<void> > >::call(boost::function<bool (planner_msgs::planner_srvRequest_<std::allocator<void> >&, planner_msgs::planner_srvResponse_<std::allocator<void> >&)> const&, ros::ServiceSpecCallParams<planner_msgs::planner_srvRequest_<std::allocator<void> >, planner_msgs::planner_srvResponse_<std::allocator<void> > >&) (params=<synthetic pointer>..., cb=...)
    at /home/justin/workspaces/ros_catkin_ws/rinstall/include/ros/service_callback_helper.h:125
#8  ros::ServiceCallbackHelperT<ros::ServiceSpec<planner_msgs::planner_srvRequest_<std::allocator<void> >, planner_msgs::planner_srvResponse_<std::allocator<void> > > >::call (this=0x555555ae8b00, params=...) at /home/justin/workspaces/ros_catkin_ws/rinstall/include/ros/service_callback_helper.h:182
#9  0x00007ffff7dd5b55 in ros::ServiceCallback::call (this=0x7fffe000a500)
    at /home/justin/workspaces/ros_catkin_ws/src/ros_comm/roscpp/src/libros/service_publication.cpp:125
#10 0x00007ffff7e27e82 in ros::CallbackQueue::callOneCB (this=0x555555ad22c0, tls=0x555555b1b930)
    at /home/justin/workspaces/ros_catkin_ws/src/ros_comm/roscpp/src/libros/callback_queue.cpp:410
#11 0x00007ffff7e294b3 in ros::CallbackQueue::callAvailable (this=this@entry=0x555555ad22c0, timeout=...)
    at /home/justin/workspaces/ros_catkin_ws/src/ros_comm/roscpp/src/libros/callback_queue.cpp:351
#12 0x00007ffff7e7b2ff in ros::SingleThreadedSpinner::spin (this=<optimized out>, queue=<optimized out>)
    at /home/justin/workspaces/ros_catkin_ws/src/ros_comm/roscpp/src/libros/spinner.cpp:141
#13 0x00007ffff7e63d4f in ros::spin () at /home/justin/workspaces/ros_catkin_ws/src/ros_comm/roscpp/src/libros/init.cpp:563
#14 0x000055555555dca7 in main (argc=<optimized out>, argv=<optimized out>)
    at /home/justin/workspaces/mbplanner_ws/src/exploration/mbplanner_ros/mbplanner/src/mbplanner_ros_node.cpp:143

The ROOT INSIDE OBSTACLE warning indicates that the voxel map contains obstacles within the robot's bounding box at the time of planning. The action taken is to remove all such obstacles (I assume the reasoning is that the robot is there so it must be safe) using the function MapManagerVoxblox<SDFServerType, SDFVoxelType>::augmentFreeBox in planner_common/src/map_manager_voxblox_impl.cpp:272.
The function populates block_voxel_list and iterates over the contents. The initial populated block_voxel_list is fine (no errors when printing the object in gdb). The segfault occurs while iterating; by this point block_voxel_list is corrupted (...std::vector of length -1789569706, capacity 0 = {Cannot access memory at address 0x700000002...)

I noticed that the segfault did not occur when using my 'debug' profile (`-O0 -g), so I moved the problematic code to a separate compilation unit with no optimizations enabled, and it seems to work fine now, and the only code that is slowed is the code that would otherwise have crashed the program.

However, as I noted previously, this is really just a workaround; it works for me, but I can't guarantee it will work for any other system. It would be much better to identify and fix the true source of the memory corruption.

I have pushed my changes to github, wstool update -t src should pull them for you. Or, since you've made other modifications to the workspace, you can just pull the mbplanner_ros repo.

Hope this helps.