Galactic sync 6 (#2940)

* fix invert logic (#2772)

* Bugfix tf lookup of goal pose in nav2_controller (#2780)

Signed-off-by: Erik Örjehag <erik@orjehag.se>

* Feature to call controller action server to follow path (#2789)

* Added call to controller action server with a path to follow
++ Added a new function  in robot_navigator,py
++ Added a launch script to test  function
++ Updated setup.py to include demo_follow_path.py

* Code refactoring

* Code refactoring

* Code refactoring for consistency

* Updated README.md

* Resolved executable conflict in setup.py for example_follow_path.py

* Code refactoring with  ament_flake8

* Add recoveries to simple commander (#2792)

* fix Typo

* add recoveries

* add docs

* added demo using backup and spin

* rename isNavigationComplete to isTaskComplete

* rename cancelNav to cancelTask

* add prints

* fix premature exit

* rename NavResult to TaskResult

* fix readme order

* fix import order

* renaming

* renaming

* added planner_id (#2806)

* Fixing the issue #2781: raytraceLine with same start and end point (#2784)

* Fixing the issue #2781: raytraceLine with same start and end point no longer causes segmentation fault

* Some whitespace modifications to make the code pass release_test

* Add testcase for raytraceLine the same point

Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>

* Add optional node names to wait (#2811)

* add option to specify navigator and localizer to wait for

* add docs for waituntilNav2Active

* wait for pose only for amcl

* revert order

* remove resizing on update message callback (#2818)

* restrict test_msgs to test_dependency (#2827)

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

* remove unused odometry smoother in bt navigator (#2829)

* remove unused odometry smoother in bt navigator

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

* reorganize bt navigator to shared odom_smoother object with servers

Signed-off-by: Alberto Soragna <alberto.soragna@gmail.com>

* Add all action options (#2834)

* added options

* update docs

* Adding theta* to the main packages list

* Fix: bt_navigator crashes on lc transitions (#2848)

* fix empty address access on halt all actions

* fix unsafe declaration of parameters

* restore odom smoother

* fix styling issues

* add missing semicolumn

* fix-collision checker must capture lethal before unknow (#2854)

* Removing old unused function and comment (#2863)

* Better Costmap Error Message (#2884)

* Better Costmap Error Message

* PR Feedback

* add getRobotRadius() in costmap_2d_ros (#2896)

* Add clock time to costmaps (#2899)

* update goal checker plugin to plugins (#2909)

* Allow usage of std::string in searchAndGetParam() (#2918)

* Allow usage of std::string in searchAndGetParam()

Removed also old TODO related to legacy ROS API

* Fix

* Clean up action clients in nav2_simple_commander (#2924)

* Clean up action clients in nav2_simple_commander

* Add camelCase version

* Add docs

* Added mutex to prevent SEGFAULT on map change in AMCL (#2933)

* Added mutex to prevent SEGFAULT on map change

* Removing redundant mutex

Co-authored-by: Antonio Marangi <antonio.marangi@alba-robot.com>

Co-authored-by: G.Doisy <doisyg@users.noreply.github.com>
Co-authored-by: Erik Örjehag <erik@orjehag.se>
Co-authored-by: Ekansh Sharma <ekanshh.sharma@gmail.com>
Co-authored-by: Tony Najjar <tony.najjar@logivations.com>
Co-authored-by: Joshua Wallace <47819219+jwallace42@users.noreply.github.com>
Co-authored-by: janx8r <95029851+janx8r@users.noreply.github.com>
Co-authored-by: Alexey Merzlyakov <60094858+AlexeyMerzlyakov@users.noreply.github.com>
Co-authored-by: Andrii Maistruk <71632363+andriimaistruk@users.noreply.github.com>
Co-authored-by: Alberto Soragna <alberto.soragna@gmail.com>
Co-authored-by: Erwin Lejeune <erwin.lejeune@cm-robotics.com>
Co-authored-by: Hossein Sheikhi Darani <64957461+HosseinSheikhi@users.noreply.github.com>
Co-authored-by: David V. Lu!! <davidvlu@gmail.com>
Co-authored-by: Carlos Andrés Álvarez Restrepo <candres.alv@gmail.com>
Co-authored-by: Patrick Roncagliolo <ronca.pat@gmail.com>
Co-authored-by: M. Mostafa Farzan <m2_farzan@yahoo.com>
Co-authored-by: mrmara <48493979+mrmara@users.noreply.github.com>
Co-authored-by: Antonio Marangi <antonio.marangi@alba-robot.com>

Author: 18 people

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -130,7 +130,7 @@ class AmclNode : public nav2_util::LifecycleNode
   bool first_map_only_{true};
   std::atomic<bool> first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;
-  std::recursive_mutex configuration_mutex_;
+  std::recursive_mutex mutex_;
   rclcpp::Subscription<nav_msgs::msg::OccupancyGrid>::ConstSharedPtr map_sub_;
 #if NEW_UNIFORM_SAMPLING
   static std::vector<std::pair<int, int>> free_space_indices;
@@ -238,7 +238,6 @@ class AmclNode : public nav2_util::LifecycleNode
    */
   static pf_vector_t uniformPoseGenerator(void * arg);
   pf_t * pf_{nullptr};
-  std::mutex pf_mutex_;
   bool pf_init_;
   pf_vector_t pf_odom_pose_;
   int resample_count_{0};

nav2_amcl/src/amcl_node.cpp

@@ -504,7 +504,7 @@ AmclNode::globalLocalizationCallback(
   const std::shared_ptr<std_srvs::srv::Empty::Request>/*req*/,
   std::shared_ptr<std_srvs::srv::Empty::Response>/*res*/)
 {
-  std::lock_guard<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   RCLCPP_INFO(get_logger(), "Initializing with uniform distribution");
 
@@ -530,7 +530,7 @@ AmclNode::nomotionUpdateCallback(
 void
 AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
 {
-  std::lock_guard<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   RCLCPP_INFO(get_logger(), "initialPoseReceived");
 
@@ -565,6 +565,7 @@ AmclNode::initialPoseReceived(geometry_msgs::msg::PoseWithCovarianceStamped::Sha
 void
 AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
 {
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
   // In case the client sent us a pose estimate in the past, integrate the
   // intervening odometric change.
   geometry_msgs::msg::TransformStamped tx_odom;
@@ -629,7 +630,7 @@ AmclNode::handleInitialPose(geometry_msgs::msg::PoseWithCovarianceStamped & msg)
 void
 AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan)
 {
-  std::lock_guard<std::mutex> lock(pf_mutex_);
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   // Since the sensor data is continually being published by the simulator or robot,
   // we don't want our callbacks to fire until we're in the active state
@@ -1166,7 +1167,7 @@ AmclNode::mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg)
 void
 AmclNode::handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg)
 {
-  std::lock_guard<std::recursive_mutex> cfl(configuration_mutex_);
+  std::lock_guard<std::recursive_mutex> cfl(mutex_);
 
   RCLCPP_INFO(
     get_logger(), "Received a %d X %d map @ %.3f m/pix",

nav2_behavior_tree/package.xml

@@ -51,6 +51,7 @@
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_cmake_gtest</test_depend>
+  <test_depend>test_msgs</test_depend>
 
   <export>
     <build_type>ament_cmake</build_type>

nav2_behavior_tree/src/behavior_tree_engine.cpp

@@ -108,6 +108,10 @@ BehaviorTreeEngine::resetGrootMonitor()
 void
 BehaviorTreeEngine::haltAllActions(BT::TreeNode * root_node)
 {
+  if (!root_node) {
+    return;
+  }
+  
   // this halt signal should propagate through the entire tree.
   root_node->halt();
 

nav2_behavior_tree/test/plugins/action/CMakeLists.txt

@@ -1,5 +1,7 @@
+find_package(test_msgs REQUIRED)
+
 ament_add_gtest(test_bt_action_node test_bt_action_node.cpp)
-ament_target_dependencies(test_bt_action_node ${dependencies})
+ament_target_dependencies(test_bt_action_node ${dependencies} test_msgs)
 
 ament_add_gtest(test_action_spin_action test_spin_action.cpp)
 target_link_libraries(test_action_spin_action nav2_spin_action_bt_node)

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -108,7 +108,7 @@ controller_server:
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     progress_checker_plugin: "progress_checker"
-    goal_checker_plugin: "goal_checker"
+    goal_checker_plugins: "goal_checker"
     controller_plugins: ["FollowPath"]
 
     # Progress checker parameters

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -108,7 +108,7 @@ controller_server:
     min_y_velocity_threshold: 0.5
     min_theta_velocity_threshold: 0.001
     progress_checker_plugin: "progress_checker"
-    goal_checker_plugin: "goal_checker"
+    goal_checker_plugins: "goal_checker"
     controller_plugins: ["FollowPath"]
 
     # Progress checker parameters

nav2_bt_navigator/include/nav2_bt_navigator/bt_navigator.hpp

@@ -89,7 +89,7 @@ class BtNavigator : public nav2_util::LifecycleNode
   nav2_bt_navigator::NavigatorMuxer plugin_muxer_;
 
   // Odometry smoother object
-  std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
 
   // Metrics for feedback
   std::string robot_frame_;

nav2_bt_navigator/include/nav2_bt_navigator/navigator.hpp

@@ -135,13 +135,15 @@ class Navigator
    * @param feedback_utils Some utilities useful for navigators to have
    * @param plugin_muxer The muxing object to ensure only one navigator
    * can be active at a time
+   * @param odom_smoother Object to get current smoothed robot's speed
    * @return bool If successful
    */
   bool on_configure(
     rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
     const std::vector<std::string> & plugin_lib_names,
     const FeedbackUtils & feedback_utils,
-    nav2_bt_navigator::NavigatorMuxer * plugin_muxer)
+    nav2_bt_navigator::NavigatorMuxer * plugin_muxer,
+    std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
   {
     auto node = parent_node.lock();
     logger_ = node->get_logger();
@@ -173,7 +175,7 @@ class Navigator
     blackboard->set<bool>("initial_pose_received", false);  // NOLINT
     blackboard->set<int>("number_recoveries", 0);  // NOLINT
 
-    return configure(parent_node) && ok;
+    return configure(parent_node, odom_smoother) && ok;
   }
 
   /**
@@ -284,7 +286,12 @@ class Navigator
   /**
    * @param Method to configure resources.
    */
-  virtual bool configure(rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/) {return true;}
+  virtual bool configure(
+    rclcpp_lifecycle::LifecycleNode::WeakPtr /*node*/
+    std::shared_ptr<nav2_util::OdomSmoother> /*odom_smoother*/)
+  {
+    return true;
+  }
 
   /**
    * @brief Method to cleanup resources.

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_through_poses.hpp

@@ -51,9 +51,11 @@ class NavigateThroughPosesNavigator
   /**
    * @brief A configure state transition to configure navigator's state
    * @param node Weakptr to the lifecycle node
+   * @param odom_smoother Object to get current smoothed robot's speed
    */
   bool configure(
-    rclcpp_lifecycle::LifecycleNode::WeakPtr node) override;
+    rclcpp_lifecycle::LifecycleNode::WeakPtr node,
+    std::shared_ptr<nav2_util::OdomSmoother> odom_smoother) override;
 
   /**
    * @brief Get action name for this navigator
@@ -106,7 +108,7 @@ class NavigateThroughPosesNavigator
   std::string path_blackboard_id_;
 
   // Odometry smoother object
-  std::unique_ptr<nav2_util::OdomSmoother> odom_smoother_;
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother_;
 };
 
 }  // namespace nav2_bt_navigator