Added Assisted Teleop feature in Navigation2 #2575

MOLOCH-dev · 2021-09-29T23:02:34Z

Basic Info

Signed-off-by: Anushree Sabnis [email protected]

Info	Please fill out this column
Ticket(s) this addresses	(add tickets here #2226 )
Primary OS tested on	(Ubuntu)
Robotic platform tested on	(gazebo simulation of turtlebot3)

Description of contribution in a few bullet points

I added Assistive Teleop as a feature in Nav2. It currently takes a Pure Collision Rejection approach to Assistive Teleop, as mentioned in the linked discourse topic.
In this, a teleop input velocity command is tested for collisions by projecting its footprint till N seconds in the future
If a collision is detected within N seconds, velocity is scaled down so the robot again remains N seconds away from a collision
Assisted Teleop is a Recovery Plugin that hosts its own action server
Added four new parameters : projection_time, linear_velocity_threshold, input_vel_topic and cmd_vel_topic
projection_time : Maximum allowed time to collision (N)
linear_velocity_threshold : Upon collision detection within projection_time, if the scaled-down velocity of the robot is below this threshold, the robot is stopped.
cmd_vel_topic : Assisted Teleop publishes velocity to this topic.
input_vel_topic : Assisted Teleop subscribes to this topic to receive velocity commands

Description of documentation updates required from your changes

Added new parameters, so need to add that to default configs and documentation page
Need to add documentation for Assisted Teleop, including an example behavior tree configuration

Future work that may be required in bullet points

Update README to include Assisted Teleop as a Recovery
I need to create a Behaviour Tree Node for Assisted Teleop
I need to potentially implement more ways for carrying out Assisted Teleop, exploring a mixture of Pure Collision Avoidance and Rejection approaches.

For Maintainers:

Check that any new parameters added are updated in navigation.ros.org
Check that any significant change is added to the migration guide
Check that any new functions have Doxygen added
Check that any new features have test coverage
Check that any new plugins is added to the plugins page
If BT Node, Additionally: add to BT's XML index of nodes for groot, BT package's readme table, and BT library lists

Signed-off-by: MOLOCH-dev <[email protected]>

SteveMacenski · 2021-10-01T17:21:06Z

scale angular velocities proportionally as well so we follow the same vector of travel
handle omni robots

SteveMacenski

Good first draft, I'll have some more review comments after you fix these, but these are necessary to get into the bigger architectural patterns

nav2_bringup/params/nav2_params.yaml

nav2_recoveries/plugins/assisted_teleop.hpp

SteveMacenski · 2021-10-01T20:44:26Z

nav2_recoveries/plugins/assisted_teleop.hpp

+  std::shared_ptr<tf2_ros::Buffer> tf_;
+  rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr vel_pub_;
+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;


You should have access to this from the main recovery server / collision checker object. You shouldn't need this?

Could you please guide me as to how I could get access to costmap_sub_ from the collision checker object? Since it is protected in the context of the CostmapTopicCollisionChecker, I'm unable to access the costmap subscriber

Why do you need the costmap sub at all?

I am unable to figure out a way to access the costmap (to use in the getResolution() method for calculating time increments ). Hence, I have retained this block of code in my most recent push. Could you please guide me as to how I can access the costmap object without creating a costmap subscriber?

You could add a method to get a reference to the costmap subscriber that the CostmapTopicCollisionChecker had internally, and then query that for the resolution. Maybe even have the costmap subscriber specifically store that to pass on in a function so that you don't need to populate a full costmap just to get a piece of metadata

SteveMacenski · 2021-10-01T20:45:08Z

nav2_recoveries/plugins/assisted_teleop.hpp

+  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr vel_sub_;
+  std::unique_ptr<nav2_costmap_2d::CostmapSubscriber> costmap_sub_;
+
+  // User defined parameters


Put all variables after all functions

Also, some of these look unnecessary (others don't have the correct naming with trailing underscores for member variables)

SteveMacenski · 2021-10-01T20:46:10Z

nav2_recoveries/plugins/assisted_teleop.cpp

+{
+using namespace std::chrono_literals; //NOLINT
+void
+AssistedTeleop::cleanup()


reorder functions to be: configure, activate, deactivate, cleanup to be in order

nav2_recoveries/plugins/assisted_teleop.cpp

nav2_msgs/action/AssistedTeleop.action

nav2_recoveries/plugins/assisted_teleop.cpp

SteveMacenski · 2021-10-13T22:09:47Z

Any update?

Signed-off-by: MOLOCH-dev <[email protected]>

MOLOCH-dev · 2021-10-14T16:23:15Z

Sorry for being a bit inactive, I was trying to figure out a way around some problems that I have mentioned here : I am done with all but 2 of the requested changes. The ones I am stuck with are the deletion of the costmap_sub_ object I have created to get access to the getResolution method and the velocity transformation from the base frame to the odom frame. I have commented under those requested changes,
I also had a doubt regarding creating the tests for assisted teleop, I have been working on creating a test that is a modification of the current test_recoveries.cpp file. Is that the correct approach? I can push the newly created test for review if that seems correct.

mergify · 2021-10-15T21:00:39Z

@MOLOCH-dev, your PR has failed to build. Please check CI outputs and resolve issues.
You may need to rebase or pull in main due to API changes (or your contribution genuinely fails).

SteveMacenski · 2021-10-18T18:46:02Z

nav2_recoveries/plugins/assisted_teleop.cpp

+  node->get_parameter("cmd_vel_topic", cmd_vel_topic_);
+  node->get_parameter("input_vel_topic", input_vel_topic_);
+
+  vel_sub_ = node->create_subscription<geometry_msgs::msg::Twist>(


Shouldn't this be created in execute now? So that its made when requested to start?

SteveMacenski · 2021-10-18T18:52:10Z

nav2_recoveries/plugins/assisted_teleop.hpp

+
+protected:
+  // Clock
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};


Remove, not used

SteveMacenski · 2021-10-18T18:52:33Z

nav2_recoveries/plugins/assisted_teleop.hpp

+  std::string cmd_vel_topic_;
+  std::string input_vel_topic_;
+
+  geometry_msgs::msg::PoseStamped current_pose;


doesn't need to be a class member

SteveMacenski · 2021-10-18T18:52:51Z

nav2_recoveries/plugins/assisted_teleop.hpp

+  geometry_msgs::msg::PoseStamped current_pose;
+  std::string recovery_name_;
+  std::shared_ptr<nav2_costmap_2d::Costmap2D> costmap_ros_;
+  geometry_msgs::msg::TransformStamped transform;


doesn't need to be a class member

SteveMacenski · 2021-10-18T18:53:01Z

nav2_recoveries/plugins/assisted_teleop.hpp

+  geometry_msgs::msg::TransformStamped transform;
+
+  // Parameters for Assisted Teleop
+  double scaling_factor = 1;


doesn't need to be a class member

SteveMacenski · 2021-10-18T19:02:02Z

On testing:

Create the object and call moveRobot with an example message, verify that it was received on the right topic with the right information based on a changed output velocity parameter
Create an example costmap to publish and call computeVelocity to test a few velocity commands, some that should be in collision, which it should back out of, and some that are not, that should be passed through, check the return command
Call stopRobot and check works to return on the right topic and 0 call
Create a system test to bring up the system in simulation and call the assisted teleop action and test that it works fine / timeout works / no collisions

SteveMacenski · 2021-10-18T18:59:59Z

nav2_recoveries/plugins/assisted_teleop.cpp

+}
+
+void
+AssistedTeleop::vel_callback(const geometry_msgs::msg::Twist::SharedPtr msg)


velCallback()

Signed-off-by: MOLOCH-dev <[email protected]>

mergify · 2021-11-18T20:24:54Z

This pull request is in conflict. Could you fix it @MOLOCH-dev?

MOLOCH-dev added 3 commits September 30, 2021 04:12

Assisted Teleop Recovery

30a9e1f

Signed-off-by: MOLOCH-dev <[email protected]>

Assisted Teleop Recovery

cf9816e

Signed-off-by: MOLOCH-dev <[email protected]>

Fixed build errors

9cd9745

Signed-off-by: MOLOCH-dev <[email protected]>

MOLOCH-dev mentioned this pull request Oct 1, 2021

Migrate Recovery Server to Behavior Server #2549

Closed

5 tasks

Omni robots

0c7d714

SteveMacenski requested changes Oct 1, 2021

View reviewed changes

Velocity transformation

60d33ea

Signed-off-by: MOLOCH-dev <[email protected]>

ros-navigation deleted a comment from mergify bot Oct 15, 2021

SteveMacenski reviewed Oct 18, 2021

View reviewed changes

ros-navigation deleted a comment from mergify bot Nov 8, 2021

Added getter function for costmap resolution

98fd7b1

Signed-off-by: MOLOCH-dev <[email protected]>

SteveMacenski closed this Mar 24, 2022

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added Assisted Teleop feature in Navigation2 #2575

Added Assisted Teleop feature in Navigation2 #2575

MOLOCH-dev commented Sep 29, 2021 •

edited

Loading

SteveMacenski commented Oct 1, 2021

SteveMacenski left a comment

SteveMacenski Oct 1, 2021

MOLOCH-dev Oct 5, 2021

SteveMacenski Oct 6, 2021

MOLOCH-dev Oct 14, 2021

SteveMacenski Oct 18, 2021 •

edited

Loading

SteveMacenski Oct 1, 2021

SteveMacenski Oct 1, 2021

SteveMacenski commented Oct 13, 2021

MOLOCH-dev commented Oct 14, 2021 •

edited

Loading

mergify bot commented Oct 15, 2021

SteveMacenski Oct 18, 2021

SteveMacenski Oct 18, 2021

SteveMacenski Oct 18, 2021

SteveMacenski Oct 18, 2021

SteveMacenski Oct 18, 2021

SteveMacenski commented Oct 18, 2021

SteveMacenski Oct 18, 2021

mergify bot commented Nov 18, 2021

Added Assisted Teleop feature in Navigation2 #2575

Added Assisted Teleop feature in Navigation2 #2575

Conversation

MOLOCH-dev commented Sep 29, 2021 • edited Loading

Basic Info

Description of contribution in a few bullet points

Description of documentation updates required from your changes

Future work that may be required in bullet points

For Maintainers:

SteveMacenski commented Oct 1, 2021

SteveMacenski left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Oct 18, 2021 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski commented Oct 13, 2021

MOLOCH-dev commented Oct 14, 2021 • edited Loading

mergify bot commented Oct 15, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski commented Oct 18, 2021

Choose a reason for hiding this comment

mergify bot commented Nov 18, 2021

MOLOCH-dev commented Sep 29, 2021 •

edited

Loading

SteveMacenski Oct 18, 2021 •

edited

Loading

MOLOCH-dev commented Oct 14, 2021 •

edited

Loading