diff --git a/nav2_behavior_tree/plugins/decorator/distance_controller.cpp b/nav2_behavior_tree/plugins/decorator/distance_controller.cpp index c8a703e99247f9773c2a9d1b9ea0b24fdb53b150..239c42855b5b0884433c5e67be0d25560b9f6da7 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<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 index c43351534ff77f0d4375b53f8a4f285af9e2fe6b..9e83994d6b773a54645329afae8fcf9a80d6db27 100644 --- a/nav2_behavior_tree/plugins/decorator/distance_controller.hpp +++ b/nav2_behavior_tree/plugins/decorator/distance_controller.hpp @@ -38,7 +38,9 @@ public: 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 @@ private: geometry_msgs::msg::PoseStamped start_pose_; double distance_; + + std::string global_frame_; + std::string robot_base_frame_; + bool first_time_; };