CostmapLayer::matchSize may be executed concurrently

[Cryst4L9527]

---

Basic Info

Info	Please fill out this column
Ticket(s) this addresses
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of turtlebot

---

Description of contribution in a few bullet points

just the second last commit is about this issue, Sorry I failed to avoid keeping the commit before still shown, maybe I should fork this project again?

It's just like PR #2513 ,

there's a opportunity that onInitialize() and matchSize() can be executed together:

void ObstacleLayer::onInitialize()
{
  bool track_unknown_space;
  double transform_tolerance;
  …………
  ObstacleLayer::matchSize();

void VoxelLayer::matchSize()
{
  std::lock_guard<Costmap2D::mutex_t>guard(*getMutex());
  ObstacleLayer::matchSize();
  voxel_grid_.resize(size_x_, size_y_, size_z_);
  assert(voxel_grid_.sizeX() == size_x_ && voxel_grid_.sizeY() == size_y_);
}

and this ObstacleLayer is a child of CostmapLayer, the matchSize() is just CostmapLayer::matchSize().

void CostmapLayer::matchSize()
{
  std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
  Costmap2D * master = layered_costmap_->getCostmap();
  resizeMap(
    master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
    master->getOriginX(), master->getOriginY());
}

And, in the resizeMap(), here are two function operating the costmap_:

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
  std::unique_lock<mutex_t> lock(*access_);
  delete[] costmap_;
  costmap_ = new unsigned char[size_x * size_y];
}

void Costmap2D::resizeMap(
  unsigned int size_x, unsigned int size_y, double resolution,
  double origin_x, double origin_y)
{
  size_x_ = size_x;
  size_y_ = size_y;
  resolution_ = resolution;
  origin_x_ = origin_x;
  origin_y_ = origin_y;
  initMaps(size_x, size_y);
  // reset our maps to have no information
  resetMaps();
}

void Costmap2D::resetMaps()
{
  std::unique_lock<mutex_t> lock(*access_);
  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

Although both of initMaps() and resetMaps() have a lock, but there is a possibility, when the initMaps() is finished，before resetMaps(), another resizeMap() go in and change the size_x_ and size_y_，leading to a overflow at the memset(), just like the message below shows. I've output some information in the process to confirm this cause.

==1629489==ERROR: AddressSanitizer: heap-buffer-overflow on address 0x626000032810 at pc 0x00000049652f bp 0x7ffe55a1d070 sp 0x7ffe55a1c838
WRITE of size 147456 at 0x626000032810 thread T0
    #0 0x49652e in __asan_memset (/home/r1/ros2_nav_fuzz/build/nav2_planner/planner_server+0x49652e)
    #1 0x7f1c96687cbc in nav2_costmap_2d::Costmap2D::resetMaps() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/src/costmap_2d.cpp:94:3
    #2 0x7f1c970f6609 in nav2_costmap_2d::VoxelLayer::resetMaps() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:125:18
    #3 0x7f1c96687a08 in nav2_costmap_2d::Costmap2D::resizeMap(unsigned int, unsigned int, double, double, double) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/src/costmap_2d.cpp:88:3
    #4 0x7f1c96926314 in nav2_costmap_2d::CostmapLayer::matchSize() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/src/costmap_layer.cpp:59:3
    #5 0x7f1c96f9e9e8 in nav2_costmap_2d::ObstacleLayer::onInitialize() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/plugins/obstacle_layer.cpp:104:18
    #6 0x7f1c970f32aa in nav2_costmap_2d::VoxelLayer::onInitialize() /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/plugins/voxel_layer.cpp:62:18
    #7 0x7f1c96693ced in nav2_costmap_2d::Layer::initialize(nav2_costmap_2d::LayeredCostmap*, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, tf2_ros::Buffer*, std::shared_ptr<rclcpp_lifecycle::LifecycleNode>, std::shared_ptr<rclcpp::Node>, std::shared_ptr<rclcpp::Node>) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/src/layer.cpp:61:3
    #8 0x7f1c966b8a75 in nav2_costmap_2d::Costmap2DROS::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_costmap_2d/src/costmap_2d_ros.cpp:153:13
    #9 0x7f1c9751051c in nav2_planner::PlannerServer::on_configure(rclcpp_lifecycle::State const&) /home/r1/ros2_nav_fuzz/src/navigation2/nav2_planner/src/planner_server.cpp:71:17
    #10 0x7f1c973b3c27 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::execute_callback(unsigned int, rclcpp_lifecycle::State const&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2bc27)
    #11 0x7f1c973b416c in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::change_state(unsigned char, rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2c16c)
    #12 0x7f1c973b4677 in rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::on_change_state(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2c677)
    #13 0x7f1c973ab821 in std::_Function_handler<void (std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >), std::_Bind<void (rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl::* (rclcpp_lifecycle::LifecycleNode::LifecycleNodeInterfaceImpl*, std::_Placeholder<1>, std::_Placeholder<2>, std::_Placeholder<3>))(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<rmw_request_id_t>&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Request_<std::allocator<void> > >&&, std::shared_ptr<lifecycle_msgs::srv::ChangeState_Response_<std::allocator<void> > >&&) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x23821)
    #14 0x7f1c973ae17a in rclcpp::Service<lifecycle_msgs::srv::ChangeState>::handle_request(std::shared_ptr<rmw_request_id_t>, std::shared_ptr<void>) (/opt/ros/foxy/lib/librclcpp_lifecycle.so+0x2617a)
    #15 0x7f1c9586f5dc  (/opt/ros/foxy/lib/librclcpp.so+0xd65dc)
    #16 0x7f1c9587002b  (/opt/ros/foxy/lib/librclcpp.so+0xd702b)
    #17 0x7f1c9587029b in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>) (/opt/ros/foxy/lib/librclcpp.so+0xd729b)
    #18 0x7f1c9587113c in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/foxy/lib/librclcpp.so+0xd813c)
    #19 0x7f1c95875a4b in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/foxy/lib/librclcpp.so+0xdca4b)
    #20 0x7f1c958734e7 in rclcpp::spin(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>) (/opt/ros/foxy/lib/librclcpp.so+0xda4e7)
    #21 0x4c90d9 in main /home/r1/ros2_nav_fuzz/src/navigation2/nav2_planner/src/main.cpp:25:3
    #22 0x7f1c94efb0b2 in __libc_start_main (/lib/x86_64-linux-gnu/libc.so.6+0x270b2)
    #23 0x41e68d in _start (/home/r1/ros2_nav_fuzz/build/nav2_planner/planner_server+0x41e68d)

Description of documentation updates required from your changes

---

Future work that may be required in bullet points

In fact, there is also some other places in costmap_2d.cpp where the _size_x and _size_y is changed first, and is followed by initMaps(). then another operating of costmap_ using _size_x and _size_y, which may cause the same type of crashes like above, I think maybe we can change the _size_x and _size_y in the initMaps(), for example :

void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
  std::unique_lock<mutex_t> lock(*access_);
  delete[] costmap_;
  costmap_ = new unsigned char[size_x * size_y];
  size_x_ = size_x;
  size_y_ = size_y;
}

void Costmap2D::resizeMap(
  unsigned int size_x, unsigned int size_y, double resolution,
  double origin_x, double origin_y)
{
  // size_x_ = size_x;
  // size_y_ = size_y;
  resolution_ = resolution;
  origin_x_ = origin_x;
  origin_y_ = origin_y;
  initMaps(size_x, size_y);
  // reset our maps to have no information
  resetMaps();
}

void Costmap2D::resetMaps()
{
  std::unique_lock<mutex_t> lock(*access_);
  memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

which can keep the two params fit the real size of costmap all the time.

For Maintainers:

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[mergify]

@Cryst4L9527, please properly fill in PR template in the future. @SteveMacenski, use this instead.

• Check that any new parameters added are updated in navigation.ros.org
• Check that any significant change is added to the migration guide
• Check that any new features OR changes to existing behaviors are reflected in the tuning guide
• Check that any new functions have Doxygen added
• Check that any new features have test coverage
• Check that any new plugins is added to the plugins page
• If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

[codecov]

Codecov Report

Base: 82.27% // Head: 82.10% // Decreases project coverage by -0.17% ⚠️

Coverage data is based on head (e112a49) compared to base (987fe90).
Patch has no changes to coverable lines.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #3250      +/-   ##
==========================================
- Coverage   82.27%   82.10%   -0.18%     
==========================================
  Files         341      341              
  Lines       15537    15537              
==========================================
- Hits        12783    12756      -27     
- Misses       2754     2781      +27     

Impacted Files	Coverage Δ
nav2_costmap_2d/src/costmap_layer.cpp	38.20% <ø> (ø)
...av2_dwb_controller/dwb_critics/src/oscillation.cpp	72.00% <0.00%> (-24.00%)	⬇️
..._planner/include/nav2_smac_planner/node_hybrid.hpp	92.85% <0.00%> (-7.15%)	⬇️
...stmap_2d/plugins/costmap_filters/binary_filter.cpp	94.80% <0.00%> (-2.60%)	⬇️
..._amcl/src/sensors/laser/likelihood_field_model.cpp	95.00% <0.00%> (-2.50%)	⬇️
...ostmap_2d/plugins/costmap_filters/speed_filter.cpp	95.18% <0.00%> (-2.41%)	⬇️
nav2_navfn_planner/src/navfn_planner.cpp	89.17% <0.00%> (-1.04%)	⬇️
...tree/include/nav2_behavior_tree/bt_action_node.hpp	80.70% <0.00%> (-0.88%)	⬇️

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

☔ View full report at Codecov.
📢 Do you have feedback about the report comment? Let us know in this issue.

[AlexeyMerzlyakov]

Here is the concurrent flow diagram for Costmap2D::resizeMap(), I've briefly drawn:

From this, currently we have the following vulnerable/unsafe calls for concurrent usage of Costmap2D::resizeMap() with LayeredCostmap::resizeMap() (marked with red at the diagram):

• ObstacleLayer::onInitialize()
• VoxelLayer::onInitialize()
• RangeSensorLayer::onInitialize()

All these concurrent call chains might be locked by one place in CostampLayer::matchSize() which is actually done by this PR.

Additionally, do we need now the lock powered by #2513 in VoxelLayer::matchSize() ? It calls the CostmapLayer::matchSize() that will be already locked, and voxel_grid_.resize() that should be safe from the first sight.

[Cryst4L9527]

@AlexeyMerzlyakov I don't think so, see first example in issue #2508 , although voxel_grid_.resize() is fine itself, when run CostmapLayer::matchSize() ,and in it Costmap2D::resizeMap(), and in it Costmap2D::resetMaps(), it runs actually the VoxelLayer::resetMaps() (VoxelLayer is child of Costmap2D) which contains Costmap2D::resetMaps() and voxel_grid_.reset(),

here my crash happened because the conflicts between Costmap2D::resetMaps() and Costmap2D::initMaps() : the size may change after init's allocation of costmap and reset use another size to memset;

and the crash in #2508 is because the conflicts between voxel_grid_.reset() and voxel_grid_.resize(), when resize of voxel_grid the data will be delete first and reset can't find the freed data.

But Whatever, all this unsafe calls caused by CL::matchSize() will be avoided by these PRs this time. I've checked there is only a VoxelLayer::resetMaps() overwriting the method in CL::matchSize() in all these classes

[AlexeyMerzlyakov]

Okay, got it. Indeed, the visible to be safe from first glance voxel_grid_.resize() is gonna not to be safe, because of concurrent usage with itself in VoxelLayer::matchSize() and also with VoxelLayer::resetMaps() (which actually to be called from LC::resizeMap() triggered by static layer incoming map).
Thus, the lock for VoxelLayer::matchSize() is added to protect voxel_grid_ variable, and lock for CostmapLayer::matchSize() should be added to keep size_x_ and size_y_ values in Costmap2D correct before memset of costmap_.
Thank you for the diligent work on this!

[SteveMacenski]

Thanks for taking care of this - I agree with this change