diff --git a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp index 989f4ffd8dc..8539b98432e 100644 --- a/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp +++ b/nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp @@ -42,6 +42,8 @@ class ComputePathToPoseAction : public BtActionNode("path", "Path created by ComputePathToPose node"), BT::InputPort("goal", "Destination to plan to"), + BT::InputPort( + "start", "Start pose of the path if overriding current robot pose"), BT::InputPort("planner_id", ""), }); } diff --git a/nav2_behavior_tree/nav2_tree_nodes.xml b/nav2_behavior_tree/nav2_tree_nodes.xml index 9a36ccdc6e0..0963fea7db0 100644 --- a/nav2_behavior_tree/nav2_tree_nodes.xml +++ b/nav2_behavior_tree/nav2_tree_nodes.xml @@ -18,6 +18,7 @@ Destination to plan to + Start pose of the path if overriding current robot pose Path created by ComputePathToPose node diff --git a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp index efbabbdbea9..83b29dfe3a3 100644 --- a/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp @@ -30,8 +30,11 @@ ComputePathToPoseAction::ComputePathToPoseAction( void ComputePathToPoseAction::on_tick() { - getInput("goal", goal_.pose); + getInput("goal", goal_.goal); getInput("planner_id", goal_.planner_id); + if (getInput("start", goal_.start)) { + goal_.use_start = true; + } } BT::NodeStatus ComputePathToPoseAction::on_success() diff --git a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp index d5d1cd6621a..595f88e0e34 100644 --- a/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp +++ b/nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp @@ -41,8 +41,13 @@ class ComputePathToPoseActionServer : public TestActionServerget_goal(); auto result = std::make_shared(); - result->path.poses.resize(1); - result->path.poses[0].pose.position.x = goal->pose.pose.position.x; + result->path.poses.resize(2); + result->path.poses[1].pose.position.x = goal->goal.pose.position.x; + if (goal->use_start) { + result->path.poses[0].pose.position.x = goal->start.pose.position.x; + } else { + result->path.poses[0].pose.position.x = 0.0; + } goal_handle->succeed(result); } }; @@ -136,14 +141,16 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) // the goal should have reached our server EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_FALSE(action_server_->getCurrentGoal()->use_start); EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); // check if returned path is correct nav_msgs::msg::Path path; config_->blackboard->get("path", path); - EXPECT_EQ(path.poses.size(), 1u); - EXPECT_EQ(path.poses[0].pose.position.x, 1.0); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); // halt node so another goal can be sent tree_->rootNode()->halt(); @@ -158,11 +165,80 @@ TEST_F(ComputePathToPoseActionTestFixture, test_tick) } EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); - EXPECT_EQ(action_server_->getCurrentGoal()->pose.pose.position.x, -2.5); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); config_->blackboard->get("path", path); - EXPECT_EQ(path.poses.size(), 1u); - EXPECT_EQ(path.poses[0].pose.position.x, -2.5); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 0.0); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); +} + +TEST_F(ComputePathToPoseActionTestFixture, test_tick_use_start) +{ + // create tree + std::string xml_txt = + R"( + + + + + )"; + + tree_ = std::make_shared(factory_->createTreeFromText(xml_txt, config_->blackboard)); + + // create new start and set it on blackboard + geometry_msgs::msg::PoseStamped start; + start.header.stamp = node_->now(); + start.pose.position.x = 2.0; + config_->blackboard->set("start", start); + + // create new goal and set it on blackboard + geometry_msgs::msg::PoseStamped goal; + goal.header.stamp = node_->now(); + goal.pose.position.x = 1.0; + config_->blackboard->set("goal", goal); + + // tick until node succeeds + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + // the goal should have reached our server + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(tree_->rootNode()->getInput("planner_id"), std::string("GridBased")); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, 1.0); + EXPECT_EQ(action_server_->getCurrentGoal()->start.pose.position.x, 2.0); + EXPECT_TRUE(action_server_->getCurrentGoal()->use_start); + EXPECT_EQ(action_server_->getCurrentGoal()->planner_id, std::string("GridBased")); + + // check if returned path is correct + nav_msgs::msg::Path path; + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, 2.0); + EXPECT_EQ(path.poses[1].pose.position.x, 1.0); + + // halt node so another goal can be sent + tree_->rootNode()->halt(); + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::IDLE); + + // set new goal and new start + goal.pose.position.x = -2.5; + start.pose.position.x = -1.5; + config_->blackboard->set("goal", goal); + config_->blackboard->set("start", start); + + while (tree_->rootNode()->status() != BT::NodeStatus::SUCCESS) { + tree_->rootNode()->executeTick(); + } + + EXPECT_EQ(tree_->rootNode()->status(), BT::NodeStatus::SUCCESS); + EXPECT_EQ(action_server_->getCurrentGoal()->goal.pose.position.x, -2.5); + + config_->blackboard->get("path", path); + EXPECT_EQ(path.poses.size(), 2u); + EXPECT_EQ(path.poses[0].pose.position.x, -1.5); + EXPECT_EQ(path.poses[1].pose.position.x, -2.5); } int main(int argc, char ** argv) diff --git a/nav2_msgs/action/ComputePathToPose.action b/nav2_msgs/action/ComputePathToPose.action index add60a824dc..090c28ccaf4 100644 --- a/nav2_msgs/action/ComputePathToPose.action +++ b/nav2_msgs/action/ComputePathToPose.action @@ -1,6 +1,8 @@ #goal definition -geometry_msgs/PoseStamped pose +geometry_msgs/PoseStamped goal +geometry_msgs/PoseStamped start string planner_id +bool use_start # If true, use current robot pose as path start, if false, use start above instead --- #result definition nav_msgs/Path path diff --git a/nav2_planner/src/planner_server.cpp b/nav2_planner/src/planner_server.cpp index 79167d0f3a0..16df788cf0d 100644 --- a/nav2_planner/src/planner_server.cpp +++ b/nav2_planner/src/planner_server.cpp @@ -232,8 +232,11 @@ PlannerServer::computePlan() r.sleep(); } + // Use start pose if provided otherwise use current robot pose geometry_msgs::msg::PoseStamped start; - if (!costmap_ros_->getRobotPose(start)) { + if (goal->use_start) { + start = goal->start; + } else if (!costmap_ros_->getRobotPose(start)) { action_server_->terminate_current(); return; } @@ -242,13 +245,13 @@ PlannerServer::computePlan() goal = action_server_->accept_pending_goal(); } - result->path = getPlan(start, goal->pose, goal->planner_id); + result->path = getPlan(start, goal->goal, goal->planner_id); if (result->path.poses.size() == 0) { RCLCPP_WARN( get_logger(), "Planning algorithm %s failed to generate a valid" " path to (%.2f, %.2f)", goal->planner_id.c_str(), - goal->pose.pose.position.x, goal->pose.pose.position.y); + goal->goal.pose.position.x, goal->goal.pose.position.y); action_server_->terminate_current(); return; } @@ -256,8 +259,8 @@ PlannerServer::computePlan() RCLCPP_DEBUG( get_logger(), "Found valid path of size %lu to (%.2f, %.2f)", - result->path.poses.size(), goal->pose.pose.position.x, - goal->pose.pose.position.y); + result->path.poses.size(), goal->goal.pose.position.x, + goal->goal.pose.position.y); // Publish the plan for visualization purposes publishPlan(result->path); @@ -276,8 +279,8 @@ PlannerServer::computePlan() } catch (std::exception & ex) { RCLCPP_WARN( get_logger(), "%s plugin failed to plan calculation to (%.2f, %.2f): \"%s\"", - goal->planner_id.c_str(), goal->pose.pose.position.x, - goal->pose.pose.position.y, ex.what()); + goal->planner_id.c_str(), goal->goal.pose.position.x, + goal->goal.pose.position.y, ex.what()); // TODO(orduno): provide information about fail error to parent task, // for example: couldn't get costmap update action_server_->terminate_current();