diff --git a/nav2_bt_navigator/behavior_trees/simple_sequential.xml b/nav2_bt_navigator/behavior_trees/simple_sequential.xml index 4f6a441d4ee4a73fd8d4b2d8c963b9f3c2db3296..4a0844c88f2683a78fc9a88c8ddd1fd34d28ef2e 100644 --- a/nav2_bt_navigator/behavior_trees/simple_sequential.xml +++ b/nav2_bt_navigator/behavior_trees/simple_sequential.xml @@ -1,15 +1,12 @@ <!-- - This Behavior Tree does the bare minimum - it invokes ComputePathToPose to - compute a path using the global planner. It then invokes the local planner - (FollowPath) with this path. The path endpoints (input to the global planner) - and the resulting path (output of the global planner and input to the local - planner) are passed on the blackboard. + This Behavior Tree does the minimum - it invokes ComputePathToPose once to compute a path + using the global planner. It then invokes the local planner (FollowPath) with this path. --> <root main_tree_to_execute="MainTree"> <BehaviorTree ID="MainTree"> <SequenceStar name="root"> - <ComputePathToPose goal="${goal}" path="${path}"/> + <ComputePathToPose goal="${goal}"/> <FollowPath path="${path}"/> </SequenceStar> </BehaviorTree> diff --git a/nav2_bt_navigator/src/bt_navigator.cpp b/nav2_bt_navigator/src/bt_navigator.cpp index 3e6516dedd278ce0b66eac1003f1d4a2779b0a91..f0b2b2feb64fd0c5ea697979a5a1ca82c9d7fc5e 100644 --- a/nav2_bt_navigator/src/bt_navigator.cpp +++ b/nav2_bt_navigator/src/bt_navigator.cpp @@ -51,7 +51,7 @@ BtNavigator::on_configure(const rclcpp_lifecycle::State & /*state*/) self_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>( client_node_, "NavigateToPose"); - goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>("goal", + goal_sub_ = rclcpp_node_->create_subscription<geometry_msgs::msg::PoseStamped>("/move_base_simple/goal", rclcpp::SystemDefaultsQoS(), std::bind(&BtNavigator::onGoalPoseReceived, this, std::placeholders::_1));