Galactic sync 6

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

nav2_bt_navigator/include/nav2_bt_navigator/navigators/navigate_to_pose.hpp

@@ -50,9 +50,11 @@ class NavigateToPoseNavigator
   /**
    * @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 A cleanup state transition to remove memory allocated
@@ -122,7 +124,7 @@ class NavigateToPoseNavigator
   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

nav2_bt_navigator/src/bt_navigator.cpp

@@ -107,21 +107,21 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/)
   feedback_utils.robot_frame = robot_frame_;
   feedback_utils.transform_tolerance = transform_tolerance_;
 
+  // Odometry smoother object for getting current speed
+  odom_smoother_ = std::make_shared<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_)
+
   if (!pose_navigator_->on_configure(
-      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_))
+      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
   {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
   if (!poses_navigator_->on_configure(
-      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_))
+      shared_from_this(), plugin_lib_names, feedback_utils, &plugin_muxer_, odom_smoother_))
   {
     return nav2_util::CallbackReturn::FAILURE;
   }
 
-  // Odometry smoother object for getting current speed
-  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(shared_from_this(), 0.3, odom_topic_);
-
   return nav2_util::CallbackReturn::SUCCESS;
 }
 

nav2_bt_navigator/src/navigators/navigate_through_poses.cpp

@@ -24,19 +24,26 @@ namespace nav2_bt_navigator
 
 bool
 NavigateThroughPosesNavigator::configure(
-  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
 {
   start_time_ = rclcpp::Time(0);
   auto node = parent_node.lock();
-  node->declare_parameter("goals_blackboard_id", std::string("goals"));
+
+  if (!node->has_parameter("goals_blackboard_id")) {
+    node->declare_parameter("goals_blackboard_id", std::string("goals"));
+  }
+
   goals_blackboard_id_ = node->get_parameter("goals_blackboard_id").as_string();
+
   if (!node->has_parameter("path_blackboard_id")) {
     node->declare_parameter("path_blackboard_id", std::string("path"));
   }
+
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
   // Odometry smoother object for getting current speed
-  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, 0.3);
+  odom_smoother_ = odom_smoother;
 
   return true;
 }
@@ -47,12 +54,16 @@ NavigateThroughPosesNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  std::string pkg_share_dir =
-    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  node->declare_parameter<std::string>(
-    "default_nav_through_poses_bt_xml",
-    pkg_share_dir +
-    "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
+
+  if (!node->has_parameter("default_nav_through_poses_bt_xml")) {
+    std::string pkg_share_dir =
+      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+    node->declare_parameter<std::string>(
+      "default_nav_through_poses_bt_xml",
+      pkg_share_dir +
+      "/behavior_trees/navigate_through_poses_w_replanning_and_recovery.xml");
+  }
+
   node->get_parameter("default_nav_through_poses_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;

nav2_bt_navigator/src/navigators/navigate_to_pose.cpp

@@ -24,19 +24,26 @@ namespace nav2_bt_navigator
 
 bool
 NavigateToPoseNavigator::configure(
-  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node)
+  rclcpp_lifecycle::LifecycleNode::WeakPtr parent_node,
+  std::shared_ptr<nav2_util::OdomSmoother> odom_smoother)
 {
   start_time_ = rclcpp::Time(0);
   auto node = parent_node.lock();
-  node->declare_parameter("goal_blackboard_id", std::string("goal"));
+
+  if (!node->has_parameter("goal_blackboard_id")) {
+    node->declare_parameter("goal_blackboard_id", std::string("goal"));
+  }
+
   goal_blackboard_id_ = node->get_parameter("goal_blackboard_id").as_string();
+
   if (!node->has_parameter("path_blackboard_id")) {
     node->declare_parameter("path_blackboard_id", std::string("path"));
   }
+
   path_blackboard_id_ = node->get_parameter("path_blackboard_id").as_string();
 
   // Odometry smoother object for getting current speed
-  odom_smoother_ = std::make_unique<nav2_util::OdomSmoother>(node, 0.3);
+  odom_smoother_ = odom_smoother;
 
   self_client_ = rclcpp_action::create_client<ActionT>(node, getName());
 
@@ -53,12 +60,16 @@ NavigateToPoseNavigator::getDefaultBTFilepath(
 {
   std::string default_bt_xml_filename;
   auto node = parent_node.lock();
-  std::string pkg_share_dir =
-    ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
-  node->declare_parameter<std::string>(
-    "default_nav_to_pose_bt_xml",
-    pkg_share_dir +
-    "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
+
+  if (!node->has_parameter("default_nav_to_pose_bt_xml")) {
+    std::string pkg_share_dir =
+      ament_index_cpp::get_package_share_directory("nav2_bt_navigator");
+    node->declare_parameter<std::string>(
+      "default_nav_to_pose_bt_xml",
+      pkg_share_dir +
+      "/behavior_trees/navigate_to_pose_w_replanning_and_recovery.xml");
+  }
+
   node->get_parameter("default_nav_to_pose_bt_xml", default_bt_xml_filename);
 
   return default_bt_xml_filename;

nav2_controller/include/nav2_controller/nav2_controller.hpp

@@ -243,7 +243,7 @@ class ControllerServer : public nav2_util::LifecycleNode
   double failure_tolerance_;
 
   // Whether we've published the single controller warning yet
-  geometry_msgs::msg::Pose end_pose_;
+  geometry_msgs::msg::PoseStamped end_pose_;
 
   // Last time the controller generated a valid command
   rclcpp::Time last_valid_cmd_time_;

nav2_controller/src/nav2_controller.cpp

@@ -417,18 +417,14 @@ void ControllerServer::setPlannerPath(const nav_msgs::msg::Path & path)
   }
   controllers_[current_controller_]->setPlan(path);
 
-  auto end_pose = path.poses.back();
-  end_pose.header.frame_id = path.header.frame_id;
-  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
-  nav_2d_utils::transformPose(
-    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
-    end_pose, end_pose, tolerance);
+  end_pose_ = path.poses.back();
+  end_pose_.header.frame_id = path.header.frame_id;
   goal_checkers_[current_goal_checker_]->reset();
 
   RCLCPP_DEBUG(
     get_logger(), "Path end point is (%.2f, %.2f)",
-    end_pose.pose.position.x, end_pose.pose.position.y);
-  end_pose_ = end_pose.pose;
+    end_pose_.pose.position.x, end_pose_.pose.position.y);
+
   current_path_ = path;
 }
 
@@ -555,7 +551,16 @@ bool ControllerServer::isGoalReached()
 
   nav_2d_msgs::msg::Twist2D twist = getThresholdedTwist(odom_sub_->getTwist());
   geometry_msgs::msg::Twist velocity = nav_2d_utils::twist2Dto3D(twist);
-  return goal_checkers_[current_goal_checker_]->isGoalReached(pose.pose, end_pose_, velocity);
+
+  geometry_msgs::msg::PoseStamped transformed_end_pose;
+  rclcpp::Duration tolerance(rclcpp::Duration::from_seconds(costmap_ros_->getTransformTolerance()));
+  nav_2d_utils::transformPose(
+    costmap_ros_->getTfBuffer(), costmap_ros_->getGlobalFrameID(),
+    end_pose_, transformed_end_pose, tolerance);
+
+  return goal_checkers_[current_goal_checker_]->isGoalReached(
+    pose.pose, transformed_end_pose.pose,
+    velocity);
 }
 
 bool ControllerServer::getRobotPose(geometry_msgs::msg::PoseStamped & pose)

nav2_core/include/nav2_core/recovery.hpp

@@ -65,15 +65,6 @@ class Recovery
    * @brief Method to deactive recovery and any threads involved in execution.
    */
   virtual void deactivate() = 0;
-
-  /**
-   * @brief Method Execute recovery behavior
-   * @param  name The name of this planner
-   * @return true if successful, false is failed to execute fully
-   */
-  // TODO(stevemacenski) evaluate design for recoveries to not host
-  // their own servers and utilize a recovery server exposed action.
-  // virtual bool executeRecovery() = 0;
 };
 
 }  // namespace nav2_core