Expose distance controller frames to ports (#1741)

ros-navigation · May 15, 2020 · f0cf034 · f0cf034
1 parent 21e2fec
commit f0cf034
Show file tree

Hide file tree

Showing 2 changed files with 14 additions and 4 deletions.
diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp
@@ -35,9 +35,13 @@ DistanceController::DistanceController(
   const BT::NodeConfiguration & conf)
 : BT::DecoratorNode(name, conf),
   distance_(1.0),
+  global_frame_("map"),
+  robot_base_frame_("base_link"),
   first_time_(false)
 {
   getInput("distance", distance_);
+  getInput("global_frame", global_frame_);
+  getInput("robot_base_frame", robot_base_frame_);
   node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
   tf_ = config().blackboard->get<std::shared_ptr<tf2_ros::Buffer>>("tf_buffer");
 }
@@ -47,7 +51,7 @@ inline BT::NodeStatus DistanceController::tick()
   if (status() == BT::NodeStatus::IDLE) {
     // Reset the starting position since we're starting a new iteration of
     // the distance controller (moving from IDLE to RUNNING)
-    if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
+    if (!nav2_util::getCurrentPose(start_pose_, *tf_, global_frame_, robot_base_frame_)) {
       RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
       return BT::NodeStatus::FAILURE;
     }
@@ -58,7 +62,7 @@ inline BT::NodeStatus DistanceController::tick()
 
   // Determine distance travelled since we've started this iteration
   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 BT::NodeStatus::FAILURE;
   }
@@ -81,7 +85,7 @@ inline BT::NodeStatus DistanceController::tick()
         return BT::NodeStatus::RUNNING;
 
       case BT::NodeStatus::SUCCESS:
-        if (!nav2_util::getCurrentPose(start_pose_, *tf_)) {
+        if (!nav2_util::getCurrentPose(start_pose_, *tf_, global_frame_, robot_base_frame_)) {
           RCLCPP_DEBUG(node_->get_logger(), "Current robot pose is not available.");
           return BT::NodeStatus::FAILURE;
         }

diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp b/nav2_behavior_tree/plugins/decorator/distance_controller.hpp
@@ -38,7 +38,9 @@ class DistanceController : public BT::DecoratorNode
   static BT::PortsList providedPorts()
   {
     return {
-      BT::InputPort<double>("distance", 1.0, "Distance")
+      BT::InputPort<double>("distance", 1.0, "Distance"),
+      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")
     };
   }
 
@@ -51,6 +53,10 @@ class DistanceController : public BT::DecoratorNode
 
   geometry_msgs::msg::PoseStamped start_pose_;
   double distance_;
+
+  std::string global_frame_;
+  std::string robot_base_frame_;
+
   bool first_time_;
 };