feat: Add object feedback nodes and launch files

README.md

@@ -1,35 +1,142 @@
 # Eurobot-2026-VisionOnBoard
-This repository contains the vision system for the Eurobot 2026 competition.
-## Start with Docker
-### Build image and container
+
+Vision system for Eurobot 2026 competition. Supports 4 RealSense cameras with ArUco marker detection.
+
+## Quick Start
+
+### 1. Start Docker
 ```bash
 cd docker/
 bash start_docker.sh
 ```
 
-### Launch realsense node and hazelnut detector (objector_detector.cpp) together
+### 2. Launch Vision System
+
+**Option A: tmux (recommended for debugging)**
+```bash
+./start_vision_tmux.sh
 ```
-bash start_vision_tmux.sh
+- Window 0: 4 camera panes (front, back, left, right)
+- Window 1: Detector + Scanner nodes
+- Window 2: RViz
+
+**Option B: Single launch file**
+```bash
+# Inside Docker container
+source /opt/ros/humble/setup.bash
+colcon build
+source install/setup.bash
+ros2 launch aruco_object multi_camera.launch.py
 ```
 
-### Run Object Detector with Parameters
-You can run the object detector node with customizable parameters using the launch file:
+## Launch Parameters
+
+| Parameter | Default | Description |
+|-----------|---------|-------------|
+| `default_active_camera` | `right` | Initial active camera (front/back/left/right) |
+| `team_color` | `blue` | Team color (blue/yellow) |
+| `log_level` | `info` | ROS log level |
+
+Example:
 ```bash
-ros2 launch aruco_object object_detector.launch.py
+ros2 launch aruco_object multi_camera.launch.py default_active_camera:=front team_color:=yellow
 ```
-This will load parameters from `src/aruco_ros/aruco_object/config/params.yaml`.
 
-You can also override parameters from the command line:
+## Switch Active Camera
+
+Only one camera processes at a time. Publish to `/robot/dock_side` (Int16):
+
+| Side | Value |
+|------|-------|
+| Front | 0 |
+| Right | 1 |
+| Back | 2 |
+| Left | 3 |
+
+```bash
+ros2 topic pub /robot/dock_side std_msgs/msg/Int16 "data: 2" -1  # Switch to back
+```
+
+## RViz
+
 ```bash
-ros2 launch aruco_object object_detector.launch.py cluster_radius:=0.5
+xhost +local:docker  # Run BEFORE entering container
+ros2 run rviz2 rviz2
+```
+
+## Architecture
+
+```
+start_vision_tmux.sh / multi_camera.launch.py
+├── 4× RealSense cameras (640x480@15fps, depth disabled)
+├── 4× object_detector nodes (ArUco pose detection)
+└── 4× aruco_row_scanner nodes (hazelnut flip detection)
 ```
 
-### Run rviz
-Before going in the container, run this in terminal to allow allow GUI window inside container
+All nodes subscribe to `/robot/dock_side` - only the matching camera side processes images.
+
+## Individual Node Launch
+
+Launch individual camera, detector, or scanner nodes with camera position:
+
+### Camera Node (RealSense)
+
+start docker first
 ```
-xhost +local:docker
+docker exec -it vision-ws bash
 ```
-Open RViz to visualize camera images and TF frames:
+
+### Object Detector Node
 ```bash
-ros2 run rviz2 rviz2
+ros2 run aruco_object aruco_detector_node --ros-args \
+    -p camera_position:=left
+```
+
+### Row Scanner Node
+```bash
+ros2 run aruco_object aruco_row_scanner --ros-args \
+    -p camera_position:=front \
+    -p team_color:=blue
+```
+
+### Multi-Node Launch Files
+```bash
+# Launch all 4 detector nodes
+ros2 launch aruco_object detector_multi.launch.py default_dock_side:=1
+
+# Launch all 4 scanner nodes
+ros2 launch aruco_object scanner_multi.launch.py default_dock_side:=1 team_color:=blue
+```
+
+## camera serial number
+use `rs-enumerate-devices` to list the serial number
+left: 218622276534
+right: 218622276687
+back: 419122270813
+front (d435): 313522070126
+
+### launch with serial number
+```
+ros2 launch realsense2_camera rs_launch.py \
+    camera_namespace:=right \
+    camera_name:=right \
+    serial_no:="'218622276687'" \
+    rgb_camera.color_profile:=640,480,15 \
+    enable_depth:=false
+```
+```
+ros2 launch realsense2_camera rs_launch.py \
+    camera_namespace:=back \
+    camera_name:=back \
+    serial_no:="'419122270813'" \
+    rgb_camera.color_profile:=640,480,15 \
+    enable_depth:=false
+```
+```
+ros2 launch realsense2_camera rs_launch.py \
+    camera_namespace:=left \
+    camera_name:=left \
+    serial_no:="'218622276534'" \
+    rgb_camera.color_profile:=640,480,15 \
+    enable_depth:=false
 ```

docker/compose.yaml

@@ -5,7 +5,7 @@ services:
       dockerfile: Dockerfile
       args:
         USER_UID: ${USER_UID:-1000}
-    image: eurobot-2026-vision:latest
+    image: eurobot-2026-vision-onboard:latest
     container_name: vision-ws
     stdin_open: true
     tty: true
@@ -21,4 +21,4 @@ services:
       - ROS_WORKSPACE=/home/vision/vision_ws
     volumes:
       - /dev:/dev
-      - ../:/home/vision/vision_ws
+      - ../:/home/vision/vision_ws

inside_start_vision_tmux.sh

@@ -0,0 +1,101 @@
+#!/bin/bash
+
+SESSION_NAME="vision_inside"
+
+# Check if the session exists
+tmux has-session -t $SESSION_NAME 2>/dev/null
+
+if [ $? == 0 ]; then
+  # Session exists, kill it to start fresh
+  echo "Session $SESSION_NAME already exists. Killing it."
+  tmux kill-session -t $SESSION_NAME
+fi
+
+# Create a new detached session
+tmux new-session -d -s $SESSION_NAME
+
+# Get the window ID to be robust against base-index settings
+WIN_ID=$(tmux list-windows -t $SESSION_NAME -F "#{window_index}" | head -n 1)
+
+# === Window 0: Vision System (2x2 grid) ===
+# Start with Pane 0 (Top Left)
+
+# 1. Split vertically: Top (0) and Bottom (1)
+tmux split-window -v -t ${SESSION_NAME}:${WIN_ID}
+
+# 2. Split Top (0) horizontally: TL (0) and TR (1). Old Bottom becomes 2.
+tmux split-window -h -t ${SESSION_NAME}:${WIN_ID}.0
+
+# 3. Split Bottom (2) horizontally: BL (2) and BR (3).
+tmux split-window -h -t ${SESSION_NAME}:${WIN_ID}.2
+
+# Ensure tiled layout
+tmux select-layout -t ${SESSION_NAME}:${WIN_ID} tiled
+
+# Pane 0 (Top Left): All Cameras
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.0 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_multi_camera_launch.py" C-m
+
+# Pane 1 (Top Right): Detector
+DETECTOR_CMD='
+export ROS_DOMAIN_ID=13
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+echo "=== [Detector] Waiting for BACK, LEFT, RIGHT camera topics... ==="
+while true; do
+    BACK=$(ros2 topic list 2>/dev/null | grep -E "/back/back/color/camera_info")
+    LEFT=$(ros2 topic list 2>/dev/null | grep -E "/left/left/color/camera_info")
+    RIGHT=$(ros2 topic list 2>/dev/null | grep -E "/right/right/color/camera_info")
+    if [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then
+        echo "=== [Detector] All 3 cameras found! ==="
+        break
+    fi
+    echo "[Detector] Waiting... (back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})"
+    sleep 2
+done
+sleep 2
+echo "=== [Detector] Launching ArUco Detector ==="
+ros2 launch aruco_object detector_multi.launch.py
+'
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.1 "$DETECTOR_CMD" C-m
+
+# Pane 2 (Bottom Left): Scanner
+SCANNER_CMD='
+export ROS_DOMAIN_ID=13
+source /opt/ros/humble/setup.bash
+source install/setup.bash
+
+echo "=== [Scanner] Waiting for BACK, LEFT, RIGHT camera topics... ==="
+while true; do
+    BACK=$(ros2 topic list 2>/dev/null | grep -E "/back/back/color/camera_info")
+    LEFT=$(ros2 topic list 2>/dev/null | grep -E "/left/left/color/camera_info")
+    RIGHT=$(ros2 topic list 2>/dev/null | grep -E "/right/right/color/camera_info")
+    if [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then
+        echo "=== [Scanner] All 3 cameras found! ==="
+        break
+    fi
+    echo "[Scanner] Waiting... (back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})"
+    sleep 2
+done
+sleep 2
+echo "=== [Scanner] Launching ArUco Scanner ==="
+ros2 launch aruco_object scanner_multi.launch.py
+'
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.2 "$SCANNER_CMD" C-m
+
+# Pane 3 (Bottom Right): Shell / Utils
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.3 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash' C-m
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.3 'echo "Ready to use. Run: ros2 topic list"' C-m
+tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.3 'ros2 topic list' C-m
+
+# Function to kill the session when script exits
+cleanup() {
+    tmux kill-session -t $SESSION_NAME
+}
+
+# Trap EXIT signal (happens when script ends or user detaches)
+trap cleanup EXIT
+
+# Attach to the tmux session to view the output
+tmux select-window -t ${SESSION_NAME}:${WIN_ID}
+tmux attach-session -t $SESSION_NAME

src/aruco_ros/aruco_object/config/combined_params.yaml

@@ -4,15 +4,11 @@ object_detector:
     cluster_radius: 0.4
     blue_id: 36
     yellow_id: 47
-    # Camera position: "front", "back", "left", or "right"
-    camera_position: "back"
     # Smoothing factor for pose filter (0.0-1.0, lower = more smoothing)
     smooth_alpha: 0.3
 
 aruco_row_scanner:
   ros__parameters:
-    # Team color: "yellow" or "blue" (determines which IDs are targets)
-    team_color: "blue"
     blue_id: 36
     yellow_id: 47
     # Voting system: how many consistent frames needed

src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp

@@ -3,6 +3,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
+#include <std_msgs/msg/int16.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <visualization_msgs/msg/marker.hpp>
 #include <tf2_ros/transform_broadcaster.h>
@@ -11,6 +12,7 @@
 #include <opencv2/aruco.hpp>
 #include <cv_bridge/cv_bridge.h>
 #include <opencv2/opencv.hpp>
+#include <map>
 #include "aruco_cluster_detect/process_logic.hpp"
 
 class CameraOnBoardNode : public rclcpp::Node {
@@ -25,6 +27,11 @@ class CameraOnBoardNode : public rclcpp::Node {
     rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera_info_sub_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr object_pose_publisher_;
     rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub_;
+    rclcpp::Subscription<std_msgs::msg::Int16>::SharedPtr dock_side_sub_;
+
+    int16_t my_side_ = -1;           // This node's side number
+    int16_t active_side_ = -1;       // Currently active side from /robot/dock_side
+    std::map<std::string, int16_t> position_to_side_;
 
     std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
     std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
@@ -40,7 +47,7 @@ class CameraOnBoardNode : public rclcpp::Node {
     int YELLOW_ID_;
     std::string CAMERA_POSITION_;
     double SMOOTH_ALPHA_;
-    bool intrinsics_received_ = false; // because we only need to send intrinsic once
+    bool intrinsics_received_ = false;
 
     ProcessLogic logic_;
 

src/aruco_ros/aruco_object/include/aruco_cluster_detect/process_logic.hpp

@@ -25,8 +25,7 @@ class ProcessLogic {
     geometry_msgs::msg::PoseStamped compute_perpendicular_pose_from_floor_points(
         const std::vector<cv::Point2d> &floor_points,
         const cv::Point2d &camera_in_base,
-        const rclcpp::Time &now,
-        rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr marker_pub);
+        const rclcpp::Time &now);
 
 private:
     double marker_length_;

src/aruco_ros/aruco_object/launch/detector_multi.launch.py

@@ -0,0 +1,51 @@
+from launch import LaunchDescription, LaunchContext
+from launch_ros.actions import Node
+from launch.substitutions import LaunchConfiguration
+from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction
+import os
+from ament_index_python.packages import get_package_share_directory
+
+
+def launch_setup(context: LaunchContext):
+    """Launch 4 object_detector nodes"""
+    # Side mapping: 0=front, 1=right, 2=back, 3=left
+    camera_positions = ['front', 'back', 'left', 'right']
+    default_side = int(LaunchConfiguration('default_dock_side').perform(context))
+
+    if default_side not in [0, 1, 2, 3]:
+        raise Exception(f"[ERROR] Invalid dock_side: '{default_side}'. Must be 0-3.")
+
+    pkg_share = get_package_share_directory('aruco_object')
+    config_file = os.path.join(pkg_share, 'config', 'combined_params.yaml')
+    log_level = LaunchConfiguration('log_level').perform(context)
+
+    nodes = []
+
+    for position in camera_positions:
+        node = Node(
+            package='aruco_object',
+            executable='aruco_detector_node',
+            name=f'object_detector_{position}',
+            parameters=[config_file, {'camera_position': position}],
+            output='screen',
+            arguments=['--ros-args', '--log-level', log_level]
+        )
+        nodes.append(node)
+
+    # Publish initial dock_side
+    publish_cmd = ExecuteProcess(
+        cmd=['bash', '-c', 
+             f'sleep 2 && ros2 topic pub /robot/dock_side std_msgs/msg/Int16 "data: {default_side}" -1'],
+        output='screen'
+    )
+    nodes.append(publish_cmd)
+
+    return nodes
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('log_level', default_value='info'),
+        DeclareLaunchArgument('default_dock_side', default_value='1'),  # 0=front, 1=right, 2=back, 3=left
+        OpaqueFunction(function=launch_setup)
+    ])