diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index c8a703e992..239c42855b 100644 --- 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("node"); tf_ = config().blackboard->get>("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 index c43351534f..9e83994d6b 100644 --- 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("distance", 1.0, "Distance") + BT::InputPort("distance", 1.0, "Distance"), + BT::InputPort("global_frame", std::string("map"), "Global frame"), + BT::InputPort("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_; };