Following server #43

ajtudela · 2024-06-07T19:04:29Z

I've opened this draft so you can take a look at the code and open a discussion to add changes, improvemnet, etc. Later, I'll create a new PR with all the changes.

The following server is currently working, as seen in this demo video I made: https://youtu.be/YaQQm7aWb-Y

I'm working on the following new features:

Add the local costmap so the robot can avoid obstacles (this would be an optional parameter). In some clutter spaces, like our lab, sometimes the controller generates a trajectory directly to the tables 😅. I'm thinking that part of this behavior to avoid collisions and the trajectory visualizer can be shared from the graceful controller: by adding it to the smooth control law or share the already implemented class to avoid collisions from the controller. That way we can share code and reduce errors.
Implement some kind of recovery mecanism for when the robot loses sight of the object to follow.

As the following and docking server share a large part of the code. I'll try to extract the code into an ApproachServer class (I'm really bad with the names :/) so both server can inherit from it.

SteveMacenski · 2024-06-11T14:25:34Z

Something with the smoothness would be great too! I think the thread in ros-navigation/navigation2#4115 covers that intention a bit on the controller side. Otherwise, I think your other descriptions make sense. I actually filed a ticket when migrating this over to Nav2 (ros-navigation/navigation2#4404) on the collision checking bits, I agree that it would also be applicable here too!

This generally looks good! I think there's thing we should do to reduce code duplication across the different servers, but that's something once we have a working final prototype so we know what we need to abstract out or see if its sensible to have some common base class or something similar

ajtudela · 2024-06-11T15:04:38Z

Great. I'll keep going then.

Because the docking is ported to nav2, should I make the PR here, merge here and then port it to nav2 or make it directly to nav2?

SteveMacenski · 2024-06-11T15:43:53Z

Probably just leave it here for the moment. The Nav2 version has some refactoring that we can wait to address until we're done with a POC

ajtudela · 2024-12-02T11:02:43Z

@SteveMacenski I'd like to start finishing this. Due to recent changes in docking and the deprecation of this repo, do you mind if I move this PR to a new PR in the nav2 repo?

SteveMacenski · 2024-12-02T21:21:34Z

Sure! Though, I think some of this work is blocked by the Graceful Controller's selection of the path point to be smooth ros-navigation/navigation2#4115

So, I'd argue perhaps leaving this here until we're unblocked might be a good choice.

ajtudela · 2024-12-03T07:58:37Z

Sure! Though, I think some of this work is blocked by the Graceful Controller's selection of the path point to be smooth ros-navigation/navigation2#4115

So, I'd argue perhaps leaving this here until we're unblocked might be a good choice.

No problem, it was to make everything easier with the final steps, testing, etc.

Although any improvements made to the Graceful Controller must also be applied here, this server only uses the control law.

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela · 2025-04-25T07:44:51Z

Last month we had a convention and we tested the following, here you can see a video of the following server working:

https://www.youtube.com/watch?v=g-g58J1g9Ww

I think it's mostly finished, you can start reviewing it if you have time.

I made a demo package with instructions for a showcase.

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela · 2025-06-27T11:25:16Z

I'm addressing all the comments you left me. I'll let you know when it's ready for a second review.

Signed-off-by: Alberto Tudela <[email protected]>

opennav_following/include/opennav_following/following_exceptions.hpp

opennav_following/include/opennav_following/following_server.hpp

opennav_following_msgs/action/FollowObject.action

opennav_following/src/following_server.cpp

SteveMacenski · 2025-07-15T00:09:37Z

@ajtudela sorry for the delay, I'm steadily working through my backlog :-)

Generally looks really good! I have some comments but large strokes look great!

…ew range for error codes Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Alberto Tudela <[email protected]>

SteveMacenski · 2025-07-17T17:21:04Z

@ajtudela re-request a review when you'd like me to take a look again :-)

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela · 2025-07-18T12:57:21Z

@ajtudela re-request a review when you'd like me to take a look again :-)

I think all the comments have been addressed and it's ready for a new review.

SteveMacenski · 2025-08-05T22:12:43Z

opennav_following/src/following_server.cpp

+  const double dt = 1.0 / controller_frequency_;
+  auto target_pose = object_pose;
+  target_pose.pose.orientation = nav2_util::geometry_utils::orientationAroundZAxis(
+    tf2::getYaw(target_pose.pose.orientation) + M_PI);


So PI makes it turn 180, but always turns one way. As such, we don't fully explore the space. Should we do:

A full rotation?

Do a 90 degree left and then 180 deg right (so we do search 90 degrees left and right)? Kind of a wiggle move

I like the wiggle move. I think it looks more 'natural'.

opennav_following_msgs/action/FollowObject.action

SteveMacenski · 2025-08-05T22:15:31Z

opennav_following_bt/src/follow_object_action.cpp

+
+BT::NodeStatus FollowObjectAction::on_cancelled()
+{
+  setOutput("error_code_id", ActionResult::NONE);


total_elapsed_time set as well?

SteveMacenski · 2025-08-05T22:16:03Z

opennav_following_bt/include/opennav_following_bt/follow_object_action.hpp

+    return providedBasicPorts(
+      {
+        BT::InputPort<std::string>(
+          "pose_topic", "Topic to publish the pose of the object to follow"),


Maybe set a default for the BT node?

SteveMacenski · 2025-08-05T22:18:01Z

opennav_following/src/following_server.cpp

+{
+  RCLCPP_INFO(get_logger(), "Creating %s", get_name());
+
+  declare_parameter("controller_frequency", 50.0);


Once we approve here, this is something we'll want a docs PR for in the configuration guide + migration guide to bring this to folks' mind. If you have a nice video / gif of the following (I think you already showed some) that would be a great place to use it in the migration & configuration guides. Maybe a tutorial for how to use and important features like the pose vs TF, static obstacle timeout, rotate vs wait for target when lost, etc?

SteveMacenski · 2025-08-05T22:45:37Z

opennav_following/src/following_server.cpp

+  return false;
+}
+
+geometry_msgs::msg::PoseStamped FollowingServer::getRobotPoseInFrame(const std::string & frame)


Can this be replaced using Nav2 Util's robot utils?

SteveMacenski · 2025-08-05T22:45:52Z

opennav_following/src/following_server.cpp

+void FollowingServer::publishZeroVelocity()
+{
+  auto cmd_vel = std::make_unique<geometry_msgs::msg::TwistStamped>();
+  cmd_vel->header.stamp = now();


Maybe add the header for good measure?

SteveMacenski · 2025-08-05T22:46:30Z

opennav_following/src/following_server.cpp

+  // is not set correctly or has a lot of noise.
+  // Then, we skip the target orientation by pointing it
+  // in the same orientation than the vector from the robot to the object.
+  if (skip_orientation_) {


Note: nice idea, I agree :-)

SteveMacenski · 2025-08-05T22:48:06Z

opennav_following/src/following_server.cpp

+  }
+
+  // Filter the detected pose
+  dynamic_pose_ = filter_->update(pose);


I don't think dynamic_pose_ is used anywhere (?). detected_dynamic_pose_ is from the topic, but not the frame.

Ditto with a similar block above in getRefinedPose

SteveMacenski · 2025-08-05T22:53:33Z

opennav_following/src/following_server.cpp

+      robot_pose.pose.position.x - target_pose.pose.position.x,
+      robot_pose.pose.position.y - target_pose.pose.position.y);
+    bool reversing = allow_backward_ &&
+      dist < desired_distance_ && target_pose.pose.position.x < robot_pose.pose.position.x;


I don't quite understand this point dist < desired_distance_. Why if we're closer only to the desired distance do we allow reversing?

I might also be misunderstanding the reversing topic. Is this a temporary reversing or is this now just executing the task in the reverse direction?

Update on this?

Yes, sorry. I wanted to push all the commits and then address the comments.

The reversing option is a planned feature that will be executed when the robot is closer to the object than the required distance. This allows the robot to be pushed back to maintain the distance.

I never tried it in practice because the person was always faster than the robot, and the graceful controller parameters were set a bit low.

So I can remove it without any problems.

Signed-off-by: Alberto Tudela <[email protected]>

Co-authored-by: Steve Macenski <[email protected]>

…CMake test dependencies Signed-off-by: Alberto Tudela <[email protected]>

…t tracking Signed-off-by: Alberto Tudela <[email protected]>

…y for improved clarity Signed-off-by: Alberto Tudela <[email protected]>

…l::getCurrentPose Signed-off-by: Alberto Tudela <[email protected]>

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela · 2025-09-04T09:26:03Z

Most of the messages have been addressed. The only thing that bothers me is that this callback is never executed, and I can't understand why.

https://github.com/grupo-avispa/opennav_docking/blob/e03929a5d74aba74b428f361504ca07acd20da6e/opennav_following/src/following_server.cpp#L247-L250

ajtudela mentioned this pull request Jun 11, 2024

Nav2 Docking: Optional Collision Checking on undocking / docking ros-navigation/navigation2#4404

Closed

ajtudela force-pushed the following branch from 423f801 to 7c6fd61 Compare December 12, 2024 08:21

ajtudela added 14 commits March 3, 2025 09:24

Added following_msgs

98d4a11

Signed-off-by: Alberto Tudela <[email protected]>

Added initial dynamic following server

2fc67d4

Signed-off-by: Alberto Tudela <[email protected]>

Remove controller and link docking

c25f5c8

Signed-off-by: Alberto Tudela <[email protected]>

Rename package to opennav_following

2b5a94e

Signed-off-by: Alberto Tudela <[email protected]>

Added trajectory publisher

bac3f8a

Signed-off-by: Alberto Tudela <[email protected]>

Added following BT

46c5040

Signed-off-by: Alberto Tudela <[email protected]>

Added exceptions and improve feedback

d942982

Signed-off-by: Alberto Tudela <[email protected]>

Skip perception orientation

2a939fe

Signed-off-by: Alberto Tudela <[email protected]>

Stop, goalreached, etc

971ecdf

Signed-off-by: Alberto Tudela <[email protected]>

Remove approach

e0a8ac1

Signed-off-by: Alberto Tudela <[email protected]>

Initial demo package

f2b4cd1

Signed-off-by: Alberto Tudela <[email protected]>

Make the following server resumable.

9e930b0

Signed-off-by: Alberto Tudela <[email protected]>

Update with last changes

b57a8bb

Signed-off-by: Alberto Tudela <[email protected]>

Minor fixes in tests

7b84d8b

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela force-pushed the following branch from 1fa0a84 to 7b84d8b Compare March 3, 2025 08:24

ajtudela added 6 commits March 3, 2025 09:37

Added retries

50bc34c

Signed-off-by: Alberto Tudela <[email protected]>

Improve backward motion

b4c0f38

Signed-off-by: Alberto Tudela <[email protected]>

Add rotation recovery

7082319

Signed-off-by: Alberto Tudela <[email protected]>

Minor fixes for CMakelists

5e74049

Signed-off-by: Alberto Tudela <[email protected]>

Update tests

d62de30

Signed-off-by: Alberto Tudela <[email protected]>

Added readme to demo

79b5ef9

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela added 2 commits June 27, 2025 12:08

Remove error timeout

6d043cd

Signed-off-by: Alberto Tudela <[email protected]>

Remove initial perception

56f25f2

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela added 3 commits July 8, 2025 10:57

Remove pose from goal

7959fb1

Signed-off-by: Alberto Tudela <[email protected]>

Added optional target_frame_id

595ef70

Signed-off-by: Alberto Tudela <[email protected]>

Added error_msg

90e36f6

Signed-off-by: Alberto Tudela <[email protected]>

SteveMacenski reviewed Jul 15, 2025

View reviewed changes

opennav_following/src/following_server.cpp Outdated Show resolved Hide resolved

SteveMacenski requested changes Jul 15, 2025

View reviewed changes

opennav_following/src/following_server.cpp Outdated Show resolved Hide resolved

opennav_following/src/following_server.cpp Outdated Show resolved Hide resolved

ajtudela added 5 commits July 16, 2025 10:03

Refactor dependencies exception handling with docking exception and n…

5b3a831

…ew range for error codes Signed-off-by: Alberto Tudela <[email protected]>

Deleted dynamic param shared pointer

68326fa

Signed-off-by: Alberto Tudela <[email protected]>

Added rate to main loop

cfb52fe

Signed-off-by: Alberto Tudela <[email protected]>

Added iteration timestamp and transform tolerances

958a22a

Signed-off-by: Alberto Tudela <[email protected]>

Added static_object_timeout

2476d02

Signed-off-by: Alberto Tudela <[email protected]>

ajtudela added 3 commits July 18, 2025 14:33

Fix getPoseAtDistance

cd4bcba

Signed-off-by: Alberto Tudela <[email protected]>

Added 'search_by_rotating' param

e7a4989

Signed-off-by: Alberto Tudela <[email protected]>

Added topic to action message

1d7c4ac

Signed-off-by: Alberto Tudela <[email protected]>

SteveMacenski requested changes Aug 5, 2025

View reviewed changes

ajtudela and others added 8 commits September 2, 2025 11:13

feat: add odom_duration parameter and update odometry handling

1f699e7

Signed-off-by: Alberto Tudela <[email protected]>

Update opennav_following_msgs/action/FollowObject.action

45cfb39

Co-authored-by: Steve Macenski <[email protected]>

fix: update follow_object_action to include elapsed time and improve …

90bc82f

…CMake test dependencies Signed-off-by: Alberto Tudela <[email protected]>

feat: add target_frame parameter to rotateToObject for enhanced objec…

d011099

…t tracking Signed-off-by: Alberto Tudela <[email protected]>

fix: refactor rotateToObject to use geometry_msgs::msg::Twist directl…

b85d801

…y for improved clarity Signed-off-by: Alberto Tudela <[email protected]>

refactor: remove getRobotPoseInFrame method and replace with nav2_uti…

aab7426

…l::getCurrentPose Signed-off-by: Alberto Tudela <[email protected]>

fix: set frame_id in publishZeroVelocity

739a386

Signed-off-by: Alberto Tudela <[email protected]>

refactor: remove dynamic_pose_ variable and related methods

e03929a

Signed-off-by: Alberto Tudela <[email protected]>

Following server #43

Are you sure you want to change the base?

Following server #43

Uh oh!

Conversation

ajtudela commented Jun 7, 2024

Uh oh!

SteveMacenski commented Jun 11, 2024 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

ajtudela commented Jun 11, 2024

Uh oh!

SteveMacenski commented Jun 11, 2024

Uh oh!

ajtudela commented Dec 2, 2024

Uh oh!

SteveMacenski commented Dec 2, 2024

Uh oh!

ajtudela commented Dec 3, 2024

Uh oh!

ajtudela commented Apr 25, 2025

Uh oh!

ajtudela commented Jun 27, 2025

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Uh oh!

SteveMacenski commented Jul 15, 2025

Uh oh!

SteveMacenski commented Jul 17, 2025

Uh oh!

ajtudela commented Jul 18, 2025

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

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!

ajtudela commented Sep 4, 2025

Uh oh!

Uh oh!

SteveMacenski commented Jun 11, 2024 •

edited

Loading