Use parameterized frames (ros-planning#1726)

Signed-off-by: ymd-stella <world.applepie@gmail.com>
ymd-stella · May 14, 2020 · 71f9a62 · 71f9a62
1 parent 13633cd
commit 71f9a62
Show file tree

Hide file tree

Showing 7 changed files with 32 additions and 9 deletions.
diff --git a/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp b/nav2_behavior_tree/plugins/condition/goal_reached_condition.cpp
@@ -60,6 +60,8 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
     node_->get_parameter_or<double>("goal_reached_tol", goal_reached_tol_, 0.25);
+    node_->get_parameter_or<std::string>("global_frame", global_frame_, "map");
+    node_->get_parameter_or<std::string>("robot_base_frame", robot_base_frame_, "base_link");
     tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 
     initialized_ = true;
@@ -70,7 +72,7 @@ class GoalReachedCondition : public BT::ConditionNode
   {
     geometry_msgs::msg::PoseStamped current_pose;
 
-    if (!nav2_util::getCurrentPose(current_pose, *tf_)) {
+    if (!nav2_util::getCurrentPose(current_pose, *tf_, global_frame_, robot_base_frame_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return false;
     }
@@ -105,6 +107,8 @@ class GoalReachedCondition : public BT::ConditionNode
 
   bool initialized_;
   double goal_reached_tol_;
+  std::string global_frame_;
+  std::string robot_base_frame_;
 };
 
 }  // namespace nav2_behavior_tree

diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/collision_checker.hpp
@@ -44,7 +44,8 @@ class CollisionChecker
     FootprintSubscriber & footprint_sub,
     tf2_ros::Buffer & tf,
     std::string name = "collision_checker",
-    std::string global_frame = "map");
+    std::string global_frame = "map",
+    std::string robot_base_frame = "base_link");
 
   ~CollisionChecker();
 
@@ -65,6 +66,7 @@ class CollisionChecker
   // 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_;

diff --git a/nav2_costmap_2d/src/collision_checker.cpp b/nav2_costmap_2d/src/collision_checker.cpp
@@ -34,9 +34,11 @@ CollisionChecker::CollisionChecker(
   FootprintSubscriber & footprint_sub,
   tf2_ros::Buffer & tf,
   std::string name,
-  std::string global_frame)
+  std::string global_frame,
+  std::string robot_base_frame)
 : name_(name),
   global_frame_(global_frame),
+  robot_base_frame_(robot_base_frame),
   tf_(tf),
   costmap_sub_(costmap_sub),
   footprint_sub_(footprint_sub)
@@ -173,7 +175,7 @@ void CollisionChecker::unorientFootprint(
   std::vector<geometry_msgs::msg::Point> & reset_footprint)
 {
   geometry_msgs::msg::PoseStamped current_pose;
-  if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_)) {
+  if (!nav2_util::getCurrentPose(current_pose, tf_, global_frame_, robot_base_frame_)) {
     throw CollisionCheckerException("Robot pose unavailable.");
   }
 

diff --git a/nav2_recoveries/include/nav2_recoveries/recovery.hpp b/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("odom_frame", odom_frame_);
+    node_->get_parameter("robot_base_frame", robot_base_frame_);
 
     action_server_ = std::make_shared<ActionServer>(
       node_, recovery_name_,
@@ -140,6 +142,8 @@ class Recovery : public nav2_core::Recovery
 
   double cycle_frequency_;
   double enabled_;
+  std::string odom_frame_;
+  std::string robot_base_frame_;
 
   void execute()
   {

diff --git a/nav2_recoveries/plugins/back_up.cpp b/nav2_recoveries/plugins/back_up.cpp
@@ -53,7 +53,7 @@ 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")) {
+  if (!nav2_util::getCurrentPose(initial_pose_, *tf_, odom_frame_, robot_base_frame_)) {
     RCLCPP_ERROR(node_->get_logger(), "Initial robot pose is not available.");
     return Status::FAILED;
   }
@@ -64,7 +64,7 @@ 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")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

diff --git a/nav2_recoveries/plugins/spin.cpp b/nav2_recoveries/plugins/spin.cpp
@@ -70,7 +70,7 @@ 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")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }
@@ -88,7 +88,7 @@ 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")) {
+  if (!nav2_util::getCurrentPose(current_pose, *tf_, odom_frame_, robot_base_frame_)) {
     RCLCPP_ERROR(node_->get_logger(), "Current robot pose is not available.");
     return Status::FAILED;
   }

diff --git a/nav2_recoveries/src/recovery_server.cpp b/nav2_recoveries/src/recovery_server.cpp
@@ -45,6 +45,13 @@ RecoveryServer::RecoveryServer()
   declare_parameter(
     "plugin_types",
     rclcpp::ParameterValue(plugin_types));
+
+  declare_parameter(
+    "odom_frame",
+    rclcpp::ParameterValue("odom"));
+  declare_parameter(
+    "robot_base_frame",
+    rclcpp::ParameterValue("base_link"));
 }
 
 
@@ -71,8 +78,12 @@ 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 odom_frame, robot_base_frame;
+  this->get_parameter("odom_frame", odom_frame);
+  this->get_parameter("robot_base_frame", robot_base_frame);
   collision_checker_ = std::make_shared<nav2_costmap_2d::CollisionChecker>(
-    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), "odom");
+    *costmap_sub_, *footprint_sub_, *tf_, this->get_name(), odom_frame, robot_base_frame);
 
   this->get_parameter("plugin_names", plugin_names_);
   this->get_parameter("plugin_types", plugin_types_);