Vision Tracking with Depth

This tutorial guides you through creating a vision tracking system using a depth camera. We leverage RGBD with the VisionRGBDFollower in Kompass to detect and follow objects more robustly. With depth information available, this creates a more precise understanding of the environment and leads to more accurate and robust object following compared to using RGB images alone.

Before You Start

Setup Your Depth Camera ROS 2 Node

Your robot needs a depth camera to see in 3D and get the RGBD input. For this tutorial, we are using an Intel RealSense that is available on many mobile robots and well supported in ROS 2 and in simulation.

To get your RealSense camera running:

sudo apt install ros-<ros2-distro>-realsense2-camera

# Launch the camera node to start streaming both color and depth images
ros2 launch realsense2_camera rs_camera.launch.py

Start vision detection using an ML model

To implement and run this example we will need a detection model processing the RGBD camera images to provide the Detection information. Similarly to the RGB tutorial, we will use EmbodiedAgents. It provides a Vision Component which will allow us to easily deploy a ROS node in our system that interacts with vision models.

Step 1: Vision Component and Model Client

In this example, we will set enable_local_classifier to True in the vision component so the model would be deployed directly on the robot. Additionally, we will set the input topic to be the RGBD camera topic. This setting will allow the Vision component to publish both the depth and the RGB image data along with the detections.

from agents.components import Vision
from agents.config import VisionConfig
from agents.ros import Topic

image0 = Topic(name="/camera/rgbd", msg_type="RGBD")
detections_topic = Topic(name="detections", msg_type="Detections")

detection_config = VisionConfig(threshold=0.5, enable_local_classifier=True)
vision = Vision(
    inputs=[image0],
    outputs=[detections_topic],
    trigger=image0,
    config=detection_config,
    component_name="detection_component",
)

See also

See all available VisionModel options in the Models reference, and all available model clients in the Clients reference.

Step 2: Robot Configuration

You can set up your robot in the same way we did in the RGB tutorial. Here we use an Ackermann model as an example:

from kompass.robot import (
    AngularCtrlLimits,
    LinearCtrlLimits,
    RobotGeometry,
    RobotType,
    RobotConfig,
)
import numpy as np

# Setup your robot configuration
my_robot = RobotConfig(
    model_type=RobotType.ACKERMANN,
    geometry_type=RobotGeometry.Type.CYLINDER,
    geometry_params=np.array([0.1, 0.3]),
    ctrl_vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=3.0, max_decel=2.5),
    ctrl_omega_limits=AngularCtrlLimits(
        max_vel=4.0, max_acc=6.0, max_decel=10.0, max_steer=np.pi / 3
    ),
)

Step 3: Controller with VisionRGBDFollower

Now we set up the Controller component to use the VisionRGBDFollower. In Kompass the controller selects between path-following and vision-following modes via its algorithm, and VisionRGBDFollower and VisionRGBFollower are sibling controllers that live behind ControllersID.VISION_DEPTH and ControllersID.VISION_IMG respectively.

Compared to the RGB version, the RGBD path needs two additional inputs:

  • The detections topic from the vision component

  • The depth camera info topic for depth-to-3D projection

from kompass.components import Controller, ControllerConfig
from kompass.control import ControllersID

depth_cam_info_topic = Topic(name="/camera/aligned_depth_to_color/camera_info", msg_type="CameraInfo")

config = ControllerConfig(ctrl_publish_type="Parallel")
controller = Controller(component_name="controller", config=config)
controller.algorithm = ControllersID.VISION_DEPTH
controller.inputs(vision_detections=detections_topic, depth_camera_info=depth_cam_info_topic)

Step 4: Helper Components

To make the system more complete and robust, we add:

  • DriveManager – to handle sending direct commands to the robot and ensure safety with its emergency stop

  • LocalMapper – to provide the controller with more robust local perception; to do so we also set the controller’s direct_sensor property to False

from kompass.components import DriveManager, LocalMapper

controller.direct_sensor = False
driver = DriveManager(component_name="driver")
mapper = LocalMapper(component_name="local_mapper")

Full Recipe Code

vision_depth_follower.py
 1from agents.components import Vision
 2from agents.config import VisionConfig
 3from agents.ros import Topic
 4from kompass.components import Controller, ControllerConfig, DriveManager, LocalMapper
 5from kompass.control import ControllersID
 6from kompass.robot import (
 7    AngularCtrlLimits,
 8    LinearCtrlLimits,
 9    RobotGeometry,
10    RobotType,
11    RobotConfig,
12)
13from kompass.ros import Launcher
14import numpy as np
15
16
17image0 = Topic(name="/camera/rgbd", msg_type="RGBD")
18detections_topic = Topic(name="detections", msg_type="Detections")
19
20detection_config = VisionConfig(threshold=0.5, enable_local_classifier=True)
21vision = Vision(
22    inputs=[image0],
23    outputs=[detections_topic],
24    trigger=image0,
25    config=detection_config,
26    component_name="detection_component",
27)
28
29# Setup your robot configuration
30my_robot = RobotConfig(
31    model_type=RobotType.ACKERMANN,
32    geometry_type=RobotGeometry.Type.CYLINDER,
33    geometry_params=np.array([0.1, 0.3]),
34    ctrl_vx_limits=LinearCtrlLimits(max_vel=1.0, max_acc=3.0, max_decel=2.5),
35    ctrl_omega_limits=AngularCtrlLimits(
36        max_vel=4.0, max_acc=6.0, max_decel=10.0, max_steer=np.pi / 3
37    ),
38)
39
40depth_cam_info_topic = Topic(name="/camera/aligned_depth_to_color/camera_info", msg_type="CameraInfo")
41
42# Setup the controller
43config = ControllerConfig(ctrl_publish_type="Parallel")
44controller = Controller(component_name="controller", config=config)
45controller.algorithm = ControllersID.VISION_DEPTH
46controller.inputs(vision_detections=detections_topic, depth_camera_info=depth_cam_info_topic)
47controller.direct_sensor = False
48
49# Add additional helper components
50driver = DriveManager(component_name="driver")
51mapper = LocalMapper(component_name="local_mapper")
52
53# Bring it up with the launcher
54launcher = Launcher()
55launcher.add_pkg(
56    components=[vision],
57    package_name="automatika_embodied_agents",
58    multiprocessing=True,
59    ros_log_level="warn",
60)
61launcher.add_pkg(
62    components=[controller, mapper, driver],
63    package_name="kompass",
64    multiprocessing=True,
65)
66# Set the robot config for all components
67launcher.robot = my_robot
68launcher.bringup()

Tip

You can take your design to the next step and make your system more robust by adding some events or defining some fallbacks.

Tip

Promote this recipe to production. While you’re shaping it, the script runs straight with python recipe.py. Once it’s solid, drop it at ~/emos/recipes/<your_name>/recipe.py and run emos run <your_name> – you’ll get sensor pre-flight checks, persistent logs, and a card on the dashboard so an operator can launch it from a browser. See Running Recipes for the full development-vs-production comparison and install-mode pitfalls (especially in Container mode).