Skip to content

InflationLayer update collide when footprint is updated #1931

@daisukes

Description

@daisukes

Bug report

Required Info:

  • Operating System:
    • Ubuntu20.04
  • ROS2 Version:
    • Foxy src build
  • Version or commit hash:
    • master
  • DDS implementation:
    • Fast-DDS

Steps to reproduce issue

This rarely happens with my robot simulation test, once in a few hours. My robot changes footprint based on the situation.

I could reproduce this intentionally with the tb3 simulation.

  • launch tb3 simulation and activate by providing an initial position
  • publish random footprint 100hz

Backtraces

  • Here are some of the backtraces
#0  0x00007f0f181f418b in raise () from /usr/lib/x86_64-linux-gnu/libc.so.6
#1  0x00007f0f181d3859 in abort () from /usr/lib/x86_64-linux-gnu/libc.so.6
#2  0x00007f0f1823e3ee in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
#3  0x00007f0f1824647c in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
#4  0x00007f0f1824812c in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
#5  0x00007f0ef70ef1fd in void std::vector<nav2_costmap_2d::CellData, std::allocator<nav2_costmap_2d::CellData> >::emplace_back<unsigned int&, unsigned int&, unsigned int&, unsigned int&, unsigned int&>(unsigned int&, unsigned int&, unsigned int&, unsigned int&, unsigned int&) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/liblayers.so
#6  0x00007f0ef70ec76e in nav2_costmap_2d::InflationLayer::updateCosts(nav2_costmap_2d::Costmap2D&, int, int, int, int) ()
   from /opt/overlay_ws/install/nav2_costmap_2d/lib/liblayers.so
#7  0x00007f0f189965f1 in nav2_costmap_2d::LayeredCostmap::updateMap(double, double, double) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#8  0x00007f0f1899c53c in nav2_costmap_2d::Costmap2DROS::updateMap() () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#9  0x00007f0f1899ca64 in nav2_costmap_2d::Costmap2DROS::mapUpdateLoop(double) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#10 0x00007f0f185e2cb4 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#11 0x00007f0f186f6609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#12 0x00007f0f182d0103 in clone () from /usr/lib/x86_64-linux-gnu/libc.so.6
#0  0x00007f8721cf6a41 in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
[Current thread is 1 (Thread 0x7f86feffd700 (LWP 180))]
(gdb) bt
#0  0x00007f8721cf6a41 in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
#1  0x00007f8721cf7bc9 in ?? () from /usr/lib/x86_64-linux-gnu/libc.so.6
#2  0x00007f871c39c4aa in nav2_costmap_2d::InflationLayer::computeCaches() () from /opt/overlay_ws/install/nav2_costmap_2d/lib/liblayers.so
#3  0x00007f871c39c94a in nav2_costmap_2d::InflationLayer::onFootprintChanged() () from /opt/overlay_ws/install/nav2_costmap_2d/lib/liblayers.so
#4  0x00007f8722445742 in nav2_costmap_2d::LayeredCostmap::setFootprint(std::vector<geometry_msgs::msg::Point_<std::allocator<void> >, std::allocator<geometry_msgs::msg::Point_<std::allocator<void> > > > const&) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#5  0x00007f8722448059 in nav2_costmap_2d::Costmap2DROS::setRobotFootprintPolygon(std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >) ()
   from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#6  0x00007f872245d2d8 in std::_Function_handler<void (std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >), std::_Bind<void (nav2_costmap_2d::Costmap2DROS::*(nav2_costmap_2d::Costmap2DROS*, std::_Placeholder<1>))(std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >&&) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#7  0x00007f87224881fd in rclcpp::AnySubscriptionCallback<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<geometry_msgs::msg::Polygon_<std::allocator<void> > >, rclcpp::MessageInfo const&) () from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#8  0x00007f87224892f8 in rclcpp::Subscription<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<void>, rclcpp::message_memory_strategy::MessageMemoryStrategy<geometry_msgs::msg::Polygon_<std::allocator<void> >, std::allocator<void> > >::handle_message(std::shared_ptr<void>&, rclcpp::MessageInfo const&) ()
   from /opt/overlay_ws/install/nav2_costmap_2d/lib/libnav2_costmap_2d_core.so
#9  0x00007f8722bf2b06 in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>)::{lambda()#6}::operator()() const ()
   from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#10 0x00007f8722bf597d in std::_Function_handler<void (), rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>)::{lambda()#6}>::_M_invoke(std::_Any_data const&) () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#11 0x00007f8722bd6174 in std::function<void ()>::operator()() const () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#12 0x00007f8722bf21b8 in take_and_do_error_handling(char const*, char const*, std::function<bool ()>, std::function<void ()>) () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#13 0x00007f8722bf31d6 in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#14 0x00007f8722bf1f05 in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#15 0x00007f8722bff91c in rclcpp::executors::SingleThreadedExecutor::spin() () from /opt/ros/foxy/rclcpp/lib/librclcpp.so
#16 0x00007f872238db4d in std::thread::_State_impl<std::thread::_Invoker<std::tuple<nav2_util::NodeThread::NodeThread(std::shared_ptr<rclcpp::node_interfaces::NodeBaseInterface>)::{lambda()#1}> > >::_M_run() () from /opt/overlay_ws/install/nav2_util/lib/libnav2_util_core.so
#17 0x00007f8722092cb4 in ?? () from /usr/lib/x86_64-linux-gnu/libstdc++.so.6
#18 0x00007f87221a6609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#19 0x00007f8721d80103 in clone () from /usr/lib/x86_64-linux-gnu/libc.so.6

Additional information

I suspected multithread issue and tried to fix
CMU-cabot@e524132

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions