Inappropriate pointer free order in amcl may trigger use-after-free bug #4068

GoesM · 2024-01-25T13:36:00Z

Bug report

Required Info:

Operating System:
- Ubuntu22.04
ROS2 Version:
- humble
Version or commit hash:
- the latest
DDS implementation:
- defaulted

Steps to reproduce issue

During my very ordinary use, an unexpected situation was triggered.

I launched the navigation2 normally, as following steps:

#!/bin/bash
export ASAN_OPTIONS=halt_on_error=0:new_delete_type_mismatch=0:detect_leaks=0:log_pah=asan
source install/setup.bash
export TURTLEBOT3_MODEL=waffle
export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:/opt/ros/humble/share/turtlebot3_gazebo/models
ros2 launch nav2_bringup tb3_simulation_launch.py headless:=True use_rviz:=False use_composition:=False

Expected behavior

Actual behavior

During my usage, the ASAN report a use-after-free bug to me, as following:

=================================================================
==1627006==ERROR: AddressSanitizer: heap-use-after-free on address 0x61700009e6b8 at pc 0x7ff9b0cf3539 bp 0x7ff9a7db9230 sp 0x7ff9a7db9228
READ of size 8 at 0x61700009e6b8 thread T11
    #0 0x7ff9b0cf3538 in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x4f3538) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #1 0x7ff9b0d38790 in tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::transformReadyCallback(tf2_ros::TransformStampedFuture const&, unsigned long) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x538790) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #2 0x7ff9b0d3cc6e in std::_Function_handler<void (tf2_ros::TransformStampedFuture const&), std::_Bind<void (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>::* (tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*, std::_Placeholder<1>, unsigned long))(tf2_ros::TransformStampedFuture const&, unsigned long)> >::_M_invoke(std::_Any_data const&, tf2_ros::TransformStampedFuture const&) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x53cc6e) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #3 0x7ff9b2091e8d  (/opt/ros/humble/lib/libtf2_ros.so+0x4fe8d) (BuildId: dc6ad94876402bc27e6b8ba01e2b78c714ce71a6)
    #4 0x7ff9b200d5fe in tf2::BufferCore::testTransformableRequests() (/opt/ros/humble/lib/libtf2.so+0x105fe) (BuildId: 3860beff8328ff840e10c2fded56403848cba9a0)
    #5 0x7ff9b2010321 in tf2::BufferCore::setTransformImpl(tf2::Transform const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::chrono::time_point<std::chrono::_V2::system_clock, std::chrono::duration<long, std::ratio<1l, 1000000000l> > >, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (/opt/ros/humble/lib/libtf2.so+0x13321) (BuildId: 3860beff8328ff840e10c2fded56403848cba9a0)
    #6 0x7ff9b2010b69 in tf2::BufferCore::setTransform(geometry_msgs::msg::TransformStamped_<std::allocator<void> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, bool) (/opt/ros/humble/lib/libtf2.so+0x13b69) (BuildId: 3860beff8328ff840e10c2fded56403848cba9a0)
    #7 0x7ff9b209abb0 in tf2_ros::TransformListener::subscription_callback(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>, bool) (/opt/ros/humble/lib/libtf2_ros.so+0x58bb0) (BuildId: dc6ad94876402bc27e6b8ba01e2b78c714ce71a6)
    #8 0x7ff9b209aa89 in std::_Function_handler<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>), std::_Bind<void (tf2_ros::TransformListener::* (tf2_ros::TransformListener*, std::_Placeholder<1>, bool))(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>, bool)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>&&) (/opt/ros/humble/lib/libtf2_ros.so+0x58a89) (BuildId: dc6ad94876402bc27e6b8ba01e2b78c714ce71a6)
    #9 0x7ff9b20af601 in std::__detail::__variant::__gen_vtable_impl<std::__detail::__variant::_Multi_array<std::__detail::__variant::__deduce_visit_result<void> (*)(rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)::'lambda'(auto&&)&&, std::variant<std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&)>, std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&)>, std::integer_sequence<unsigned long, 8ul> >::__visit_invoke(rclcpp::AnySubscriptionCallback<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::allocator<void> >::dispatch(std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)::'lambda'(auto&&)&&, std::variant<std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&)>, std::function<void (tf2_msgs::msg::TFMessage_<std::allocator<void> > const&, rclcpp::MessageInfo const&)>, std::function<void (rclcpp::SerializedMessage const&)>, std::function<void (rclcpp::SerializedMessage const&, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >)>, std::function<void (std::unique_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> >, std::default_delete<tf2_msgs::msg::TFMessage_<std::allocator<void> > > >, rclcpp::MessageInfo const&)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >)>, std::function<void (std::unique_ptr<rclcpp::SerializedMessage, std::default_delete<rclcpp::SerializedMessage> >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const>, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const> const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage const> const&, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >)>, std::function<void (std::shared_ptr<tf2_msgs::msg::TFMessage_<std::allocator<void> > >, rclcpp::MessageInfo const&)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>)>, std::function<void (std::shared_ptr<rclcpp::SerializedMessage>, rclcpp::MessageInfo const&)> >&) (/opt/ros/humble/lib/libtf2_ros.so+0x6d601) (BuildId: dc6ad94876402bc27e6b8ba01e2b78c714ce71a6)
    #10 0x7ff9b20ae93e  (/opt/ros/humble/lib/libtf2_ros.so+0x6c93e) (BuildId: dc6ad94876402bc27e6b8ba01e2b78c714ce71a6)
    #11 0x7ff9b1c4c7ab in rclcpp::Executor::execute_subscription(std::shared_ptr<rclcpp::SubscriptionBase>) (/opt/ros/humble/lib/librclcpp.so+0xe77ab) (BuildId: 79589eb7552577357ad155ddcc914b38e18f0811)
    #12 0x7ff9b1c4cfae in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&) (/opt/ros/humble/lib/librclcpp.so+0xe7fae) (BuildId: 79589eb7552577357ad155ddcc914b38e18f0811)
    #13 0x7ff9b1c5489f in rclcpp::executors::SingleThreadedExecutor::spin() (/opt/ros/humble/lib/librclcpp.so+0xef89f) (BuildId: 79589eb7552577357ad155ddcc914b38e18f0811)
    #14 0x7ff9b02dc252  (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc252) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)
    #15 0x7ff9afe94ac2 in start_thread nptl/./nptl/pthread_create.c:442:8
    #16 0x7ff9aff26a3f  misc/../sysdeps/unix/sysv/linux/x86_64/clone3.S:81

0x61700009e6b8 is located 56 bytes inside of 704-byte region [0x61700009e680,0x61700009e940)
freed by thread T1 here:
    #0 0x55697b23a98d in operator delete(void*) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0xe998d) (BuildId: 4e1ce3fcf62a6d0e51a98b07383906fe5ca789ec)
    #1 0x7ff9b0bcbabc in std::__uniq_ptr_impl<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>, std::default_delete<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer> > >::reset(tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>*) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x3cbabc) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #2 0x7ff9b1b48b8c  (/opt/ros/humble/lib/librclcpp_lifecycle.so+0x28b8c) (BuildId: ccb1b39efb3eaee63de490386a8850a5752f307d)

previously allocated by thread T0 here:
    #0 0x55697b23a12d in operator new(unsigned long) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0xe912d) (BuildId: 4e1ce3fcf62a6d0e51a98b07383906fe5ca789ec)
    #1 0x7ff9b0bc407f in std::_MakeUniq<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer> >::__single_object std::make_unique<tf2_ros::MessageFilter<sensor_msgs::msg::LaserScan_<std::allocator<void> >, tf2_ros::Buffer>, message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> >, rclcpp_lifecycle::LifecycleNode>&, tf2_ros::Buffer&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, int, std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>, std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface>, std::chrono::duration<long, std::ratio<1l, 1000000000l> >&>(message_filters::Subscriber<sensor_msgs::msg::LaserScan_<std::allocator<void> >, rclcpp_lifecycle::LifecycleNode>&, tf2_ros::Buffer&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >&, int&&, std::shared_ptr<rclcpp::node_interfaces::NodeLoggingInterface>&&, std::shared_ptr<rclcpp::node_interfaces::NodeClockInterface>&&, std::chrono::duration<long, std::ratio<1l, 1000000000l> >&) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x3c407f) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)

Thread T11 created by T0 here:
    #0 0x55697b1e87dc in __interceptor_pthread_create (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0x977dc) (BuildId: 4e1ce3fcf62a6d0e51a98b07383906fe5ca789ec)
    #1 0x7ff9b02dc328 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)
    #2 0x7ff9b0cd3359 in std::_Sp_counted_ptr_inplace<tf2_ros::TransformListener, std::allocator<tf2_ros::TransformListener>, (__gnu_cxx::_Lock_policy)2>::_Sp_counted_ptr_inplace<tf2_ros::Buffer&>(std::allocator<tf2_ros::TransformListener>, tf2_ros::Buffer&) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x4d3359) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #3 0x7ff9b0b60ad2 in nav2_amcl::AmclNode::initTransforms() (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x360ad2) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #4 0x7ff9b0b59aa9 in nav2_amcl::AmclNode::on_configure(rclcpp_lifecycle::State const&) (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x359aa9) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0)
    #5 0x7ff9b1b48b8c  (/opt/ros/humble/lib/librclcpp_lifecycle.so+0x28b8c) (BuildId: ccb1b39efb3eaee63de490386a8850a5752f307d)

Thread T1 created by T0 here:
    #0 0x55697b1e87dc in __interceptor_pthread_create (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/nav2_amcl/amcl+0x977dc) (BuildId: 4e1ce3fcf62a6d0e51a98b07383906fe5ca789ec)
    #1 0x7ff9b02dc328 in std::thread::_M_start_thread(std::unique_ptr<std::thread::_State, std::default_delete<std::thread::_State> >, void (*)()) (/lib/x86_64-linux-gnu/libstdc++.so.6+0xdc328) (BuildId: e37fe1a879783838de78cbc8c80621fa685d58a2)

SUMMARY: AddressSanitizer: heap-use-after-free (/home/****/ROS_ws/nav2/install/nav2_amcl/lib/libamcl_core.so+0x4f3538) (BuildId: 427f1d8584980a1646cc60c7a8e1d2739c4c5ca0) in message_filters::Signal1<sensor_msgs::msg::LaserScan_<std::allocator<void> > >::call(message_filters::MessageEvent<sensor_msgs::msg::LaserScan_<std::allocator<void> > const> const&)
Shadow bytes around the buggy address:
  0x0c2e8000bc80: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bc90: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bca0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bcb0: fd fd fd fd fd fa fa fa fa fa fa fa fa fa fa fa
  0x0c2e8000bcc0: fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa fa
=>0x0c2e8000bcd0: fd fd fd fd fd fd fd[fd]fd fd fd fd fd fd fd fd
  0x0c2e8000bce0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bcf0: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bd00: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bd10: fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd fd
  0x0c2e8000bd20: fd fd fd fd fd fd fd fd fa fa fa fa fa fa fa fa
Shadow byte legend (one shadow byte represents 8 application bytes):
  Addressable:           00
  Partially addressable: 01 02 03 04 05 06 07 
  Heap left redzone:       fa
  Freed heap region:       fd
  Stack left redzone:      f1
  Stack mid redzone:       f2
  Stack right redzone:     f3
  Stack after return:      f5
  Stack use after scope:   f8
  Global redzone:          f9
  Global init order:       f6
  Poisoned by user:        f7
  Container overflow:      fc
  Array cookie:            ac
  Intra object redzone:    bb
  ASan internal:           fe
  Left alloca redzone:     ca
  Right alloca redzone:    cb
==1627006==ABORTING

Additional information

It seems like a shutdown-issue, and caused by the inappropriate Ptr release order

focus on code lines in AmclNode::on_cleanup() : amcl_node.cpp#L331-L344, the laser_scan_filter_.reset() is done before tf_listener_.reset().

SO, If there happens to be a message comming within the interval between two pointers being reset, tf_listener_ would be in callback-work. And during the work, it would access the laser_scan_filter_ to filter compliance messages, while the laser_scan_filter_ has been freed. Finally, it would trigger an error similar to the UAF we reported this time.

a suggestion that may be helpful:

shall we place tf_listener_.reset() before laser_scan_filter_.reset() ?

The text was updated successfully, but these errors were encountered:

SteveMacenski · 2024-01-25T15:11:18Z

Sure! Open the PR please

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

…ed in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (open-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (open-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (open-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (open-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (open-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (open-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (open-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (open-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (open-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (open-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (open-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (open-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> New MPPI Cost Critic (Contrib: Brice Renaudeau) (open-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (open-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (open-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (open-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (open-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (open-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (open-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> [behavior_tree] support both ports and blackboard (open-navigation#4060) * check result of blackboard->get and use halTree Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove unused header Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * BT: add port inputs when missing and use getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add description Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * change return value of getInputPortOrBlackboard Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * updated tree XML Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Use ament_export_targets for all targets (open-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> adding new Friends of Nav2 Sponsors! Update README.md (open-navigation#4114) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> CI green P3 (open-navigation#4117) Refactors `possible_inscribed_cost` to `possible_circumscribed_cost` in collision checker for smac_planner and mppi_controller (open-navigation#4113). (open-navigation#4118) * Utilizes circumscribed radius to assess potential collision points on the robot. * Ensures safety when the robot's center is outside the circumscribed radius, eliminating additional checks and optimizing CPU usage. Signed-off-by: Alan Xue <alan.xuefei@googlemail.com> Added cast to float to getClosestAngularBin return for behavior consistency (open-navigation#4123) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> Adding context to MPPI readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> [behavior_tree] Add generate_nav2_tree_nodes_xml (open-navigation#4073) * Add generate_nav2_tree_nodes_xml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * behavior_tree: removed list of plugins from yaml Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * check result of get_parameter(plugin_lib_names) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix previous commit (uncrustify) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * revert change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Fix compilation with clang (open-navigation#4131) Signed-off-by: Ramon Wijnands <ramon.wijnands@nobleo.nl> code improvement based on feedback. Name changes and minor code improvement Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix goal handle was not freed correctly (open-navigation#4137) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Adding Nvidia as Gold Sponsor! (open-navigation#4144) Prevent a possible segmentation fault (open-navigation#4141) (open-navigation#4147) * Prevent a possible segmentation fault Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> * Cleanup Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> --------- Signed-off-by: Joni Pöllänen <joni.pollanen@karelics.fi> Updating stereolabs logo (open-navigation#4158) Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> behavior_tree: migration to BT.CPP 4.5.x (open-navigation#4128) * behavior_tree: migration to BT.CPP 4.5.x Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix formatting Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix lint warning Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove setStatus(BT::NodeStatus::IDLE) Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * handle SKIPPED state Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix unit tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add emitWakeUpSignal() Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fit test Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * remove SequenceStar Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * keep resetStatus() in action_node Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * add suggestions from peer reviewer Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * fix compilation Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> bug fix (open-navigation#4160) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> Stop planner if the goal is cancelled (open-navigation#4148) * Add support for stopping planners when the action is cancelled Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Smac planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Theta* planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Support cancel in Navfn planner Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Update nav2_system_tests to use cancel checker Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add terminal_checking_interval parameter Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Handle timeout and cancel check on the same branch and default to 5000 iterations Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> * Add system tests for cancel Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> --------- Signed-off-by: Kemal Bektas <kemal.bektas@node-robotics.com> Co-authored-by: Kemal Bektas <kemal.bektas@node-robotics.com> Improvement on the wat the distance heuristic is calculated Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com> Fix BT.CPP import (open-navigation#4165) Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Update default recommendation from Obstacles to Cost critic in MPPI (open-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> nav2_controller: add loop rate log (open-navigation#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> improvement based on feedback Signed-off-by: stevedan <stevedan.o.omodolor@gmail.com>

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

* Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com>

* nav2_controller: add loop rate log (#4171) * update smac_planner README Signed-off-by: ARK3r <kermani.areza@gmail.com> * added current controller loop rate logging Signed-off-by: ARK3r <kermani.areza@gmail.com> * linting Signed-off-by: ARK3r <kermani.areza@gmail.com> * uncrustify lint Signed-off-by: ARK3r <kermani.areza@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update nav2_controller/src/controller_server.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * chore(nav2_behavior_tree): log actual wait period in bt_action_node (#4178) Signed-off-by: Felix <felix@fzeltner.de> Co-authored-by: Felix <felix@fzeltner.de> * replace throw-error with error-log to avoid UAF mentioned in #4175 (#4180) * replace throw-error with error-log to avoid UAF Signed-off-by: GoesM <GoesM@buaa.edu.cn> * fix typo Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix missing param declare (#4203) Signed-off-by: nelson <kaichie.lee@gmail.com> * Revert "nav2_controller: add loop rate log (#4171)" (#4210) This reverts commit 4737462. * add polygon_subscribe_transient_local parameter in collision monitor (#4207) Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> * nav2_controller: add loop rate log (#4228) * added current loop rate printout Signed-off-by: ARK3r <kermani.areza@gmail.com> * remove empty line Signed-off-by: ARK3r <kermani.areza@gmail.com> --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> * bump to 1.2.7 for iron sync * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Fix typo (#4087) Fix AttributeError: 'BasicNavigator' object has no attribute '_BasicNavigator__getPathThroughPosesImpl' Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> * Add velocity based polygon (#3708) * add velocity based polygon * fix header, copyright and variable name change * optimise polygon update * optimise duplicated code with setPolygonShape * add warning log for uncovered speed * update feedback * rename polygon velocity to velocity polygon * cleanup * fix typo * add dynamic support for velocity polygon * wrap try catch for getting parameters * update naming and linting * use switch case * Revert "use switch case" This reverts commit 1230ede. * fix proper return for invalid parameters * remove topic parameter for velocity polygon * fix formatting manually * continue if points are not defined * rewrite velocity polygon with polygon base class Signed-off-by: nelson <kaichie.lee@gmail.com> * update review comments and description Signed-off-by: nelson <kaichie.lee@gmail.com> * add VelocityPolygon to detector node Signed-off-by: nelson <kaichie.lee@gmail.com> * review update Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * Update nav2_collision_monitor/src/velocity_polygon.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> * add velocity polygon tests Signed-off-by: nelson <kaichie.lee@gmail.com> * fix cpplint Signed-off-by: nelson <kaichie.lee@gmail.com> * add in-line comment Signed-off-by: nelson <kaichie.lee@gmail.com> * fix push back Signed-off-by: nelson <kaichie.lee@gmail.com> * minor change and update README Signed-off-by: nelson <kaichie.lee@gmail.com> * update README Signed-off-by: nelson <kaichie.lee@gmail.com> --------- Signed-off-by: nelson <kaichie.lee@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * avoid implicit type case (#4098) * adding progress checker selector BT node (#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * fix merge conflict * Revert "Add velocity based polygon (#3708)" This reverts commit a3ed745. * custom backported version of cost critic inscribed cost --------- Signed-off-by: ARK3r <kermani.areza@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Felix <felix@fzeltner.de> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: nelson <kaichie.lee@gmail.com> Signed-off-by: asarazin <anaelle.sarazin@robocc.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: Reza Kermani <kermani.areza@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: bi0ha2ard <bi0ha2ard@users.noreply.github.com> Co-authored-by: Felix <felix@fzeltner.de> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: nelson <kaichie.lee@gmail.com> Co-authored-by: anaelle-sw <63144493+anaelle-sw@users.noreply.github.com> Co-authored-by: asarazin <anaelle.sarazin@robocc.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: nfriedrich00 <69539098+nfriedrich00@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com>

* change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in #4078 (#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * adding progress checker selector BT node (#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * Use ament_export_targets for all targets (#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

* change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in open-navigation#4078 (open-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * adding progress checker selector BT node (open-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (open-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (open-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (open-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (open-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (open-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (open-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (open-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * Use ament_export_targets for all targets (open-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (open-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (open-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (open-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (open-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (open-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (open-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (open-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

* Fix open-navigation#4001 (open-navigation#4028) * Humble Sync 9: Jan 23, 2024 (open-navigation#4064) * Update README.md: fix typo (open-navigation#3842) * Update package.xml * fix typo (open-navigation#3850) * adjust link to point to v3.8 of behavior tree docs (open-navigation#3851) BT.CPP_v3 is used, thereby the correct docs should be linked * Fix bug in nav2_behavior_tree/bt_action_node (open-navigation#3849) * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * Fix bug in nav2_behavior_tree/bt_action_node * Fixed the bug in halt function inside nav2_behavior_tree/plugin/action/bt_action_node.hpp * Added new case to nav2_behavior_tree/plugin/action/bt_action_node.hpp for testing the scenario to cancel * Refactored existing cases in nav2_behavior_tree/plugin/action/bt_action_node.hpp Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> * simplication of lattice_generator.py, fixes open-navigation#3858 (open-navigation#3859) * simplification of equation to compute the max_value/outer edge of the lattice based on number of headings. * Fix class doxygen * fix minor typos (open-navigation#3892) Signed-off-by: Anton Kesy <antonkesy@gmail.com> * fix use after free (open-navigation#3910) * fix build mppi (open-navigation#3927) Signed-off-by: kevin <kevin@floatic.io> Co-authored-by: kevin <kevin@floatic.io> * Removing old TODOs * Fixed links for install and build in README (open-navigation#3963) Currently the readme is linking to an invalida page in the docs (404 error). * fix a few outdated comments in smac planners (open-navigation#3978) * Update README.md * bump to 1.1.13 for release --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Signed-off-by: Anton Kesy <antonkesy@gmail.com> Signed-off-by: kevin <kevin@floatic.io> Co-authored-by: thandal <than@timbrel.org> Co-authored-by: Anton Kesy <antonkesy@gmail.com> Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Abiel Fernandez <empoleom@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> * Fixes for compiling with clang on macOS (open-navigation#4051) amcl: declare void parameter for functions with no args amcl: remove unused variables behavior_tree: address clang compilation issues behaviors: add missing override specifier bt_navigator: add missing override specifier collision_monitor: address clang compilation issues constrained_smoother: address clang compilation issues controller: address clang compilation issues costmap_2d: add missing override specifier costmap_2d: address clang compilation issues costmap_2d: fix link issue for order_layer dwb_controller: fix clang compile issue, replace ulong with uint32_t map_server: replace std::experimental::filesystem map_server: remove dependency on stdc++fs waypoint_follower: address clang compilation issues waypoint_follower: remove dependency on stdc++fs waypoint_follower: replace std::experimental::filesystem smoother: address clang compilation issues smoother: remove unused variables system_tests: remove dependency on stdc++fs rotation_shim_controller: update percentage arg in setSpeedLimit to boolean planner: remove unused variables costmap_2d: address clang compilation issues mppi_controller: replace use of auto as function param with templates mppi_controller: address clang compilation issues costmap_2d: resolve clang issue with std::pair non-const copy Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> * critical fix for path tracking MPPI (open-navigation#4099) (open-navigation#4101) (cherry picked from commit 8f7b6ce) Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * backport graceful controller to humble (open-navigation#4119) * Added cast to float to getClosestAngularBin return for behavior consistency (open-navigation#4123) (open-navigation#4125) * Added cast to float to getClosestAngularBin return for behavior consistency. Signed-off-by: Hunter Song <hsong@macleanengineering.com> * Revised test name Signed-off-by: Hunter Song <hsong@macleanengineering.com> --------- Signed-off-by: Hunter Song <hsong@macleanengineering.com> (cherry picked from commit c59e0f3) Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> * Fix goal handle was not freed correctly (open-navigation#4137) (open-navigation#4143) * Fix goal handle was not freed correctly * Update nav2_util/include/nav2_util/simple_action_server.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Ziohang <hang.zhou@agvsz.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> (cherry picked from commit 2e491b1) Co-authored-by: iceuw <m201770623@hust.edu.cn> * bug fix (open-navigation#4160) (open-navigation#4162) Signed-off-by: Tianchu <tianchu.zhang0706@gmail.com> (cherry picked from commit 623434b) Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> * Add uint suffix (open-navigation#4213) (open-navigation#4216) (cherry picked from commit 46ff0ec) Co-authored-by: Michael Wrock <michael.wrock@picknik.ai> * Fix merge conflict in nav2_mppi_controller README (humble) (open-navigation#4239) Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> * Humble sync 10: April 4, 2024 (open-navigation#4241) * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> * free `map_sub_` before `map_free(map_)` to avoid UAF&&NullPtr bug mentioned in open-navigation#4078 (open-navigation#4079) * free `map_sub_` before `map_free(map_)` Signed-off-by: GoesM <GoesM@buaa.edu.cn> * reformat Signed-off-by: GoesM <GoesM@buaa.edu.cn> --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * adding progress checker selector BT node (open-navigation#4109) * New MPPI Cost Critic (Contrib: Brice Renaudeau) (open-navigation#4090) * Share code Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update inflation_cost_critic.hpp - copyright - ifndef Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix lint cpp - extra space Signed-off-by: Brice <brice.renaudeau@gmail.com> * Fix Smac Planner confined collision checker (open-navigation#4055) * Update collision_checker.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix tests Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update test_a_star.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Prevent analytic expansions from shortcutting Smac Planner feasible paths (open-navigation#3962) * a potential solution to smac shortcutting * costmap reoslution * some fixes * completed prototype * some fixes for collision detection and performance * completing shortcutting fix * updating tests * adding readme --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * change pointer free order in amcl to avoid use-after-free bug mentioned in open-navigation#4068 (open-navigation#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [Smac Planner] Massive Improvement of Behavior for SE2 Footprint Checking (ie non-circular robots) In Confined Settings (open-navigation#4067) * prototype to test SE2 footprint H improvements * some fixes * fixed * invert logic * Working final prototype to be tested * complete unit test conversions * Update inflation_layer.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Adding new Smac paper to readme Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Update README.md Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * [behavior_tree] don't repeat yourself in "blackboard->set" (open-navigation#4074) * don't repeat yourself: templates in tests Signed-off-by: Davide Faconti <davide.faconti@gmail.com> * misse change Signed-off-by: Davide Faconti <davide.faconti@gmail.com> --------- Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Allow path end pose deviation revive (open-navigation#4065) * Support stitching paths in compute path to poses * Update nav2_planner/src/planner_server.cpp Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> * Rename parameter to allow_path_through_poses_goal_deviation * Fix description * restore nav2_params * missing whitespace * lint fix * removed parameter Signed-off-by: gg <josho.wallace@gmail.com> * Update planner_server.hpp * Update planner_server.cpp --------- Signed-off-by: gg <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Updated code to use getInflationLayer() method (open-navigation#4076) * updated code to use getInflationLayer method Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> * Fix linting Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> --------- Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * 1594 twist stamped publisher (open-navigation#4077) * Add TwistStamped to controller_server via TwistPublisher util * Add a new util class for publishing either Twist or TwistStamped * Add a new parameter for selecting to stamp the twist data * Consume TwistPublisher in nav2_controller Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix small issues * Unused variable * Incorrect doxygen Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove stored node and assert Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add tests for node * Facing timeout even though it does the same thing as velocity smoother test Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add missing spin call to solve timeout Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix copyright (me instead of intel) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add full test coverage with subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused rclcpp fixture * Can't use it due to needing to join the pub thread after rclcpp shuts down Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in nav2_behaviors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistStamped in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistStamped readme updates to velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add TwistSubscriber implementation Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix syntax errors Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in test_velocity_smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in assisted_teleop Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in collision monitor node Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use TwistSubscriber in velocity smoother Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused code Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * add timestamp and frame_id to TwistStamped message * Add missing utility include Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document TwistPublisher and TwistSubscriber usage Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Use pass-by-reference * Instead of std::move(std::unique_ptr<TwistStamped>) Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Finish twist subscriber tests Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Add other constructor and docs Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Fix linter issues Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Manually fix paren alignment Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove GSoC reference Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Document twist bool param in README Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Handle twistPublisher in collision monitor * Implement behavior in the stamped callback * Unstamped callback calls the stamped callback * Switch to unique pointer for publisher Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Convert to using TwistStamped interally * Use incoming twistStamped timestamp if available * Convert all internal representations to use TwistStamped Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove nav2_util usage instructions Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Remove unused Twist only subscriber Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * More linter fixes Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Prefer working with unique_ptr for cmd_vel * This makes it easier to switch to std::move instead of dereference on publish Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Completing twist stamped migration * shared to unique ptr Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * twist add stamps and properly propogated * nav2_util: fix for compiling with clang - Resolve error: moving a temporary object prevents copy elision [-Werror,-Wpessimizing-move] Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> --------- Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * Change costmap_queue to shared library (open-navigation#4072) Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> * fix include of hpp Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> * inflation cost optmiizations and cleanu * rename, add defaults, and docs * smoke test addition * lintg * normalize weight * update readme * increment cache * Update cost_critic.hpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Update cost_critic.cpp Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> --------- Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Kino <cybao292261@163.com> * Use ament_export_targets for all targets (open-navigation#4112) * Matches new internal ALIAS targets * Use ALIAS targets for all internal linkage * Remove unnecessary calls to ament_target_dependencies in test code * Export includes in proper folders for overlays in colcon Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> * Update default recommendation from Obstacles to Cost critic in MPPI (open-navigation#4170) Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * completely shutdown inital_pose_sub_ (open-navigation#4176) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn> * fix typos in description messages (open-navigation#4188) Signed-off-by: Antonio Park <kaypaklabs@gmail.com> * AMCL: Set an initial guess by service call (open-navigation#4182) * Added initial guess service. Signed-off-by: Alexander Mock Signed-off-by: Alexander Mock <amock@uos.de> * - Removed added empty line - Renamed initialGuessCallback to initialPoseReceivedSrv - Added new line to SetInitialPose service definition - Removed mutex from initialPoseReceived - Cleanup service server Signed-off-by: Alexander Mock <amock@uos.de> * added whitespace Signed-off-by: Alexander Mock <amock@uos.de> * renamed initial pose service in callback bind Signed-off-by: Alexander Mock <amock@uos.de> --------- Signed-off-by: Alexander Mock <amock@uos.de> * Move lines for pre-computation to outside a loop (open-navigation#4191) Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> * Fix typo (open-navigation#4196) * Fix BT.CPP import Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update README.md --------- Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> * Update footprint iif changed (open-navigation#4193) Signed-off-by: Brice <brice.renaudeau@gmail.com> * bump humble to 1.1.14 for sync * fix merge conflict * custom version of cost critic's cost method --------- Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com> * Fix graceful humble release version Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> * Fix graceful_controller exceptions --------- Signed-off-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Signed-off-by: Anton Kesy <antonkesy@gmail.com> Signed-off-by: kevin <kevin@floatic.io> Signed-off-by: Rhys Mainwaring <rhys.mainwaring@me.com> Signed-off-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> Signed-off-by: GoesM <GoesM@buaa.edu.cn> Signed-off-by: gg <josho.wallace@gmail.com> Signed-off-by: cybaol <cybao292261@163.com> Signed-off-by: Brice <brice.renaudeau@gmail.com> Signed-off-by: Steve Macenski <stevenmacenski@gmail.com> Signed-off-by: Davide Faconti <davide.faconti@gmail.com> Signed-off-by: Jose Faria <20467009+jncfa@users.noreply.github.com> Signed-off-by: Ryan Friedman <ryanfriedman5410+github@gmail.com> Signed-off-by: Brice Renaudeau <brice.renaudeau@gmail.com> Signed-off-by: Antonio Park <kaypaklabs@gmail.com> Signed-off-by: Alexander Mock <amock@uos.de> Signed-off-by: Kyungsik Park <dkalxkwl@gmail.com> Signed-off-by: Tony Najjar <tony.najjar.1997@gmail.com> Co-authored-by: Steve Macenski <stevenmacenski@gmail.com> Co-authored-by: thandal <than@timbrel.org> Co-authored-by: Anton Kesy <antonkesy@gmail.com> Co-authored-by: CihatAltiparmak <cihataltiparmak1@gmail.com> Co-authored-by: Anil Kumar Chavali <44644339+akchobby@users.noreply.github.com> Co-authored-by: 정찬희 <60467877+ladianchad@users.noreply.github.com> Co-authored-by: kevin <kevin@floatic.io> Co-authored-by: Abiel Fernandez <empoleom@gmail.com> Co-authored-by: Michael Ferguson <mfergs7@gmail.com> Co-authored-by: Rhys Mainwaring <rhys.mainwaring@me.com> Co-authored-by: mergify[bot] <37929162+mergify[bot]@users.noreply.github.com> Co-authored-by: hsong-MLE <158601314+hsong-MLE@users.noreply.github.com> Co-authored-by: iceuw <m201770623@hust.edu.cn> Co-authored-by: Chortine <107395103+Chortine@users.noreply.github.com> Co-authored-by: Michael Wrock <michael.wrock@picknik.ai> Co-authored-by: Stephanie Eng <stephanie-eng@users.noreply.github.com> Co-authored-by: GoesM <130988564+GoesM@users.noreply.github.com> Co-authored-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: Joshua Wallace <josho.wallace@gmail.com> Co-authored-by: pepisg <pedro.gonzalez@eia.edu.co> Co-authored-by: Pedro Alejandro González <71234974+pepisg@users.noreply.github.com> Co-authored-by: Kino <cybao292261@163.com> Co-authored-by: BriceRenaudeau <48433002+BriceRenaudeau@users.noreply.github.com> Co-authored-by: Davide Faconti <davide.faconti@gmail.com> Co-authored-by: jncfa <20467009+jncfa@users.noreply.github.com> Co-authored-by: Ryan <ryanfriedman5410+github@gmail.com> Co-authored-by: pedro-fuoco <pedrofuoco6@gmail.com> Co-authored-by: Antonio Park <kaypaklabs@gmail.com> Co-authored-by: Alexander Mock <amock@uos.de> Co-authored-by: Tony Najjar <tony.najjar.1997@gmail.com>

GoesM mentioned this issue Jan 25, 2024

change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 #4069

Closed

7 tasks

SteveMacenski closed this as completed Jan 25, 2024

GoesM mentioned this issue Jan 25, 2024

change pointer free order in amcl to avoid use-after-free bug mentioned in #4068 #4070

Merged

7 tasks

SteveMacenski pushed a commit that referenced this issue Jan 25, 2024

change pointer free order in amcl to avoid use-after-free bug mention…

1348e52

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

SteveMacenski pushed a commit that referenced this issue Apr 4, 2024

change pointer free order in amcl to avoid use-after-free bug mention…

dc952dc

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

SteveMacenski pushed a commit that referenced this issue Apr 4, 2024

change pointer free order in amcl to avoid use-after-free bug mention…

1dea56b

…ed in #4068 (#4070) Signed-off-by: GoesM <GoesM@buaa.edu.cn> Co-authored-by: GoesM <GoesM@buaa.edu.cn>

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Inappropriate pointer free order in amcl may trigger use-after-free bug #4068

Inappropriate pointer free order in amcl may trigger use-after-free bug #4068

GoesM commented Jan 25, 2024

SteveMacenski commented Jan 25, 2024

Inappropriate pointer free order in amcl may trigger use-after-free bug #4068

Inappropriate pointer free order in amcl may trigger use-after-free bug #4068

Comments

GoesM commented Jan 25, 2024

Bug report

Steps to reproduce issue

Expected behavior

Actual behavior

Additional information

SteveMacenski commented Jan 25, 2024