Robot vision and navigation

The goal of this minitask is to develop a robot behaviour where the robot patrols between three pre-determined locations, searching for a green object at each location. If the robot finds the location, it should trigger its second behaviour and beacon towards the object until it gets close.

The deadline for this minitask is 23:59 Monday 17/11/2025. You will need to demo your code on a robot to a lab TA to get full marks.

Goals

By the end of this minitask, you should be able to:

1. Create a map for your robot
2. Use the action server to send movement goals and get updates on their completion
3. Use launch files to manage multiple ROS2 nodes
4. Understand and use ROS2 image topics and messages
5. Use OpenCV to do basic colour slicing on an image

Before you begin

This minitask requires us to set up our Gazebo environment to make things a bit easier for us. Open up the TurtleBot3 House environment in Gazebo using ros2 launch turtlebot3_gazebo turtlebot3_house.launch.py.

First, we need to make sure our robot can "see" the green trash can in the house as green. It's very dark in the house, so we'll need to add a light!

1. Run rqt in a new terminal, add an image view (Plugins -> Visualisation -> Image View), and select the /camera/image_raw topic from the drop-down.
2. In Gazebo, position your robot so it is in front of the green trash can (see image below)
3. Add a spot-light from the Gazebo toolbar (circled in red in the image)

You can position the light much like you can move your robot around, including changing its orientation. Play about with it until you're happy your robot can "see" that the trash can is green (you can always add another light, if you can't get it right with just one).

Finally, you can move your robot to its start position - use the image below for an approximate location. Task 1 has you making a map, and for speed do not drive your robot past where the red line is shown in the image below (we won't be using that half of the map for this task).

You may have noticed my Gazebo textures look kind of strange! This is a fairly common rendering bug in Gazebo, likely caused by a conflict with the graphics card driver in my machine. If this happens to you, you can use rqt to check if the robot's camera is able to see the textures correctly. In my case, it can! So we can ignore the textures in Gazebo being wrong since it's just a visual glitch and won't affect the running of the robot.

Task 1: Using actions for navigation

Part 1: making a map

In order for your robot to move autonomously, you must be able to generate a map of the world. To do this, you'll use the SLAM functionality of the ROS2 Nav2 Stack. Our turtlebots have a SLAM tool known as cartographer, you'll start by creating a map using teleop.

❗Remember to source each of your terminals❗

Make sure you have the world and launch files from the first section, then run colcon build. Then, ros2 launch turtlebot3_gazebo mt3.launch.py. You should see a lovely flat for your robot to explore.

Then, in another terminal, run: ros2 launch turtlebot3_cartographer cartographer.launch.py use_sim_time:=True. You should see rviz open.

In the rviz window, feel free to untick the TF box, as it can get in the way, then press Add, and look for the RobotModel display type. Once you've added it, find it in the left window, open up the dropdown, and set the Description Topic to /robot_description, you should now see your robot in the rviz window. Additionally, you should also see the LaserScan from the Lidar sensor (Red points). And a background map, made of 3 different colored pixels.

❓What do the three different map colours represent?

❓Where do you think the origin of this map is?

Now, to make a full map, you will need to drive your robot around. Use the teleoperation node (ros2 run turtlebot3_teleop teleop_keyboard) in a new terminal, and move around the house until you have a full map. You might want to increase the Step Size in Gazebo, or you'll be mapping for a while...

Once you are happy with your map, create a folder maps in src, and then save the map by running ros2 run nav2_map_server map_saver_cli -f src/maps/my_map. **You might need to run this command 2-3 times before it works **

Kill all your terminals before proceeding.

Part 1.5: Testing autonomous navigation

Relaunch the turtlebot house world, then, run the following command to start the turtlebot navigation node: ros2 launch turtlebot3_navigation2 navigation2.launch.py map:=src/maps/my_map.yaml. You should see your map in rviz! Then, press the 2D Pose Estimate button, and click where on the map the robot is. This is localization, where we tell the robot where it is on our map. Once you've done this, you should see some colours. Follow the same steps from above to get rid of the TF mess, and have the robot appear.

Now, press Nav2 Goal, click where ever you want your robot to go, and then watch your gazebo simulation, the robot should find its own way. Feel free to play around with this.

❓Can you tell what these colours represent?

Part 2: Telling the robot where to go in code.

❗For this minitask, you will be creating your own packages rather than pulling skeleton code from this GitHub. You will structure your code into two packages (one for navigation, one for vision) and link them using a launch file.

You will need to create the navigation package by running cd ros2_ws/src (in vscode, after you've started Docker!) and then ros2 pkg create --build-type ament_python --license Apache-2.0 --node-name nav_node nav_package. You might need to reload your VSC window for the folder to show up.

Take a look at the package structure that has been created. You will need to edit the package.xml and the setup.py files to include descriptions of the node, your names, and a few other details. You can use this page on ROS2 packages to help you understand these files.

Now in nav_node.py, you will call the nav2 action client to follow some waypoints. In your rviz window (which should still have your map) clock the Publish Point tool, and move your cursor over 5 different locations. Make a note of the (x,y,z) points that you can see in the bottom left of the screen. You'll need these later, so keep them safe in a comment or similar.

Now that we have a map, we want to control the robot using goal points, rather than using the naive /cmd_vel from the last minitask. Take a look at this and this in the documentation to understand actions.

You will need to begin by implementing an action client (like the one found here), that calls the navigate_to_pose action, with the action type from nav2_msgs.action import NavigateToPose.

Note: you only need to implement the action client!

❗ HINT #1: self._action_client = ActionClient(self, NavigateToPose, '/navigate_to_pose')

❗ HINT #2: here is the definition of NavigateToPose, you'll have to set NavigateToPose.goal() to the right thing. For now, use one of your chosen waypoints as the x,y of the goal.

❗ The header of the above message is important! You must provide the header.stamp (current ros time) and header.frame_id (take a look at the top of your rviz Global Options > Fixed Frame)

When you are done, you should be able to run ros2 run nav_package nav_node, and (as long as the rviz map is running), the robot should head to that point all by itself!

Task 2: Vision with the OpenCV bridge

In the last minitask, your had your robot perform behaviours based on the Lidar sensor. To enable your robot to react to visual cues, you'll use some simple computer vision with the OpenCV library. You will create a node that subscribes to the camera feed, processes the data, then publishes details about green objects. Similar to before, create another package (Hint: ros2 pkg create...) called eye_package with a node called eye_node. Once you've done this, open eye_node.py, this is where you will create this node.

Part 1: I love green!

At the bottom of this wiki page, you'll find some code for eye_node.py. Paste it into your file, colcon build, source, then ros2 run it. You should see something like this:

You can use rqt to see the robot's camera, by adding a "plugin -> visualisation -> image" view. Select the robot's camera topic from the drop-down. You can also view the robot's camera view in rviz by using a similar process.

❓ Use teleop to drive the turtlebot around, and see the world from it's perspective.

Currently, this node is using cv2.imshow to display the camera view. We would like to detect green objects. You need to edit your eye_node to find the centroid of green objects by colour filtering:

❗ HINT #1: filter the image first before finding the centroid of the filtered area

Display your processed image. It should look something like this:

❓ How can we make sure that what we've detected is a real green object, and not a small visual artifact?

❓ Colours are all perfect in simulation. How do you expect this to change on the real robot?

Now you have a point in the image which you know points to a green object. We want your robot to go towards the green objects (robots love the colour green) therefore we need a way to pinpoint where this object is, so we can then use our nav_node to take us there. Conveniently, our robots are equipped with calibrated depth cameras, which we can use to determine where a given pixel is in 3D space!

We are waiting on the technicians to finalise how the depth camera works in simulation. If you get this far, feel free to skip down to "On the real robot" and make your map of the arena, or fine-tune your colour detection.

Part 2: Tell the world

So far you have only subscribed to messages, however, to use our processing above, we need to publish to a topic, so nav_node can listen in. Your eye_node should publish the (x,y) coordinates of all the green objects it has found. To do this, create a publisher inside eye_node using the tutorial available.

Instead of "String", you will probably want to use the message type "Point" (you can find the structure of that message here.

When your eye_node identifies a green object and has the (x, y) coordinates from the depth camera, it should publish this information using its publisher.

Now, you need to subscribe to your eye_node inside nav_node. You can use the tutorial from before to write a basic subscriber, making sure your topic name and message type are correct. In the callback inside nav_node, you will need to do something with the point message you receive.

❓ How can you use the navigation functionality with the point you are getting? How will you need to change/adapt the point message type?

Putting it together

Launch files

Launch files are useful scripts that tell ROS2 to open multiple nodes. In fact, you've already been using some with ros2 launch, such as when you open a world file in Gazebo.

Your main function in eye_node requires the nav_node to be running, so instead of using ros2 run twice, you will create a single launchfile to do both at the same time.

Launchfiles are stored in a launch folder, which should be inside your ros2_ws/src/eye_node folder. You can use mkdir launch from the terminal when you're inside that folder to create it. Create the file by running touch mt3.launch.py inside the launch folder, then run chmod a+x mt3.launch.py.

Open that file. You should use the launchfile tutorial to populate this file with code.

Hints:

1. In their example they are launching 3 nodes, whereas you will only need to launch 2
2. The "executable" name is the same as the file/node name
3. You don't need to perform any remapping - but it's helpful to understand this as you may need it later

Add the line <exec_depend>ros2launch</exec_depend> to your eye_node's package.xml file, then cd ~/ros2_ws and colcon build.

Run the launchfile using the following syntax: ros2 launch **name_of_package** **name_of_launch_file**.

On the real robot

Part 1: make a map

Before you can run your code on the real robot, you'll need to make a map of the arena and decide on some sensible waypoints. Turn on your robot, place it in the arena (do not start mapping before your robot is in the arena), and run: ros2 launch turtlebot3_cartographer cartographer.launch.py. You should see rviz open.

❗When on a real robot, we are removing use_sim_time:=True from the cartographer command❗

Now, run teleop in another terminal and follow the process above to make your map of the arena. Don't forget to save it!

Part 2: I still love green!

Your robot will need to detect the real green objects in the arena, but you'll remember that earlier we told you colours are always perfect in simulation and not so in real life. You will need to do some tuning on your colour detection for the objects in the arena before your code will work as expected.

eye_node.py

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2

class ImageSubscriber(Node):
    def __init__(self):
        super().__init__('image_subscriber')
        self.subscription = self.create_subscription(
            Image,
            '/camera/image_raw',
            self.listener_callback,
            10)
        self.br = CvBridge()
    
    def listener_callback(self, data):
        self.get_logger().info('Receiving video frame')
    
        current_frame = self.br.imgmsg_to_cv2(data, desired_encoding='bgr8')
        
        # Do some image processing here...
        
        cv2.imshow("camera", current_frame)
        cv2.waitKey(1)

def main(args=None):
    rclpy.init(args=args)
    image_subscriber = ImageSubscriber()
    rclpy.spin(image_subscriber)
    image_subscriber.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()