Parameterize frame IDs

nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp

@@ -33,8 +33,13 @@ class GoalReachedCondition : public BT::ConditionNode
   GoalReachedCondition(
     const std::string & condition_name,
     const BT::NodeConfiguration & conf)
-  : BT::ConditionNode(condition_name, conf), initialized_(false)
+  : BT::ConditionNode(condition_name, conf),
+    initialized_(false),
+    global_frame_("map"),
+    robot_base_frame_("base_link")
   {
+    getInput("global_frame", global_frame_);
+    getInput("robot_base_frame", robot_base_frame_);
   }
 
   GoalReachedCondition() = delete;
@@ -88,7 +93,9 @@ class GoalReachedCondition : public BT::ConditionNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination")
+      BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination"),
+      BT::InputPort<std::string>("global_frame", std::string("map"), "Global frame"),
+      BT::InputPort<std::string>("robot_base_frame", std::string("base_link"), "Robot base frame")
     };
   }
 
@@ -103,6 +110,8 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
   double transform_tolerance_;
 };
 

nav2_bringup/bringup/launch/navigation_launch.py

@@ -114,7 +114,7 @@ def generate_launch_description():
             node_executable='recoveries_server',
             node_name='recoveries_server',
             output='screen',
-            parameters=[{'use_sim_time': use_sim_time}],
+            parameters=[configured_params],
             remappings=remappings),
 
         Node(

nav2_bringup/bringup/params/nav2_multirobot_params_1.yaml

@@ -198,6 +198,22 @@ planner_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True
 
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    plugin_names: ["spin", "back_up", "wait"]
+    plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"]
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True

nav2_bringup/bringup/params/nav2_multirobot_params_2.yaml

@@ -198,6 +198,22 @@ planner_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True
 
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    plugin_names: ["spin", "back_up", "wait"]
+    plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"]
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True

nav2_bringup/bringup/params/nav2_params.yaml

@@ -135,9 +135,9 @@ local_costmap:
     ros__parameters:
       update_frequency: 5.0
       publish_frequency: 2.0
+      global_frame: odom
       robot_base_frame: base_link
       use_sim_time: True
-      global_frame: odom
       rolling_window: true
       width: 3
       height: 3
@@ -186,8 +186,8 @@ global_costmap:
     ros__parameters:
       update_frequency: 1.0
       publish_frequency: 1.0
-      robot_base_frame: base_link
       global_frame: map
+      robot_base_frame: base_link
       use_sim_time: True
       plugin_names: ["static_layer", "obstacle_layer", "voxel_layer", "inflation_layer"]
       plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer", "nav2_costmap_2d::InflationLayer"]
@@ -245,6 +245,22 @@ planner_server_rclcpp_node:
   ros__parameters:
     use_sim_time: True
 
+recoveries_server:
+  ros__parameters:
+    costmap_topic: local_costmap/costmap_raw
+    footprint_topic: local_costmap/published_footprint
+    cycle_frequency: 10.0
+    plugin_names: ["spin", "back_up", "wait"]
+    plugin_types: ["nav2_recoveries/Spin", "nav2_recoveries/BackUp", "nav2_recoveries/Wait"]
+    global_frame: odom
+    robot_base_frame: base_link
+    transform_timeout: 0.1
+    use_sim_time: true
+    simulate_ahead_time: 2.0
+    max_rotational_vel: 1.0
+    min_rotational_vel: 0.4
+    rotational_acc_lim: 3.2
+
 robot_state_publisher:
   ros__parameters:
     use_sim_time: True

nav2_costmap_2d/include/nav2_costmap_2d/costmap_topic_collision_checker.hpp

@@ -47,6 +47,7 @@ class CostmapTopicCollisionChecker
     tf2_ros::Buffer & tf,
     std::string name = "collision_checker",
     std::string global_frame = "map",
+    std::string robot_base_frame = "base_link",
     double transform_tolerance = 0.1);
 
   ~CostmapTopicCollisionChecker() = default;
@@ -62,6 +63,7 @@ class CostmapTopicCollisionChecker
   // Name used for logging
   std::string name_;
   std::string global_frame_;
+  std::string robot_base_frame_;
   tf2_ros::Buffer & tf_;
   CostmapSubscriber & costmap_sub_;
   FootprintSubscriber & footprint_sub_;

nav2_costmap_2d/src/costmap_topic_collision_checker.cpp

@@ -38,9 +38,11 @@ CostmapTopicCollisionChecker::CostmapTopicCollisionChecker(
   tf2_ros::Buffer & tf,
   std::string name,
   std::string global_frame,
+  std::string robot_base_frame,
   double transform_tolerance)
 : name_(name),
   global_frame_(global_frame),
+  robot_base_frame_(robot_base_frame),
   tf_(tf),
   costmap_sub_(costmap_sub),
   footprint_sub_(footprint_sub),
@@ -107,7 +109,7 @@ void CostmapTopicCollisionChecker::unorientFootprint(
 {
   geometry_msgs::msg::PoseStamped current_pose;
   if (!nav2_util::getCurrentPose(
-      current_pose, tf_, global_frame_, "base_link",
+      current_pose, tf_, global_frame_, robot_base_frame_,
       transform_tolerance_))
   {
     throw CollisionCheckerException("Robot pose unavailable.");

nav2_recoveries/include/nav2_recoveries/recovery.hpp

@@ -97,6 +97,8 @@ class Recovery : public nav2_core::Recovery
     tf_ = tf;
 
     node_->get_parameter("cycle_frequency", cycle_frequency_);
+    node_->get_parameter("global_frame", global_frame_);
+    node_->get_parameter("robot_base_frame", robot_base_frame_);
     node_->get_parameter("transform_tolerance", transform_tolerance_);
 
     action_server_ = std::make_shared<ActionServer>(
@@ -141,6 +143,8 @@ class Recovery : public nav2_core::Recovery
 
   double cycle_frequency_;
   double enabled_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
   double transform_tolerance_;
 
   void execute()

nav2_recoveries/plugins/back_up.cpp

@@ -54,7 +54,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
   command_x_ = command->target.x;
   command_speed_ = command->speed;
 
-  if (!nav2_util::getCurrentPose(initial_pose_, *tf_, "odom", "base_link", transform_tolerance_)) {
+  if (!nav2_util::getCurrentPose(
+      initial_pose_, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
     RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available.");
     return Status::FAILED;
   }
@@ -65,7 +68,10 @@ Status BackUp::onRun(const std::shared_ptr<const BackUpAction::Goal> command)
 Status BackUp::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

nav2_recoveries/plugins/spin.cpp

@@ -71,7 +71,10 @@ void Spin::onConfigure()
 Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
@@ -89,7 +92,10 @@ Status Spin::onRun(const std::shared_ptr<const SpinAction::Goal> command)
 Status Spin::onCycleUpdate()
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, *tf_, "odom", "base_link", transform_tolerance_)) {
+  if (!nav2_util::getCurrentPose(
+      current_pose, *tf_, global_frame_, robot_base_frame_,
+      transform_tolerance_))
+  {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

nav2_recoveries/src/recovery_server.cpp

@@ -22,7 +22,7 @@ namespace recovery_server
 {
 
 RecoveryServer::RecoveryServer()
-: nav2_util::LifecycleNode("nav2_recoveries", "", true),
+: LifecycleNode("recoveries_server", "", true),
   plugin_loader_("nav2_core", "nav2_core::Recovery")
 {
   declare_parameter(
@@ -46,6 +46,12 @@ RecoveryServer::RecoveryServer()
     "plugin_types",
     rclcpp::ParameterValue(plugin_types));
 
+  declare_parameter(
+    "global_frame",
+    rclcpp::ParameterValue(std::string("odom")));
+  declare_parameter(
+    "robot_base_frame",
+    rclcpp::ParameterValue(std::string("base_link")));
   declare_parameter(
     "transform_tolerance",
     rclcpp::ParameterValue(0.1));
@@ -63,8 +69,8 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 
   tf_ = std::make_shared<tf2_ros::Buffer>(get_clock());
   auto timer_interface = std::make_shared<tf2_ros::CreateTimerROS>(
-    this->get_node_base_interface(),
-    this->get_node_timers_interface());
+    get_node_base_interface(),
+    get_node_timers_interface());
   tf_->setCreateTimerInterface(timer_interface);
   transform_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_);
 
@@ -76,11 +82,16 @@ RecoveryServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
     shared_from_this(), costmap_topic);
   footprint_sub_ = std::make_unique<nav2_costmap_2d::FootprintSubscriber>(
     shared_from_this(), footprint_topic, 1.0);
+
+  std::string global_frame, robot_base_frame;
+  get_parameter("global_frame", global_frame);
+  get_parameter("robot_base_frame", robot_base_frame);
   collision_checker_ = std::make_shared<nav2_costmap_2d::CostmapTopicCollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom", transform_tolerance_);
+    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(),
+    global_frame, robot_base_frame, transform_tolerance_);
 
-  this->get_parameter("plugin_names", plugin_names_);
-  this->get_parameter("plugin_types", plugin_types_);
+  get_parameter("plugin_names", plugin_names_);
+  get_parameter("plugin_types", plugin_types_);
 
   loadRecoveryPlugins();