costmap_2d::VoxelLayer::raytraceFreeSpace fails with because clearing_endpoints_ is empty #2692
Description
Bug report
Required Info:
- Operating System:
Archlinux - ROS2 Version:
Galactic (built from source) (gcc 11.1.0) (repos file) (galactic branch of https://github.com/ros2/ros2) (HEAD: c7c318e) - Nav2 Version or commit hash:
Galactic (built from source) (gcc 11.1.0) (git clone) (galactic branch of this repo) (HEAD: d8f50da) - DDS implementation:
- rmw_fastrtps_cpp
Steps to reproduce issue
ros2 launch nav2_bringup tb3_simulation_launch.py slam:=True
Expected behavior
Launch a gazebo simulation with turtlebot3 in a closed environment, with Nav2 running in slam mode. User should be able to mark "2D pose estimate" and "Nav2 Goal" in rviz, and the robot should move to target position.
Actual behavior
nav2_controller node fails at startup with no error message. As a consequence, "Goal was rejected by server" is printed when user selects a goal in rviz
Additional information
I recompiled everything with debug info (-DCMAKE_BUILD_TYPE=RelWithDebInfo) and got this backtrace:
#0 0x00007ffff74a0d22 in raise () from /usr/lib/libc.so.6
#1 0x00007ffff748a862 in abort () from /usr/lib/libc.so.6
#2 0x00005555555be5fa in std::__replacement_assert (__file=<optimized out>,
__line=<optimized out>, __function=<optimized out>, __condition=<optimized out>)
at /usr/include/c++/11.1.0/x86_64-pc-linux-gnu/bits/c++config.h:504
#3 0x00007ffff7c6dcf8 in std::vector<unsigned char, std::allocator<unsigned char> >::front (
this=0x7fffb00042c8) at /usr/include/c++/11.1.0/bits/stl_vector.h:1121
#4 std::vector<unsigned char, std::allocator<unsigned char> >::front (this=0x7fffb00042c8)
at /usr/include/c++/11.1.0/bits/stl_vector.h:1121
#5 sensor_msgs::impl::PointCloud2IteratorBase<float, float, unsigned char, sensor_msgs::msg::PointCloud2_<std::allocator<void> >, sensor_msgs::PointCloud2Iterator>::PointCloud2IteratorBase (this=0x7fffc1ff7fe0, cloud_msg=..., field_name=...)
at /opt/ros2/galactic/include/sensor_msgs/impl/point_cloud2_iterator.hpp:275
#6 0x00007fffe476d80f in sensor_msgs::PointCloud2Iterator<float>::PointCloud2Iterator (
field_name="x", cloud_msg=..., this=0x7fffc1ff7fe0)
at /opt/ros2/galactic/include/sensor_msgs/point_cloud2_iterator.hpp:295
#7 nav2_costmap_2d::VoxelLayer::raytraceFreespace (this=0x555555892980,
clearing_observation=..., min_x=0x55555584cf78, min_y=0x55555584cf80,
max_x=0x55555584cf88, max_y=0x55555584cf90)
at /home/mostafa/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:350
#8 0x00007fffe4770fb6 in nav2_costmap_2d::VoxelLayer::updateBounds (this=0x555555892980,
robot_x=-1.9999543006678138, robot_y=-0.50000042315649595,
robot_yaw=0.00053837867434282133, min_x=0x55555584cf78, min_y=0x55555584cf80,
max_x=0x55555584cf88, max_y=0x55555584cf90)
at /home/mostafa/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:162
#9 0x00007ffff7bfd3f6 in nav2_costmap_2d::LayeredCostmap::updateMap (this=0x55555584ced0,
robot_x=-1.9999543006678138, robot_y=-0.50000042315649595,
robot_yaw=robot_yaw@entry=0.00053837867434282133)
at /home/mostafa/navigation2/nav2_costmap_2d/src/layered_costmap.cpp:161
#10 0x00007ffff7c07bab in nav2_costmap_2d::Costmap2DROS::updateMap (this=0x5555557713b0)
at /home/mostafa/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:458
#11 0x00007ffff7c08098 in nav2_costmap_2d::Costmap2DROS::mapUpdateLoop (this=0x5555557713b0,
frequency=<optimized out>)
at /home/mostafa/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:412
#12 0x00007ffff78623c4 in std::execute_native_thread_routine (__p=0x555555ac9c40)
at /build/gcc/src/gcc/libstdc++-v3/src/c++11/thread.cc:82
#13 0x00007ffff79cc259 in start_thread () from /usr/lib/libpthread.so.0
#14 0x00007ffff75625e3 in clone () from /usr/lib/libc.so.6So if I'm not wrong, the problem is that in this line:
https://github.com/ros-planning/navigation2/blob/7befc4975d3df4e252677a7a2b7c1d6c783fbd20/nav2_costmap_2d/plugins/voxel_layer.cpp#L350
*clearing_endpoints_ contains no data.
Running again and stepping through these lines:
https://github.com/ros-planning/navigation2/blob/7befc4975d3df4e252677a7a2b7c1d6c783fbd20/nav2_costmap_2d/plugins/voxel_layer.cpp#L326-L342
I found that the code inside that if branch never runs. In fact, when I run print publish_clearing_points, gdb says that it's optimized out.
I tried reverting aeccbe7 and the problem was gone. I could set a goal for the robot and it moved to that point just fine. Maybe that gives us a clue.