Skip to content

Follow the Path Using the Local Map

Jump to bottom
Panagiotis Liampas edited this page Oct 29, 2025 · 8 revisions

Now that you learned how to use the YOLO detections and a PID controller to pass through pairs of buoys, let's see how we can make that more robust by using our perception & navigation stack as a whole. This will serve as the foundation for when you are working on competition tasks during the year, where we want to get the most out of our system's capabilities to achieve maximum performance.

To complete this lab, you should switch to the lab1 branch.

Local Map

Cameras only give us information about the direction in which a buoy is detected, and maybe the bounding box area can help us estimate the size of it assuming all buoys have the same size. However, we have a source of depth information: the LiDAR. Since we have the bounding boxes that are published by yolov8_node, and the filtered point cloud from point_cloud_filter, we can use both to get obstacle detetions in 3D space, in the base_link (the robot's) frame.

That is being done by bbox_project_pcloud, which subscribes to the image topic of the camera (in simulation it's /wamv/sensors/cameras/front_left_camera_sensor/image_raw), the bounding_boxes topic with the YOLO detections, the LiDAR filtered point cloud point_cloud/filtered, and the camera info topic (in sim: /wamv/sensors/cameras/front_left_camera_sensor/camera_info), which provides the camera parameters needed to project points from camera optical frame (3D) to the image plane (2D).

bbox_project_pcloud publishes the obstacle_map/local topic, which is of type ObstacleMap, and contains the set of detected obstacles in the base_link (or, in sim, wamv/wamv/base_link) frame. That's the local map we are going to use first.

ObstacleMap is a message type that's being used both for the local and the global map, and thus some fields might be empty (the local or global ones) depending on the use case, and it's structured as follows:

  • header contains the global frame id (map usually) and stamp of the detections (only in the global map)
  • local_header contains the local frame id and stamp of the detections (both in the global and the local map)
  • obstacles is a list of objects/messages of type Obstacle, which has the following fields:
    • id: the obstacle id (only useful when obstacles are being tracked, in the global map)
    • label: the YOLOv8 label given to the bounding box associated with the obstacle -> the label for red is 11 and green is 17, you are going to need that later
    • local_point/global_point: the centroid of the object's detected point cloud, in the local/global frame (map/base_link respectively)
    • stacle
    • local_chull/global_chull: the convex hull of the, flattened to 2D, object's detected point cloud, in the local/global frame
    • polygon_area: the area of the convex hull
    • bbox_min/bbox_max: the points with the minimum/maximum x,y, and z (individually), of the object's point cloud, in the local frame
    • global_bbox_min/global_bbox_max: the points with the minimum/maximum x,y, and z (individually), of the object's point cloud, in the global frame

Action Servers

The simplest way of implementing a task node is to use the publisher/subscriber node model, as you did in the previous lab. However, that gives us no way to start and stop the task execution as needed, or have any feedback (except from possibly a dedicated topic) on the progress of the task.

Therefore, we are instead making nodes host respective action servers, which can accept a certain goal and give feedback on its execution, while it can be aborted as needed. We are then using a task manager node to request the specific task actions in sequence and track their execution.

To host a task action server, we need to add the following code to __init__:

self._action_server = ActionServer(
    self,
    Task,
    "follow_buoy_path",
    execute_callback=self.execute_callback,
    cancel_callback=self.default_cancel_callback,
)

and then implement the following functions:

def execute_callback(self, goal_handle):
  self.start_process("Follow buoy path started!")

  # SETUP CODE

  while not self.result:
    # Check if we should abort/cancel if a new goal arrived
    if self.should_abort():
        self.end_process("New request received. Aborting path following.")
        goal_handle.abort()
        return Task.Result()

    if goal_handle.is_cancel_requested:
        self.end_process("Cancel requested. Aborting path following.")
        goal_handle.canceled()
        return Task.Result()
    
    # CONTROL LOOP CODE

    time.sleep(self.timer_period)
  
  self.end_process("Follow buoy path completed!")
  goal_handle.succeed()
  return Task.Result(success=True)
def default_cancel_callback(self, cancel_request):
  """Canceling has no extra functionalities by default"""
  return CancelResponse.ACCEPT

You should set self.result to False before the while loop, and then set it to True when you want the while loop to stop.

To make our lives easier, we are going to make the task nodes inherit from the ActionServerBase class. This class has the start_process, end_process, and default_cancel_callback functions implemented, as well as should_abort and , so you can focus on just implementing the control loop of the action server. It also provides a get_robot_pose function that returns a tuple with the x and y position and the heading of the robot (in this order).

The task manager node (run_tasks) currently runs each task in sequence, using the following piece of code that's in __init__:

self.task_list = [
  ActionClient(self, Task, "task_init"), # runs before all the tasks
  ActionClient(self, Task, "follow_buoy_path"),
]

In order to switch between tasks (start the first task or go to the next task), press p.

Going through a pair of buoys using the local map

So, the goal of this task is the same as part 1 of the lab, but you are going to use the local map (ObstacleMap) published in the obstacle_map/local topic, instead of the YOLO bounding boxes, to navigate to that pair.

The way this is going to be done is the following:

  1. You need to subscribe to the obstacle_map/local topic, and each time you get a new local map, identify the pair of buoys you want to go through (hint: just pick the closest ones of red and green color that are in front of the robot)
  2. Now you have the positions of the buoys in the robot's frame, you want to navigate through them, so just aim for the midpoint
  3. For the control loop, you are going to implement a PID controller, like you did in the waypoint following part of the previous lab. However, you now have target coordinates in the robot's frame, in 2D space (ignore the z component). You are still going to use a PID controller (or many?), but in a slightly different way this time. You should publish the output velocity as a ControlOption message to the control_options topic (fill out the twist field, of type Twist, with the desired velocity, and set priority to 1).

Editing and running the code

The skeleton code for the action server is in follow_path_local.py, and you should fill out the parts that are needed to make the logic for the task node actually work.

To run the code, use the same launch command as the previous lab, but because we are now using the task manager, you need to press p to switch to the next task (which is "follow the path"). Of course, you need to turn off teleop (press Enter to make the robot move autonomously).

Steps to success

Here's a couple of things you need to think through before you start coding:

  • Take it one step at a time: buoy picking, waypoint computation, error computation, PID controller setup & fine-tuning.
  • Which fields are you going to use to get the detected buoys positions? check the message description above, and the respective ROS2 message types and descriptions, because ROS2 messages usually have a lot of nesting of various message types.
  • This time your goal is to reach a certain position with the robot, but that position is in the robot's frame. That actually makes things quite easy for you, because when your robot is to the left of the buoy, the buoy is to the right of the robot, and you want to go right, so there is a direct correlation of the x component of the buoy and the x velocity you want to have. Same for y. How are you going to make a PID controller that takes into account those errors to go where you want it (watch out for the signs, think of what the PID controller will do when x/y is positive/negative)?
  • Does the robot angle play a role in this task? You can get creative, think about the trajectory the robot to take in the case where: a) you don't set an angle error to the PID, b) you set the target angle to be dynamically set to the direction from the robot to the midpoint, or c) you set the target angle to be the robot facing perpendicular to the buoy pair (forwards) when it reaches it. Or you can do something different.
  • What should you do when you only see one of the buoys? The controller will definitely not be as accurate as when you see both of them, but maybe you can make some assumptions about the buoys' relative position to keep the robot moving in the correct direction (and recover, if it's gone too far right or left).
  • Parametrize: that's a good ROS coding practice, and really useful when making competition-ready code, because you can easily tune the code without changing hardcoded values in different places. You can see how to use parameters in the init function of the skeleton code.

You don't have to worry about termination conditions for now, just get the control loop working first (you can just make the controller do nothing, or send a stop command, when it doesn't see any of the two buoys).

The end result should look like this:

Screencast.from.08-30-2025.07.49.54.PM.webm

Debugging tips:

  • Use RViz markers to your advantage. They can represent arbitrary points (or even directions, if it's an arrow marker) in space, so when the controller seems to not be working properly, verify that you are setting the correct target and picking the correct buoys. Whenever you are setting a single marker (or have a predefined set of markers for certain points that dynamically change, like a waypoint for example) you can use a Marker message, otherwise (when you have an arbitrary number of markers) use a MarkerArray. In the latter case, pay attention to setting a different id to each marker, and to clearing all the markers before sending new ones (by setting a DELETALL action to the first marker of the message, or sending a separate marker array with a single marker with that action before the new array of markers, or you can set a lifetime to the markers which might be preferrable). TODO: Add an example marker/marker array publishing code to not have to debug markers.
  • Check the signs of the PID controller (error sign vs effort sign in various cases), and start with just the $K_p$ coefficient ($K_i$ and $K_d$ set to 0), then you can try making it smoother by adding a $K_d$ coefficient and if there's a steady state offset (robot constantly undercorrecting) you can add a small $K_i$ coefficient.

Clone this wiki locally