Color Tracking (No-PTZ)

1. Experiment Objective

The camera detects an object of a specified color, and the car rotates to track it so that it stays at the center of the frame. It supports Tracking mode (rotation only) and Follow mode (rotation + distance keeping).

2. Experiment Principle

1. Keyword Explanation

2. Technical Architecture

Dual-timer architecture: image processing and body control are decoupled.

/camera/image_raw (BEST_EFFORT)
  → color_tracker_node
    ├── Image processing: image conversion → flip → HSV binarization → morphology → largest contour → centroid calculation
    └── Body control timer (10Hz): pixel error → PD angular velocity + color block size distance following → /cmd_vel (BEST_EFFORT)

Mouse HSV sampling: Drag to select an ROI with the mouse → take the 5th–95th percentile HSV → automatically update the thresholds → lock the target color.

Tracking mode vs Follow mode:

• track: enable_distance_follow:=false, rotation tracking with PD only
• follow: enable_distance_follow:=true, rotation tracking + distance following

3. Core Topic Quick Reference

Firmware Uplink (Sensor Data)

Firmware Downlink (Control Commands)

3. Experiment Steps

1. Hardware Preparation

2. Start the Agent + Camera + Joint Model (Terminal 1)

xxxxxxxxxx
ros2 launch microros_v2_bringup car_base.launch.py

3. Start Color Tracking (Terminal 2)

Tracking mode (rotation tracking only):

xxxxxxxxxx
ros2 launch microros_v2_no_ptz_visual_control_pkg color_tracker.launch.py

4. Operation Instructions

4. Experiment Phenomenon

In Tracking mode:

• When the target moves left or right in the frame, the car automatically rotates to track it, keeping the target within the center deadband of the frame
• When the target is stationary, the car also stays stationary (zero-velocity command)
• After drag-selecting objects of different colors with the mouse, the HSV thresholds update automatically, enabling color switching

In Follow mode:

• While rotating to track, the car adjusts its distance to the target based on the size of the target color block
• When the target moves away, the car moves forward to approach it; when the target moves closer, the car backs away

When the target is lost:

• After consecutive lost frames exceed the threshold, the car stops moving and displays "Target:None"
• When the target reappears in the frame, tracking resumes automatically

5. Code Explanation

Source path:

• Launch (track): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/color_tracker.launch.py
• Launch (follow): ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/color_follow.launch.py
• Node source: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/color_tracker_node.py

1. Core Node Analysis

color_tracker_node uses a dual-timer architecture similar to follow_line_node. Core workflow:

• Image caching: the callback only caches the latest raw Image frame
• Image processing timer: image conversion → flip → HSV binarization → morphological opening/closing → largest contour extraction → centroid + radius calculation
• Body control timer (10Hz): horizontal pixel error → PD computes angular velocity + color block radius deviation → PD computes linear velocity → /cmd_vel

Constructor — declare parameters, create publishers/subscribers/timers:

​x
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/color_tracker_node.py
​
def __init__(self) -> None:
    super().__init__(DEFAULT_NODE_NAME)
    self._tgt_lock = threading.Lock()
    self._latest_frame: Optional[np.ndarray] = None
​
    self._state = STATE_IDLE
    self._shutdown = False
​
    # Target state: centroid coordinates, color block radius, lost-frame count
    self._has_target = False
    self._lost_frames = 0
    self._target_x: Optional[float] = None
    self._target_y: Optional[float] = None
    self._target_r: Optional[float] = None
​
    # Mouse HSV sampling state
    self._mouse_selecting = False
    self._roi_start = (0, 0)
    self._roi_end = (0, 0)
    self._roi_pending = False
​
    self._declare_params()
    self._load_params()
​
    # Image subscription (BEST_EFFORT, KEEP_LAST, depth=1)
    best_effort = QoSProfile(
        history=QoSHistoryPolicy.KEEP_LAST, depth=1,
        reliability=QoSReliabilityPolicy.BEST_EFFORT,
        durability=QoSDurabilityPolicy.VOLATILE,
    )
    self._img_sub = self.create_subscription(
        Image, self._img_topic, self._on_image, best_effort)
    self._cmd_pub = self.create_publisher(Twist, self._cmd_topic, best_effort)
​
    # Dual timers: image processing + body control
    self._img_timer = self.create_timer(
        1.0 / max(float(self._img_rate), MINIMUM_TIMER_RATE_HZ), self._process_image)
    self._body_timer = self.create_timer(
        1.0 / max(float(self._body_rate), MINIMUM_TIMER_RATE_HZ), self._body_tick)
​
    self._param_cb = self.add_on_set_parameters_callback(self._on_param_update)
​
    if self._show_win:
        self._ensure_win()

Body control timer callback — PD angular velocity + distance following:

xxxxxxxxxx
# Source file: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/color_tracker_node.py
​
def _body_tick(self) -> None:
    """Body control timer (10Hz)."""
    if self._state != STATE_TRACKING or self._shutdown:
        self._pub_zero()
        return
​
    with self._tgt_lock:
        has = self._has_target
        tx = self._target_x
        tr = self._target_r
​
    if not has or tx is None:
        self._pub_zero()
        return
​
    # Horizontal pixel error → PD angular velocity
    cx = self._img_w * 0.5
    err_x = tx - cx
    if abs(err_x) <= self._deadband_x:
        az = 0.0
    else:
        now = time.perf_counter()
        dt = now - self._prev_t if self._prev_t > 0 else 1.0 / self._body_rate
        dt = max(dt, 1e-3)
        d_err = (err_x - self._prev_err) / dt
        self._prev_err = err_x; self._prev_t = now
        az = self._angular_kp * err_x + self._angular_kd * d_err
        az = clamp(az, -self._max_az, self._max_az)
        if self._reverse_body:
            az = -az
​
    cmd = Twist()
    cmd.angular.z = az
​
    # Distance following: color block radius deviation → PD linear velocity
    if self._dist_follow and tr is not None:
        err_r = self._target_r_px - tr
        if abs(err_r) > self._dist_tol_r:
            now = time.perf_counter()
            dt = now - self._prev_dt if self._prev_dt > 0 else 1.0 / self._body_rate
            dt = max(dt, 1e-3)
            d_err_r = (err_r - self._prev_derr) / dt
            self._prev_derr = err_r; self._prev_dt = now
            lx = self._dist_kp * err_r + self._dist_kd * d_err_r
            lx = clamp(lx, -self._max_lx, self._max_lx)
            if self._reverse_linear:
                lx = -lx
            cmd.linear.x = lx
​
    self._cmd_pub.publish(cmd)

Key logic: After drag-selecting an ROI with the mouse, _roi_pending triggers _update_hsv_from_roi() — it takes the 5th–95th percentile values of the H/S/V channels within the ROI, expands them with the h_expand/s_expand/v_expand tolerances, updates the HSV thresholds, and writes them to the parameter server.

2. Key Parameters

Parameter definition file:

• Launch: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/launch/color_tracker.launch.py
• Node: ~/microros_ws/src/microros_v2_no_ptz_visual_control_pkg/microros_v2_no_ptz_visual_control_pkg/color_tracker_node.py

3. Node Description

6. Common Problems