Skip to content

Ability to get a path from the planner from any 2 poses (start_pose and goal): add use_start_pose and start_pose to ComputePathToPose action #2179

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
SteveMacenski merged 10 commits into from
Mar 5, 2021
Merged

Ability to get a path from the planner from any 2 poses (start_pose and goal): add use_start_pose and start_pose to ComputePathToPose action #2179

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
410cfc5
add use_start_pose to ComputePathToPose.action
Feb 11, 2021
6ac2b5e
add use_start_pose to ComputePathToPose bt
Feb 11, 2021
e866db6
start instead of start_pose + else if
Feb 15, 2021
8a770b1
[ComputePathToPose Action API break] change 'pose' for 'goal' +use_start
Feb 24, 2021
ef1774f
test wip
Feb 24, 2021
dd22a17
add compute path to pose BT test for use_start
Feb 24, 2021
0a10ae0
last start_pose renaming
Feb 25, 2021
4ed8eb0
Transform start and goal in costmap frame
Feb 25, 2021
ad27806
Revert "Transform start and goal in costmap frame"
Mar 4, 2021
658f80d
lint
Mar 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions nav2_behavior_tree/include/nav2_behavior_tree/plugins/action/compute_path_to_pose_action.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,8 @@ class ComputePathToPoseAction : public BtActionNode<nav2_msgs::action::ComputePa
{
BT::OutputPort<nav_msgs::msg::Path>("path", "Path created by ComputePathToPose node"),
BT::InputPort<geometry_msgs::msg::PoseStamped>("goal", "Destination to plan to"),
BT::InputPort<geometry_msgs::msg::PoseStamped>(
"start", "Start pose of the path if overriding current robot pose"),
BT::InputPort<std::string>("planner_id", ""),
});
}
Expand Down
1 change: 1 addition & 0 deletions nav2_behavior_tree/nav2_tree_nodes.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@

<Action ID="ComputePathToPose">
<input_port name="goal">Destination to plan to</input_port>
<input_port name="start">Start pose of the path if overriding current robot pose</input_port>
<output_port name="path">Path created by ComputePathToPose node</output_port>
<input_port name="planner_id"/>
</Action>
Expand Down
5 changes: 4 additions & 1 deletion nav2_behavior_tree/plugins/action/compute_path_to_pose_action.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
92 changes: 84 additions & 8 deletions nav2_behavior_tree/test/plugins/action/test_compute_path_to_pose_action.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -41,8 +41,13 @@ class ComputePathToPoseActionServer : public TestActionServer<nav2_msgs::action:
{
const auto goal = goal_handle->get_goal();
auto result = std::make_shared<nav2_msgs::action::ComputePathToPose::Result>();
result->path.poses.resize(1);
result->path.poses[0].pose.position.x = goal->pose.pose.position.x;
result->path.poses.resize(2);
Copy link
Copy Markdown
Member

@SteveMacenski SteveMacenski Mar 2, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This test calls the fixture and not the actual planner server, so it doesn't fully test what we need to test. We need a test that is going to call the actual planner server itself and shows that when the planner server has the start set that it creates a valid plan using that information. You can do this using the system_tests/planner tests. There's a random and fixed map tests you could add an additional test case to where you set use_start and a start to show that it respects that and not the current robot TF starting pose.

This test is helpful for completion (showing the BT is reacting well), but this isn't a test that fully encapsulates the main thing we need to have coverage over (that changing the start actually "works").

Copy link
Copy Markdown
Contributor Author

@doisyg doisyg Mar 3, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can do this using the system_tests/planner tests. There's a random and fixed map tests you could add an additional test case to where you set use_start and a start to show that it respects that and not the current robot TF starting pose.

It is more complex than that, as I tried to express above, the current system_tests/planner tests are never testing the ComputePathToPose action_server of the planner_server defined here:
https://github.com/ros-planning/navigation2/blob/2d88d14b1dd9f678e9b95dd50c5db774667cd62e/nav2_planner/src/planner_server.cpp#L132-L136

And hence, the logic of getting the pose of the robot as a start of the path (or the use_start potential override) as implemented in the action server of planner_server is never tested.
Instead, it is the only the createPlan(start, goal) fonction of the planner_server which is tested, not the full action. And the planner_tester implements its own logic for retrieving the start pose of the path, see here:
https://github.com/ros-planning/navigation2/blob/2d88d14b1dd9f678e9b95dd50c5db774667cd62e/nav2_system_tests/src/planning/planner_tester.hpp#L77-L86

What we need, I believe, is to create tests calling the ComputePathToPose action_server of the planner_server, for both use_start true and false. I might consider doing it if align and if you give me pointers on the architecture of it. Should planner_tester be extended to call the action ? Or a separate class ?

Copy link
Copy Markdown
Member

@SteveMacenski SteveMacenski Mar 3, 2021 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Got it, thanks for the explanation. I agree those tests are not probably in the structure for this testing.

The system tests have this structure of launching a launch description (which could just contain a lifecycle manager to transition up the planner server and the planner server) and then a test python node to trigger inputs and outputs. That could work here but the issue I see is that no info would actually come to the planner server, so you'd need to also have a map_server to publish some map for the planner server's costmap to be aware of. Then when you called ComputePathToPose in the python tester and be done (but probably have to fake out TF, which there are examples for in system_tests).

It turns into a bit of a challenge. I could wave the testing of this one in CI if you verified locally that this worked fully. Though it might be a good exercise if you haven't done something like that before and I would never argue with more 😄

Copy link
Copy Markdown
Contributor Author

@doisyg doisyg Mar 4, 2021

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hum, it will be indeed a good exercise. No promise as I am very busy atm, but don't hesitate to ping me in a week or so if you see no progress (and yes, it was tested locally).

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);
}
};
Expand Down Expand Up @@ -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<std::string>("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<nav_msgs::msg::Path>("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();
Expand All @@ -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<nav_msgs::msg::Path>("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"(
<root main_tree_to_execute = "MainTree" >
<BehaviorTree ID="MainTree">
<ComputePathToPose goal="{goal}" start="{start}" path="{path}" planner_id="GridBased"/>
</BehaviorTree>
</root>)";

tree_ = std::make_shared<BT::Tree>(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<std::string>("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<nav_msgs::msg::Path>("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<nav_msgs::msg::Path>("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)
Expand Down
4 changes: 3 additions & 1 deletion nav2_msgs/action/ComputePathToPose.action
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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
Expand Down
17 changes: 10 additions & 7 deletions nav2_planner/src/planner_server.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
Expand All @@ -242,22 +245,22 @@ 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;
}

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);
Expand All @@ -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();
Expand Down