Follow the Path Using the Navigation Stack
The fundamental subsystems of our navigation stack are the localization, mapping, planning, and control nodes.
A lot of the components described here are based on the ROS2 TF (transform) tree, which can be used to edit and access the relative location between the robot's sensors and the robot's location in the world. We have made some slides you can refer to to learn more about the TF tree, as it's pretty essential in general.
Our localization subsystem, which is enabled by the GPS and Pixhawk and enhanced using SLAM (which also performs the mapping, described below), provides the map->base_link transform (in the TF tree), providing the robot's position in the global frame, and the odometry/gps topic (or odometry/tracked, which includes the SLAM position estimates), which is of type nav_msgs/Odometry and includes position and velocity estimates.
We construct a consistent map of the environment, estimating the position of the surrounding buoys, using the object_tracking_map node, which also performs SLAM. It subscribes to the obstacle_map/local topic, as well as the localization estimates (both position and velocity), and computes a global map of the buoys. The node publishes the obstacle_map/global topic, which is of type ObstacleMap, like the local map, but includes the global coordinates of the buoys, which should be consistent over time. This is a sparse map, and has a representation that's convenient when performing tasks that involve buoys or other small static objects in the competition course.
We also construct a different type of map, a grid map, that's essential when planning a trajectory from the robot's current position to a given waypoint without crashing onto obstacles. This map is given by the grid_map_generator node, which uses raw LiDAR clusters (filtered to have a clean and usable map) and the robot's estimated position publishes the respective /dynamic_map topic, of type nav_msgs/msg/OccupancyGrid.
The grid map is being used by the navigation_server node, which runs an action server with name follow_path, meaning that it accepts requests (with a goal of type FollowPath) to compute (using the A* algorithm) and follow a trajectory from the robot's position to a given goal point without crashing on any obstacles, and then sends waypoint (position and orientation) requests to be followed by the PID controller.
The PID controller is the controller_server node, hosting the waypoint action server that accepts goals of type Waypoint, which computes the linear and angular velocity that's needed to reach the given position and orientation and publishes the cmd_vel topic, of type geometry_msgs/Twist, accordingly. The XDrive controller subscribes to that topic and converts the velocity to thruster values, enabling holonomic motion.
Now that you know how to use the ObstacleMap message and deal with buoys' positions in 2D, let's use the Arcturus localization, mapping, and navigation stack to achieve better results. Notably:
- You will subscribe to the
obstacle_map/globaltopic, which is the same type asobstacle_map/local(ObstacleMap), but it now has the fields related to the buoys' position in the global frame populated as well. So, now, instead of using the local frame coordinates, you are using global ones. - In order to pick the buoys and navigate to them, you also need to know the robot's position. You can do that using the provided
self.get_robot_pose()function of the base action server class (which your node inherits from), and it will give you an (x,y,heading) tuple with the robot's position and orientation in the global frame. - Now that you have the robot's and detected buoys' positions, how are you going to pick the buoys you want to go through? You need to make some assumptions (hint: in front of the robot, closest to it, right green, left red)
- Using the picked buoys' positions, you can easily compute the waypoint position like you did in the last part (just x,y position, no need to compute the orientation here). Then, you have all the information you need to send to the A* planner node, so you just make a
follow_pathservice request (of action typeFollowPath) using that information and setting the parameters appropriately.
In order to make a follow_path request, you should use the following code, filling out the required fields appropriately:
self.follow_path_client = ActionClient(self, FollowPath, "follow_path")in init, and
self.follow_path_client.wait_for_server()
goal_msg = FollowPath.Goal()
goal_msg.planner = "astar"
goal_msg.x = # x coordinate of goal point
goal_msg.y = # y
goal_msg.theta = # theta
goal_msg.xy_threshold = # x & y offset from goal to be considered reached (for controller)
goal_msg.theta_threshold = # acceptable theta offset from goal (controller)
goal_msg.goal_tol = # acceptable x & y offset for planner
goal_msg.obstacle_tol = # minimum grid cell value in map to be considered occupied
goal_msg.choose_every = # number of points to skip to downsample path
goal_msg.is_stationary = # whether we want to stationkeep at the last waypoint
self.follow_path_client.wait_for_server()
self.send_goal_future = self.follow_path_client.send_goal_async(
goal_msg
)in the control loop.
The code you are given includes the send_waypoint_to_server function as a helper function that takes in a waypoint (tuple with the x and y coordinates) and makes the required follow_path request.
For now, you don't have to worry about termination conditions and transitions between buoys, that'll be the next (and last) part of the lab.
To edit and run the code, you do the same as in the previous part. Only difference is you need to comment out the line with the local "follow the path" node in the task manager node and uncomment the one with the respective node that uses the global map.
- This time you don't have a control loop, like when you did the PID controller. The control loop is handled by the PID controller node, so you just send the waypoint once (or maybe more than once if the goal failed for some reason, then you do the same computation again and send a new waypoint).
- To check if a buoy is in front of the robot, there are two ways to do it. You now have both the local and global coordinates of the buoys detected, so you can use either of those, but one of them might be harder, so think about it before starting to code (you also have the robot's position and orientation). Think about the robot's coordinate frame, and keep in mind that, in that frame (not the global frame), x is forwards.
- As before, use informative markers, and publish info messages with the action server's status and maybe some computation/selection steps.
In the competition you don't have just a single pair of buoys, but instead need to go through a path with many pairs of green and red buoys. Therefore, although the idea is the same as in the previous sections, you need to detect when you've passed one pair of buoys and need to compute a new waypoint corresponding to the next pair (if it exists). You'll change the code you wrote in the previous section to handle that.
When you don't see any more buoy pairs in front, don't do anything. When that's the case for more than 5 seconds, send a goal success response to the task server.
The end result should look like this:
Screencast.from.09-02-2025.12.54.14.PM.webm
Same as the previous parts.
- You know which buoy is left and which buoy is right from when you compute them. Given that information, and assuming that you have a function that tells you, given three points in order, whether they form a clockwise or a counterclockwise angle, how can you compute whether the robot has passed the selected buoy pair? That function is given in the skeleton code of the last section.
- Markers and info messages are your friends in case of uncertainty, don't be afraid to use them. Computation/selection steps and action server status is where most of the confusion happens.
If you are done, congratulations on completing the Arcturus Autonomy Onboarding Lab! You may talk to one of the leads (Panos or Brendon) about picking up a task and getting to actual work.