[WIP] Following an object a certain distance by fmrico · Pull Request #1748 · ros-navigation/navigation2

fmrico · 2020-05-17T17:08:30Z

Signed-off-by: Francisco Martin Rico fmrico@gmail.com

Basic Info

Info	Please fill out this column
Ticket(s) this addresses	#1660
Primary OS tested on	Ubuntu
Robotic platform tested on	gazebo simulation of Tally

This is a work in progress. I don't pretend to be accepted as is. I want only to discuss this approach.

The goal of this work is following a dynamic pose to a certain distance:

The goal pose can change during the execution of navigate_to_pose action.
The robot stops when the goal is nearer than a configured distance, and then it faces the goal.

One use case could be following a person to ~1 meter in a house. We want to use it in RoboCup@Home.

I don't want to implement this as a new plugin for controller or planner. This work is independent of the algorithms used to create the plan and following it. To implement it I have:

developed a new control node in nav2_behavior_tree that overwrites the goal received in the navigate_to_pose action for the one received in the goal_update topic.
added two input ports to FollowPath for indicating that never returns from the action, and the distance to the object. The other option was to create another action node in BT, action, action client, action server...

This is the result (click for video):

I don't know if this is the right place to discuss this, but the complete real use case I want to address is:

Make the robot navigate to the desired pose on the map.
Once there, make the robot follow a person (the dynamic goal pose, provided from outside Nav2)

The behavior trees used in the two steps in this application are (could be) different, but the BT to use is defined statically in the yaml param files. One option to solve this could be to select the BT to use in NavigateToPose action. Maybe NavigateToPose action could also include some options decided from behaviors, instead to be fixed in params.

Francisco

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

SteveMacenski · 2020-05-18T20:15:28Z

nav2_behavior_tree/README.md

-| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. |
 | FollowPath | Action |Invokes the FollowPath ROS2 action server, which is implemented by the controller plugin modules loaded. The server address can be remapped using the `server_name` input port. |
+| ComputePathToPose |    Action   | Invokes the ComputePathToPose ROS2 action server, which is implemented by the nav2_planner module. The server address can be remapped using the `server_name` input port. |
+| UpdateGoalPose | Action |Overwrite goal pose by one received in a topic. |


Missing a space here between the pipe and Overwrite

SteveMacenski · 2020-05-18T20:15:48Z

nav2_behavior_tree/nav2_tree_nodes.xml

    </Action>

+    <Action ID="UpdateGoalPose">
+      <input_port name="goal">Destination to plan to</input_port>


Suggested change

<input_port name="goal">Destination to plan to</input_port>

<input_port name="goal">Dynamically updating destination to plan to</input_port>

SteveMacenski · 2020-05-18T20:16:32Z

nav2_behavior_tree/CMakeLists.txt

 add_library(nav2_compute_path_to_pose_action_bt_node SHARED plugins/action/compute_path_to_pose_action.cpp)
 list(APPEND plugin_libs nav2_compute_path_to_pose_action_bt_node)

+add_library(nav2_update_goal_pose_action_bt_node SHARED plugins/action/update_goal_pose_action.cpp)


Can we add the word "dynamically" to the naming here to make it clear its for dynamic goal updating in a single task

SteveMacenski · 2020-05-18T20:17:56Z

nav2_behavior_tree/plugins/action/follow_path_action.cpp

      {
        BT::InputPort<nav_msgs::msg::Path>("path", "Path to follow"),
        BT::InputPort<std::string>("controller_id", ""),
+        BT::InputPort<double>("distance_to_goal", "Distance to goal to stop translation"),


Shouldn't this distance to goal be computed by the dynamic new goal to follow (e.g. it is set at the distance away from the obstacle)

SteveMacenski · 2020-05-18T20:18:11Z

nav2_behavior_tree/plugins/action/update_goal_pose_action.cpp

@@ -0,0 +1,99 @@
+// Copyright (c) 2018 Intel Corporation


You should have your copyright here

SteveMacenski · 2020-05-18T20:19:53Z

nav2_behavior_tree/plugins/action/update_goal_pose_action.cpp

+    node_ = config().blackboard->get<rclcpp::Node::SharedPtr>("node");
+
+    updated_goal_sub_ = node_->create_subscription<geometry_msgs::msg::PoseStamped>("goal_update",
+      10, std::bind(&UpdateGoalPoseAction::updatedGoalCallback, this, std::placeholders::_1));


I think you really want a queue depth of 1 so that you always get the newest one

SteveMacenski · 2020-05-18T20:20:12Z

nav2_bt_navigator/behavior_trees/navigate_carrot.xml

@@ -0,0 +1,17 @@
+<!--
+  This Behavior Tree replans the global path periodically at 1 Hz.


You should update this description of the BT

SteveMacenski · 2020-05-18T20:20:41Z

nav2_bt_navigator/behavior_trees/navigate_carrot.xml

+      <RateController hz="1.0">
+        <PipelineSequence name="GeneratePath">
+          <UpdateGoalPose goal="{goal}"/>
+          <ComputePathToPose goal="{goal}" path="{path}" planner_id="GridBased"/>


Do you not also want a replanning rate in case your haven't gotten an updated dynamic goal pose recently?

SteveMacenski · 2020-05-18T20:21:31Z

nav2_controller/src/nav2_controller.cpp

+  const geometry_msgs::msg::PoseStamped & pose_a,
+  const geometry_msgs::msg::PoseStamped & pose_b)
+{
+  double x_diff = fabs(pose_a.pose.position.x - pose_b.pose.position.x);


You don't need the fabs since you're throwing them into hypot

We also have a new utility in geometry_util in the nav2_util for this sort of thing, you should use it.

SteveMacenski · 2020-05-18T20:22:42Z

nav2_controller/src/nav2_controller.cpp

+  return std::hypot(x_diff, y_diff);
+}
+
+geometry_msgs::msg::TwistStamped getTwistToPose(


Why is this here?

All of these changes in the controller server - I'm not sure they belong here. Can you explain this? This all feels application specific and might need to be in a plugin. I'm not 100% sure what the goal of this is, if you have a path to follow towards something, why does the controller need any updates?

SteveMacenski · 2020-05-18T20:25:14Z

nav2_controller/src/nav2_controller.cpp


-  progress_checker_->check(pose);
-
+  if (!keep_running) {


I'm not also sure about this keep running thing - if you've made it to N meters away from your goal, you would want it to stop. Once it starts moving again, then the controller can follow the new path to be N meters from that again.

I think the robot should slow down as it approaches the target so that you can keep smooth motion, else I think it would be pretty jerky

SteveMacenski · 2020-06-26T19:11:19Z

@fmrico what's the story with this PR?

fmrico · 2020-06-26T20:30:32Z

Hi @SteveMacenski, I have been very busy this week preparing the ROSDevDay2020, but I want to retake this work next week. I put this PR in stand by while I finished #1780, to provide a complete demo.

SteveMacenski · 2020-06-26T22:18:55Z

Look forward to your talk!

fmrico · 2020-07-08T07:09:18Z

Closed and continued in #1859

First functional version

2984543

Signed-off-by: Francisco Martin Rico <fmrico@gmail.com>

SteveMacenski requested changes May 18, 2020

View reviewed changes

SteveMacenski reviewed May 18, 2020

View reviewed changes

fmrico mentioned this pull request May 28, 2020

Select behavior Tree in NavigateToPoseAction #1780

Closed

fmrico mentioned this pull request Jul 8, 2020

Follow pose #1859

Merged

fmrico closed this Jul 8, 2020

	<input_port name="goal">Destination to plan to</input_port>
	<input_port name="goal">Dynamically updating destination to plan to</input_port>

		@@ -0,0 +1,17 @@
		<!--
		This Behavior Tree replans the global path periodically at 1 Hz.

Conversation

fmrico commented May 17, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Signed-off-by: Francisco Martin Rico fmrico@gmail.com

Basic Info

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski May 18, 2020 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

SteveMacenski commented Jun 26, 2020

Uh oh!

fmrico commented Jun 26, 2020

Uh oh!

SteveMacenski commented Jun 26, 2020

Uh oh!

fmrico commented Jul 8, 2020

Uh oh!

Reviewers

Assignees

Labels

Projects

Milestone

Development

Uh oh!

2 participants

fmrico commented May 17, 2020 •

edited

Loading

SteveMacenski May 18, 2020 •

edited

Loading