From 0e3d9149f865e414837aa5621fc020a6db1eab70 Mon Sep 17 00:00:00 2001 From: CYHsiang Date: Fri, 27 Feb 2026 18:30:31 +0800 Subject: [PATCH 01/18] feat: Add object feedback nodes and launch files --- src/object_feedback/CMakeLists.txt | 71 ++++++++++ .../launch/obj_fb_bringup.launch.py | 133 ++++++++++++++++++ .../launch/single_feedback.launch.py | 37 +++++ src/object_feedback/package.xml | 28 ++++ src/object_feedback/src/feedback_all.cpp | 94 +++++++++++++ src/object_feedback/src/object_feedback.cpp | 80 +++++++++++ 6 files changed, 443 insertions(+) create mode 100644 src/object_feedback/CMakeLists.txt create mode 100644 src/object_feedback/launch/obj_fb_bringup.launch.py create mode 100644 src/object_feedback/launch/single_feedback.launch.py create mode 100644 src/object_feedback/package.xml create mode 100644 src/object_feedback/src/feedback_all.cpp create mode 100644 src/object_feedback/src/object_feedback.cpp diff --git a/src/object_feedback/CMakeLists.txt b/src/object_feedback/CMakeLists.txt new file mode 100644 index 0000000..f57b52c --- /dev/null +++ b/src/object_feedback/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.8) +project(object_feedback) + +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(OpenCV REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(tf2_eigen REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) + + +include_directories( + include + ${OpenCV_INCLUDE_DIRS} +) + +add_executable(object_feedback_node + src/object_feedback.cpp +) + +add_executable(feedback_all_node + src/feedback_all.cpp +) + +ament_target_dependencies(object_feedback_node + rclcpp + sensor_msgs + std_msgs + cv_bridge + tf2_ros + tf2_geometry_msgs + tf2_eigen + geometry_msgs + visualization_msgs +) + +ament_target_dependencies(feedback_all_node + rclcpp + sensor_msgs + std_msgs + cv_bridge + tf2_ros +) + +target_link_libraries(object_feedback_node + ${OpenCV_LIBRARIES} +) + +install(TARGETS + object_feedback_node + feedback_all_node + DESTINATION lib/${PROJECT_NAME} +) + +# Install headers +install(DIRECTORY include/ + DESTINATION include +) + +# Install launch and config +install(DIRECTORY launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/object_feedback/launch/obj_fb_bringup.launch.py b/src/object_feedback/launch/obj_fb_bringup.launch.py new file mode 100644 index 0000000..0d86131 --- /dev/null +++ b/src/object_feedback/launch/obj_fb_bringup.launch.py @@ -0,0 +1,133 @@ + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + +ARGUMENTS = [ + # Front feedback arguments + DeclareLaunchArgument( + "front_depth_topic", + default_value="/front/front/depth/image_rect_raw", + description="Front depth image topic subscribed by object_feedback_front", + ), + DeclareLaunchArgument( + "front_dist_topic", + default_value="/front/front/obj_distance", + description="Front distance topic published by object_feedback_front", + ), + + # Right feedback arguments + DeclareLaunchArgument( + "right_depth_topic", + default_value="/right/right/depth/image_rect_raw", + description="Right depth image topic subscribed by object_feedback_right", + ), + DeclareLaunchArgument( + "right_dist_topic", + default_value="/right/right/obj_distance", + description="Right distance topic published by object_feedback_right", + ), + + # Back feedback arguments + DeclareLaunchArgument( + "back_depth_topic", + default_value="/back/back/depth/image_rect_raw", + description="Back depth image topic subscribed by object_feedback_back", + ), + DeclareLaunchArgument( + "back_dist_topic", + default_value="/back/back/obj_distance", + description="Back distance topic published by object_feedback_back", + ), + + # Left feedback arguments + DeclareLaunchArgument( + "left_depth_topic", + default_value="/left/left/depth/image_rect_raw", + description="Left depth image topic subscribed by object_feedback_left", + ), + DeclareLaunchArgument( + "left_dist_topic", + default_value="/left/left/obj_distance", + description="Left distance topic published by object_feedback_left", + ), + + # Feedback all node arguments + DeclareLaunchArgument( + "success_topic", + default_value="/robot/vision/onTakeSuccess", + description="Success topic published by feedback_all_node", + ), + DeclareLaunchArgument( + "distance_threshold_mm", + default_value="150", + description="Distance threshold in mm for feedback_all_node to determine success", + ), +] + + +def generate_launch_description(): + + object_feedback_front = Node( + package="object_feedback", + executable="object_feedback_node", + name="object_feedback_front", + parameters=[ + {"depth_topic": LaunchConfiguration("front_depth_topic")}, + {"dist_topic": LaunchConfiguration("front_dist_topic")}, + ], + ) + + object_feedback_right = Node( + package="object_feedback", + executable="object_feedback_node", + name="object_feedback_right", + parameters=[ + {"depth_topic": LaunchConfiguration("right_depth_topic")}, + {"dist_topic": LaunchConfiguration("right_dist_topic")}, + ], + ) + + object_feedback_back = Node( + package="object_feedback", + executable="object_feedback_node", + name="object_feedback_back", + parameters=[ + {"depth_topic": LaunchConfiguration("back_depth_topic")}, + {"dist_topic": LaunchConfiguration("back_dist_topic")}, + ], + ) + + object_feedback_left = Node( + package="object_feedback", + executable="object_feedback_node", + name="object_feedback_left", + parameters=[ + {"depth_topic": LaunchConfiguration("left_depth_topic")}, + {"dist_topic": LaunchConfiguration("left_dist_topic")}, + ], + ) + + feedback_all = Node( + package="object_feedback", + executable="feedback_all_node", + name="feedback_all_node", + parameters=[ + {"front_dist_topic": LaunchConfiguration("front_dist_topic")}, + {"right_dist_topic": LaunchConfiguration("right_dist_topic")}, + {"back_dist_topic": LaunchConfiguration("back_dist_topic")}, + {"left_dist_topic": LaunchConfiguration("left_dist_topic")}, + {"success_topic": LaunchConfiguration("success_topic")}, + ], + ) + + ld = LaunchDescription(ARGUMENTS) + + ld.add_action(object_feedback_front) + ld.add_action(object_feedback_right) + ld.add_action(object_feedback_back) + ld.add_action(object_feedback_left) + ld.add_action(feedback_all) + + return ld diff --git a/src/object_feedback/launch/single_feedback.launch.py b/src/object_feedback/launch/single_feedback.launch.py new file mode 100644 index 0000000..952599a --- /dev/null +++ b/src/object_feedback/launch/single_feedback.launch.py @@ -0,0 +1,37 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +def generate_launch_description(): + + depth_topic_arg = DeclareLaunchArgument( + "depth_topic", + default_value="/camera/camera/depth/image_rect_raw", + description="Depth image topic subscribed by object_feedback_node", + ) + + dist_topic_arg = DeclareLaunchArgument( + "dist_topic", + default_value="/camera/camera/obj_distance", + description="Distance topic published by object_feedback_node", + ) + + # Launch single side feedback node + launch_fb = Node( + package="object_feedback", + executable="object_feedback_node", + name="object_feedback_node", + parameters=[ + {"depth_topic": LaunchConfiguration("depth_topic")}, + {"dist_topic": LaunchConfiguration("dist_topic")}, + ], + ) + + ld = LaunchDescription() + ld.add_action(depth_topic_arg) + ld.add_action(dist_topic_arg) + ld.add_action(launch_fb) + + return ld \ No newline at end of file diff --git a/src/object_feedback/package.xml b/src/object_feedback/package.xml new file mode 100644 index 0000000..89e658a --- /dev/null +++ b/src/object_feedback/package.xml @@ -0,0 +1,28 @@ + + + object_feedback + 0.0.2 + ROS2 C++ node for detecting ArUco markers and publishing object pose. + + Sean Chen + Apache-2.0 + + ament_cmake + + rclcpp + sensor_msgs + std_msgs + cv_bridge + tf2 + tf2_ros + tf2_geometry_msgs + tf2_eigen + geometry_msgs + visualization_msgs + + opencv + + + ament_cmake + + diff --git a/src/object_feedback/src/feedback_all.cpp b/src/object_feedback/src/feedback_all.cpp new file mode 100644 index 0000000..d89ef6b --- /dev/null +++ b/src/object_feedback/src/feedback_all.cpp @@ -0,0 +1,94 @@ +#include +#include +#include + + +class FeedbackAll : public rclcpp::Node { +public: + FeedbackAll() : Node("feedback_all_node") { + + this->declare_parameter("left_topic", "/left/left/obj_distance"); + this->declare_parameter("right_topic", "/right/right/obj_distance"); + this->declare_parameter("front_topic", "/front/front/obj_distance"); + this->declare_parameter("back_topic", "/back/back/obj_distance"); + this->declare_parameter("success_topic", "/robot/vision/onTakeSuccess"); + this->declare_parameter("distance_threshold_mm", 500); + + left_topic_ = this->get_parameter("left_topic").as_string(); + right_topic_ = this->get_parameter("right_topic").as_string(); + front_topic_ = this->get_parameter("front_topic").as_string(); + back_topic_ = this->get_parameter("back_topic").as_string(); + + distance_threshold_mm_ = this->get_parameter("distance_threshold_mm").as_int(); + + success_topic_ = this->get_parameter("success_topic").as_string(); + success_publisher_ = this->create_publisher(success_topic_, 10); + + left_subscriber_ = this->create_subscription( + left_topic_, 10, + std::bind(&FeedbackAll::left_dist_callback, this, std::placeholders::_1)); + right_subscriber_ = this->create_subscription( + right_topic_, 10, + std::bind(&FeedbackAll::right_dist_callback, this, std::placeholders::_1)); + front_subscriber_ = this->create_subscription( + front_topic_, 10, + std::bind(&FeedbackAll::front_dist_callback, this, std::placeholders::_1)); + back_subscriber_ = this->create_subscription( + back_topic_, 10, + std::bind(&FeedbackAll::back_dist_callback, this, std::placeholders::_1)); + } + +private: + void left_dist_callback(const std_msgs::msg::Int32::SharedPtr msg) { + distances_[LEFT] = (msg->data < distance_threshold_mm_) ? 1 : 0; + // RCLCPP_INFO(this->get_logger(), "Received left distance: %d mm", msg->data); + check_success(); + } + + void right_dist_callback(const std_msgs::msg::Int32::SharedPtr msg) { + distances_[RIGHT] = (msg->data < distance_threshold_mm_) ? 1 : 0; + // RCLCPP_INFO(this->get_logger(), "Received right distance: %d mm", msg->data); + check_success(); + } + + void front_dist_callback(const std_msgs::msg::Int32::SharedPtr msg) { + distances_[FRONT] = (msg->data < distance_threshold_mm_) ? 1 : 0; + // RCLCPP_INFO(this->get_logger(), "Received front distance: %d mm", msg->data); + check_success(); + } + + void back_dist_callback(const std_msgs::msg::Int32::SharedPtr msg) { + distances_[BACK] = (msg->data < distance_threshold_mm_) ? 1 : 0; + // RCLCPP_INFO(this->get_logger(), "Received back distance: %d mm", msg->data); + check_success(); + } + + void check_success() { + std_msgs::msg::Int32MultiArray result_msg; + result_msg.data = distances_; + success_publisher_->publish(result_msg); + // RCLCPP_INFO(this->get_logger(), "Published success status: %d, %d, %d, %d", distances_[FRONT], distances_[RIGHT], distances_[BACK], distances_[LEFT]); + } + + rclcpp::Subscription::SharedPtr left_subscriber_; + rclcpp::Subscription::SharedPtr right_subscriber_; + rclcpp::Subscription::SharedPtr front_subscriber_; + rclcpp::Subscription::SharedPtr back_subscriber_; + rclcpp::Publisher::SharedPtr success_publisher_; + std::string left_topic_; + std::string right_topic_; + std::string front_topic_; + std::string back_topic_; + std::string success_topic_; + std::int32_t distance_threshold_mm_; + std::vector distances_{0, 0, 0, 0}; // front, right, back, left + enum Direction { FRONT = 0, RIGHT = 1, BACK = 2, LEFT = 3 }; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/src/object_feedback/src/object_feedback.cpp b/src/object_feedback/src/object_feedback.cpp new file mode 100644 index 0000000..797c588 --- /dev/null +++ b/src/object_feedback/src/object_feedback.cpp @@ -0,0 +1,80 @@ +#include +#include +#include +#include +#include + + +class ObjectFeedback : public rclcpp::Node { +public: + ObjectFeedback() : Node("object_feedback_node") { + this->declare_parameter("depth_topic", "/camera/camera/depth/image_rect_raw"); + this->declare_parameter("dist_topic", "/camera/camera/obj_distance"); + depth_topic_ = this->get_parameter("depth_topic").as_string(); + dist_topic_ = this->get_parameter("dist_topic").as_string(); + dist_publisher_ = this->create_publisher(dist_topic_, 10); + depth_subscriber_ = this->create_subscription( + depth_topic_, 10, + std::bind(&ObjectFeedback::depth_img_callback, this, std::placeholders::_1)); + } + +private: + void depth_img_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, msg->encoding); + cv::Mat depth_frame = cv_ptr->image; + + int target_x = depth_frame.cols / 4; + double sum = 0.0; + int count = 0; + + for(int v = 0; v < depth_frame.rows; ++v) { + if (msg->encoding == "16UC1") { + uint16_t depth_value = depth_frame.at(v, target_x); + if (depth_value > 0) { + sum += depth_value; + count++; + } + } + else if (msg->encoding == "32FC1") { + float depth_value = depth_frame.at(v, target_x); + if (depth_value > 0.0f) { + sum += depth_value; + count++; + } + } + } + + if (count > 0) { + double average = sum / count; + dist_publisher_->publish(std_msgs::msg::Int32{static_cast(average)}); + // RCLCPP_INFO(this->get_logger(), "Average depth at column %d: %.2f mm", target_x, average); + } + + // cv::Point pt1(depth_frame.cols / 4, 0); + // cv::Point pt2(depth_frame.cols / 4, depth_frame.rows); + // cv::Scalar line_color(255, 0, 0); + // int thickness = 2; + // cv::line(depth_frame, pt1, pt2, line_color, thickness); + + // cv::imshow("Depth View", depth_frame * 100); + // cv::waitKey(1); + + } catch (cv_bridge::Exception& e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + } + } + + rclcpp::Subscription::SharedPtr depth_subscriber_; + rclcpp::Publisher::SharedPtr dist_publisher_; + std::string depth_topic_; + std::string dist_topic_; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 9274707c191f21f8f5a7dffc28a27f281696e86f Mon Sep 17 00:00:00 2001 From: CYHsiang Date: Fri, 27 Feb 2026 20:41:05 +0800 Subject: [PATCH 02/18] fix: Fix bug when colcon build object_feedback --- src/object_feedback/src/object_feedback.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/object_feedback/src/object_feedback.cpp b/src/object_feedback/src/object_feedback.cpp index 797c588..560b7b4 100644 --- a/src/object_feedback/src/object_feedback.cpp +++ b/src/object_feedback/src/object_feedback.cpp @@ -47,7 +47,9 @@ class ObjectFeedback : public rclcpp::Node { if (count > 0) { double average = sum / count; - dist_publisher_->publish(std_msgs::msg::Int32{static_cast(average)}); + std_msgs::msg::Int32 out_msg; + out_msg.data = static_cast(average); + dist_publisher_->publish(out_msg); // RCLCPP_INFO(this->get_logger(), "Average depth at column %d: %.2f mm", target_x, average); } From b43e7d7d8bd6ace4d4d27a1516a0ad719a25cdf3 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 12:46:59 +0800 Subject: [PATCH 03/18] feat: added multi camera launch for aruco detector and row scanner. Also, moved key parameters to the launch file. --- README.md | 23 +++++ .../aruco_object/config/combined_params.yaml | 4 - .../object_detector_node.hpp | 4 + .../launch/multi_camera.launch.py | 98 +++++++++++++++++++ .../src/aruco_row_scanner_node.cpp | 21 ++++ .../aruco_object/src/object_detector.cpp | 13 +++ 6 files changed, 159 insertions(+), 4 deletions(-) create mode 100644 src/aruco_ros/aruco_object/launch/multi_camera.launch.py diff --git a/README.md b/README.md index 595e8af..aadb85c 100644 --- a/README.md +++ b/README.md @@ -33,3 +33,26 @@ Open RViz to visualize camera images and TF frames: ```bash ros2 run rviz2 rviz2 ``` + +### switch to another camera when using multiple cameras +Since only one camera is active at a time, you can switch to another camera by publishing to /active_camera topic. +#### Switch to front camera +```bash +ros2 topic pub /active_camera std_msgs/msg/String "data: 'front'" -1 +``` +#### Switch to back camera +```bash +ros2 topic pub /active_camera std_msgs/msg/String "data: 'back'" -1 +``` +#### Switch to left camera +```bash +ros2 topic pub /active_camera std_msgs/msg/String "data: 'left'" -1 +``` +#### Switch to right camera +```bash +ros2 topic pub /active_camera std_msgs/msg/String "data: 'right'" -1 +``` +#### Make all cameras dormant (empty string) +```bash +ros2 topic pub /active_camera std_msgs/msg/String "data: ''" -1 +``` diff --git a/src/aruco_ros/aruco_object/config/combined_params.yaml b/src/aruco_ros/aruco_object/config/combined_params.yaml index 44d0230..99897bf 100644 --- a/src/aruco_ros/aruco_object/config/combined_params.yaml +++ b/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 diff --git a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp b/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp index 64d1b57..a20850a 100644 --- a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp +++ b/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -25,6 +26,9 @@ class CameraOnBoardNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr camera_info_sub_; rclcpp::Publisher::SharedPtr object_pose_publisher_; rclcpp::Publisher::SharedPtr marker_pub_; + rclcpp::Subscription::SharedPtr active_camera_sub_; + + std::string active_camera_ = ""; // Currently active camera (empty = all active) std::shared_ptr tf_broadcaster_; std::unique_ptr tf_buffer_; diff --git a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py new file mode 100644 index 0000000..acddeb2 --- /dev/null +++ b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py @@ -0,0 +1,98 @@ +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): + """Called after launch arguments are resolved""" + + # Define valid camera positions + camera_positions = ['front', 'back', 'left', 'right'] + + # 1. Grab the value + default_camera = LaunchConfiguration('default_active_camera').perform(context) + + # 2. VALIDATION LOGIC + # Allow empty string if 'dormant' is intended, otherwise check the list + if default_camera and default_camera not in camera_positions: + raise Exception(f"\n\n[ERROR] Invalid camera position: '{default_camera}'. \n" + f"Valid options are: {camera_positions} or an empty string.\n") + 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) + team_color = LaunchConfiguration('team_color').perform(context) + + # Validate team_color + valid_team_colors = ['blue', 'yellow'] + if team_color not in valid_team_colors: + raise Exception(f"\n\n[ERROR] Invalid team_color: '{team_color}'. \n" + f"Valid options are: {valid_team_colors}\n") + + nodes = [] + + # Launch 4 object_detector 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) + + # Launch 4 aruco_row_scanner nodes + for position in camera_positions: + node = Node( + package='aruco_object', + executable='aruco_row_scanner', + name=f'aruco_row_scanner_{position}', + parameters=[ + config_file, + {'camera_position': position, 'team_color': team_color} + ], + output='screen', + arguments=['--ros-args', '--log-level', log_level] + ) + nodes.append(node) + + # Publish initial active camera after a short delay (to let nodes start) + if default_camera: + publish_cmd = ExecuteProcess( + cmd=['bash', '-c', + f'sleep 2 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], + output='screen' + ) + nodes.append(publish_cmd) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'log_level', + default_value='info', + description='Log level' + ), + DeclareLaunchArgument( + 'default_active_camera', + default_value='back', + description='Camera to activate at startup (front/back/left/right, empty for dormant)' + ), + DeclareLaunchArgument( + 'team_color', + default_value='blue', + description='Team color (blue/yellow) - determines which ArUco IDs are targets' + ), + OpaqueFunction(function=launch_setup) + ]) + diff --git a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp index 029cfb1..5304fd0 100644 --- a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp +++ b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -41,10 +42,26 @@ class ArucoRowScannerNode : public rclcpp::Node { // Publisher mask_pub_ = this->create_publisher("target_flip_mask", 10); + + // Camera position parameter for multi-camera support + this->declare_parameter("camera_position", "front"); + camera_position_ = this->get_parameter("camera_position").as_string(); + + // Subscribe to active camera topic for multi-camera activation control + active_camera_sub_ = this->create_subscription( + "/active_camera", 10, + [this](std_msgs::msg::String::SharedPtr msg) { + active_camera_ = msg->data; + }); } private: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + // Multi-camera activation: dormant by default until /active_camera matches our position + if (active_camera_ != camera_position_) { + return; + } + if (task_completed_) return; cv_bridge::CvImagePtr cv_ptr; @@ -151,6 +168,10 @@ class ArucoRowScannerNode : public rclcpp::Node { std::deque> vote_buffer_; bool task_completed_ = false; // Could be used if we want single-shot behavior + + std::string camera_position_; + std::string active_camera_ = ""; + rclcpp::Subscription::SharedPtr active_camera_sub_; }; int main(int argc, char **argv) { diff --git a/src/aruco_ros/aruco_object/src/object_detector.cpp b/src/aruco_ros/aruco_object/src/object_detector.cpp index 131ed11..0d6c0bc 100644 --- a/src/aruco_ros/aruco_object/src/object_detector.cpp +++ b/src/aruco_ros/aruco_object/src/object_detector.cpp @@ -24,6 +24,14 @@ CameraOnBoardNode::CameraOnBoardNode() object_pose_publisher_ = this->create_publisher("detected_dock_pose", 10); marker_pub_ = this->create_publisher("visualization_marker", 10); + // Subscribe to active camera topic for multi-camera activation control + active_camera_sub_ = this->create_subscription( + "/active_camera", 10, + [this](std_msgs::msg::String::SharedPtr msg) { + active_camera_ = msg->data; + RCLCPP_INFO_ONCE(this->get_logger(), "Received active_camera signal"); + }); + tf_broadcaster_ = std::make_shared(this); tf_buffer_ = std::make_unique(this->get_clock()); @@ -55,6 +63,11 @@ CameraOnBoardNode::CameraOnBoardNode() } void CameraOnBoardNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + // Multi-camera activation: dormant by default until /active_camera matches our position + if (active_camera_ != CAMERA_POSITION_) { + return; + } + // get the camera's internal parameters if (!intrinsics_received_) { RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 2000, "Waiting for camera intrinsics..."); From 28d525af34717a85663c42ee7453fa9efccfe28f Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 14:53:06 +0800 Subject: [PATCH 04/18] feat: Add namespace for different cameras. And Introduce multi-camera launch files for ArUco detection and row scanning with dynamic camera activation. --- .../launch/detector_multi.launch.py | 51 +++ .../launch/object_detector.launch.py | 30 -- .../launch/scanner_multi.launch.py | 43 +++ .../src/aruco_row_scanner_node.cpp | 342 +++++++++--------- .../aruco_object/src/object_detector.cpp | 35 +- start_vision_tmux.sh | 20 +- 6 files changed, 300 insertions(+), 221 deletions(-) create mode 100644 src/aruco_ros/aruco_object/launch/detector_multi.launch.py delete mode 100644 src/aruco_ros/aruco_object/launch/object_detector.launch.py create mode 100644 src/aruco_ros/aruco_object/launch/scanner_multi.launch.py diff --git a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py new file mode 100644 index 0000000..c1b8070 --- /dev/null +++ b/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""" + camera_positions = ['front', 'back', 'left', 'right'] + default_camera = LaunchConfiguration('default_active_camera').perform(context) + + if default_camera and default_camera not in camera_positions: + raise Exception(f"[ERROR] Invalid camera position: '{default_camera}'") + + 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 active camera + if default_camera: + publish_cmd = ExecuteProcess( + cmd=['bash', '-c', + f'sleep 2 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], + output='screen' + ) + nodes.append(publish_cmd) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('log_level', default_value='info'), + DeclareLaunchArgument('default_active_camera', default_value='back'), + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/aruco_ros/aruco_object/launch/object_detector.launch.py b/src/aruco_ros/aruco_object/launch/object_detector.launch.py deleted file mode 100644 index 3252286..0000000 --- a/src/aruco_ros/aruco_object/launch/object_detector.launch.py +++ /dev/null @@ -1,30 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument -import os -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - pkg_share = get_package_share_directory('aruco_object') - config_file = os.path.join(pkg_share, 'config', 'combined_params.yaml') - - logger_arg = DeclareLaunchArgument( - 'log_level', - default_value='info', - description='Log level' - ) - - object_detector_node = Node( - package='aruco_object', - executable='aruco_detector_node', - name='object_detector', - parameters=[config_file], - output='screen', - arguments=['--ros-args', '--log-level', LaunchConfiguration('log_level')] - ) - - return LaunchDescription([ - logger_arg, - object_detector_node - ]) diff --git a/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py new file mode 100644 index 0000000..308319c --- /dev/null +++ b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py @@ -0,0 +1,43 @@ +from launch import LaunchDescription, LaunchContext +from launch_ros.actions import Node +from launch.substitutions import LaunchConfiguration +from launch.actions import DeclareLaunchArgument, OpaqueFunction +import os +from ament_index_python.packages import get_package_share_directory + + +def launch_setup(context: LaunchContext): + """Launch 4 aruco_row_scanner nodes""" + camera_positions = ['front', 'back', 'left', 'right'] + + 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) + team_color = LaunchConfiguration('team_color').perform(context) + + valid_team_colors = ['blue', 'yellow'] + if team_color not in valid_team_colors: + raise Exception(f"[ERROR] Invalid team_color: '{team_color}'") + + nodes = [] + + for position in camera_positions: + node = Node( + package='aruco_object', + executable='aruco_row_scanner', + name=f'aruco_row_scanner_{position}', + parameters=[config_file, {'camera_position': position, 'team_color': team_color}], + output='screen', + arguments=['--ros-args', '--log-level', log_level] + ) + nodes.append(node) + + return nodes + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument('log_level', default_value='info'), + DeclareLaunchArgument('team_color', default_value='blue'), + OpaqueFunction(function=launch_setup) + ]) diff --git a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp index 5304fd0..963d732 100644 --- a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp +++ b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp @@ -1,183 +1,191 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class ArucoRowScannerNode : public rclcpp::Node { -public: - ArucoRowScannerNode() : Node("aruco_row_scanner") { - // Parameters - this->declare_parameter("team_color", "yellow"); - this->declare_parameter("blue_id", 36); - this->declare_parameter("yellow_id", 47); - this->declare_parameter("votes_needed", 5); - - team_color_ = this->get_parameter("team_color").as_string(); - votes_needed_ = this->get_parameter("votes_needed").as_int(); - - // logic: if team_color is blue, use yellow_id, else use blue_id - int64_t id = (team_color_ == "blue") ? - this->get_parameter("yellow_id").as_int() : - this->get_parameter("blue_id").as_int(); - - target_ids_.push_back(static_cast(id)); - - RCLCPP_INFO(this->get_logger(), "Scanner started for team: %s. Votes needed: %d", team_color_.c_str(), votes_needed_); - - // Initialize ArUco dictionary - dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); - detector_params_ = cv::aruco::DetectorParameters::create(); - - // Subscriber - image_sub_ = this->create_subscription( - "/camera/camera/color/image_rect_raw", 10, - std::bind(&ArucoRowScannerNode::image_callback, this, std::placeholders::_1)); - - // Publisher - mask_pub_ = this->create_publisher("target_flip_mask", 10); - - // Camera position parameter for multi-camera support - this->declare_parameter("camera_position", "front"); - camera_position_ = this->get_parameter("camera_position").as_string(); - - // Subscribe to active camera topic for multi-camera activation control - active_camera_sub_ = this->create_subscription( - "/active_camera", 10, - [this](std_msgs::msg::String::SharedPtr msg) { - active_camera_ = msg->data; - }); - } - -private: - void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { - // Multi-camera activation: dormant by default until /active_camera matches our position - if (active_camera_ != camera_position_) { - return; - } - - if (task_completed_) return; - - cv_bridge::CvImagePtr cv_ptr; - try { - cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); - } catch (cv_bridge::Exception &e) { - RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); - return; + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + #include + + class ArucoRowScannerNode : public rclcpp::Node { + public: + ArucoRowScannerNode() : Node("aruco_row_scanner") { + // Declare camera_position FIRST (before using it for topics) + this->declare_parameter("camera_position", "front"); + camera_position_ = this->get_parameter("camera_position").as_string(); + + // Parameters + this->declare_parameter("team_color", "yellow"); + this->declare_parameter("blue_id", 36); + this->declare_parameter("yellow_id", 47); + this->declare_parameter("votes_needed", 5); + + team_color_ = this->get_parameter("team_color").as_string(); + votes_needed_ = this->get_parameter("votes_needed").as_int(); + + // logic: if team_color is blue, use yellow_id, else use blue_id + int64_t id = (team_color_ == "blue") ? + this->get_parameter("yellow_id").as_int() : + this->get_parameter("blue_id").as_int(); + + target_ids_.push_back(static_cast(id)); + + // Initialize ArUco dictionary + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + detector_params_ = cv::aruco::DetectorParameters::create(); + + // Build namespaced topic name based on camera position + std::string image_topic = "/" + camera_position_ + "/camera/color/image_rect_raw"; + RCLCPP_INFO(this->get_logger(), "Scanner [%s] subscribing to: %s", camera_position_.c_str(), image_topic.c_str()); + + // Subscriber with namespaced topic + image_sub_ = this->create_subscription( + image_topic, 10, + std::bind(&ArucoRowScannerNode::image_callback, this, std::placeholders::_1)); + + // Publisher + mask_pub_ = this->create_publisher("target_flip_mask", 10); + + // Subscribe to active camera topic for multi-camera activation control + active_camera_sub_ = this->create_subscription( + "/active_camera", 10, + [this](std_msgs::msg::String::SharedPtr msg) { + active_camera_ = msg->data; + bool is_active = (active_camera_ == camera_position_); + RCLCPP_INFO(this->get_logger(), "Active camera: '%s' -> %s", + msg->data.c_str(), is_active ? "ACTIVE" : "dormant"); + }); + + RCLCPP_INFO(this->get_logger(), "Scanner [%s] started for team: %s. Votes needed: %d", + camera_position_.c_str(), team_color_.c_str(), votes_needed_); } - std::vector ids; - std::vector> corners; - cv::aruco::detectMarkers(cv_ptr->image, dictionary_, corners, ids, detector_params_); + private: + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { + // Multi-camera activation: dormant by default until /active_camera matches our position + if (active_camera_ != camera_position_) { + return; + } + + if (task_completed_) return; + + cv_bridge::CvImagePtr cv_ptr; + try { + cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); + } catch (cv_bridge::Exception &e) { + RCLCPP_ERROR(this->get_logger(), "cv_bridge exception: %s", e.what()); + return; + } - // Phase 2: Perfect 4 Check - if (ids.size() != 4) { - RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Seen %zu markers (need 4)", ids.size()); - return; // Ignore frame - } - struct Marker { - int id; - float y; - }; - - std::vector sorted_markers; - // calculate center y of each marker - for (size_t i = 0; i < ids.size(); ++i) { - float cy = 0; - for (const auto& p : corners[i]) cy += p.y; - cy /= 4.0; - sorted_markers.push_back({ids[i], cy}); - } - - // sort by ascending y (small to large) -> Top to Bottom in image - std::sort(sorted_markers.begin(), sorted_markers.end(), - [](const Marker& a, const Marker& b) { return a.y < b.y; }); + std::vector ids; + std::vector> corners; + cv::aruco::detectMarkers(cv_ptr->image, dictionary_, corners, ids, detector_params_); - // Generate Instant Array - std::vector current_vote; - bool all_valid_ids = true; - - // iterate through all marker - for (const auto& m : sorted_markers) { - bool is_target = false; - // iterate through all target id - for (int t_id : target_ids_) { - if (m.id == t_id) { - is_target = true; - break; + // Phase 2: Perfect 4 Check + if (ids.size() != 4) { + RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Seen %zu markers (need 4)", ids.size()); + return; // Ignore frame + } + struct Marker { + int id; + float y; + }; + + std::vector sorted_markers; + // calculate center y of each marker + for (size_t i = 0; i < ids.size(); ++i) { + float cy = 0; + for (const auto& p : corners[i]) cy += p.y; + cy /= 4.0; + sorted_markers.push_back({ids[i], cy}); + } + + // sort by ascending y (small to large) -> Top to Bottom in image + std::sort(sorted_markers.begin(), sorted_markers.end(), + [](const Marker& a, const Marker& b) { return a.y < b.y; }); + + // Generate Instant Array + std::vector current_vote; + bool all_valid_ids = true; + + // iterate through all marker + for (const auto& m : sorted_markers) { + bool is_target = false; + // iterate through all target id + for (int t_id : target_ids_) { + if (m.id == t_id) { + is_target = true; + break; + } } + current_vote.push_back(is_target ? 1 : 0); } - current_vote.push_back(is_target ? 1 : 0); - } - RCLCPP_ERROR(this->get_logger(), "Current vote: [%d, %d, %d, %d]", - current_vote[0], current_vote[1], current_vote[2], current_vote[3]); + RCLCPP_ERROR(this->get_logger(), "Current vote: [%d, %d, %d, %d]", + current_vote[0], current_vote[1], current_vote[2], current_vote[3]); - // Phase 3: Voting - vote_buffer_.push_back(current_vote); - if (vote_buffer_.size() > (size_t)votes_needed_) { - vote_buffer_.pop_front(); - } + // Phase 3: Voting + vote_buffer_.push_back(current_vote); + if (vote_buffer_.size() > (size_t)votes_needed_) { + vote_buffer_.pop_front(); + } - if (vote_buffer_.size() == (size_t)votes_needed_) { - if (check_consensus()) { - publish_result(vote_buffer_.front()); - // Optional: task_completed_ = true; - vote_buffer_.clear(); // Reset buffer after success to avoid rapid re-triggers - } else { - // A strict sliding window is better than clearing fully, TODO - if (!check_consensus()) { - vote_buffer_.clear(); + if (vote_buffer_.size() == (size_t)votes_needed_) { + if (check_consensus()) { + publish_result(vote_buffer_.front()); + // Optional: task_completed_ = true; + vote_buffer_.clear(); // Reset buffer after success to avoid rapid re-triggers + } else { + // A strict sliding window is better than clearing fully, TODO + if (!check_consensus()) { + vote_buffer_.clear(); + } } } } - } - bool check_consensus() { - if (vote_buffer_.empty()) return false; - const auto& first = vote_buffer_.front(); - for (const auto& v : vote_buffer_) { - if (v != first) return false; + bool check_consensus() { + if (vote_buffer_.empty()) return false; + const auto& first = vote_buffer_.front(); + for (const auto& v : vote_buffer_) { + if (v != first) return false; + } + return true; } - return true; - } - void publish_result(const std::vector& result) { - std_msgs::msg::Int8MultiArray msg; - msg.data = result; - mask_pub_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Consensus reached! Published mask: [%d, %d, %d, %d]", - result[0], result[1], result[2], result[3]); - } + void publish_result(const std::vector& result) { + std_msgs::msg::Int8MultiArray msg; + msg.data = result; + mask_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Consensus reached! Published mask: [%d, %d, %d, %d]", + result[0], result[1], result[2], result[3]); + } - std::string team_color_; - std::vector target_ids_; - int votes_needed_; - - cv::Ptr dictionary_; - cv::Ptr detector_params_; - - rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Publisher::SharedPtr mask_pub_; - - std::deque> vote_buffer_; - bool task_completed_ = false; // Could be used if we want single-shot behavior - - std::string camera_position_; - std::string active_camera_ = ""; - rclcpp::Subscription::SharedPtr active_camera_sub_; -}; - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} + std::string team_color_; + std::vector target_ids_; + int votes_needed_; + + cv::Ptr dictionary_; + cv::Ptr detector_params_; + + rclcpp::Subscription::SharedPtr image_sub_; + rclcpp::Publisher::SharedPtr mask_pub_; + + std::deque> vote_buffer_; + bool task_completed_ = false; // Could be used if we want single-shot behavior + + std::string camera_position_; + std::string active_camera_ = ""; + rclcpp::Subscription::SharedPtr active_camera_sub_; + }; + + int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; + } diff --git a/src/aruco_ros/aruco_object/src/object_detector.cpp b/src/aruco_ros/aruco_object/src/object_detector.cpp index 0d6c0bc..b108510 100644 --- a/src/aruco_ros/aruco_object/src/object_detector.cpp +++ b/src/aruco_ros/aruco_object/src/object_detector.cpp @@ -13,12 +13,28 @@ CameraOnBoardNode::CameraOnBoardNode() : Node("aruco_detector_node"), logic_(/*default constructed*/) { using std::placeholders::_1; + + // Declare parameters FIRST (before using them for topics) + MARKER_LENGTH_ = this->declare_parameter("marker_length", 0.03); + CLUSTER_RADIUS_ = this->declare_parameter("cluster_radius", 0.3); + BLUE_ID_ = this->declare_parameter("blue_id", 36); + YELLOW_ID_ = this->declare_parameter("yellow_id", 47); + CAMERA_POSITION_ = this->declare_parameter("camera_position", "front"); + SMOOTH_ALPHA_ = this->declare_parameter("smooth_alpha", 0.3); + + // Build namespaced topic names based on camera position + // e.g., "front" -> "/front/camera/color/image_rect_raw" + std::string image_topic = "/" + CAMERA_POSITION_ + "/camera/color/image_rect_raw"; + std::string camera_info_topic = "/" + CAMERA_POSITION_ + "/camera/color/camera_info"; + + RCLCPP_INFO(this->get_logger(), "Subscribing to: %s", image_topic.c_str()); + subscription_ = this->create_subscription( - "/camera/camera/color/image_rect_raw", 10, + image_topic, 10, std::bind(&CameraOnBoardNode::image_callback, this, _1)); camera_info_sub_ = this->create_subscription( - "/camera/camera/color/camera_info", 10, + camera_info_topic, 10, std::bind(&CameraOnBoardNode::camera_info_callback, this, _1)); object_pose_publisher_ = this->create_publisher("detected_dock_pose", 10); @@ -28,8 +44,10 @@ CameraOnBoardNode::CameraOnBoardNode() active_camera_sub_ = this->create_subscription( "/active_camera", 10, [this](std_msgs::msg::String::SharedPtr msg) { - active_camera_ = msg->data; - RCLCPP_INFO_ONCE(this->get_logger(), "Received active_camera signal"); + active_camera_ = msg->data; + bool is_active = (active_camera_ == CAMERA_POSITION_); + RCLCPP_INFO(this->get_logger(), "Active camera: '%s' -> %s", + msg->data.c_str(), is_active ? "ACTIVE" : "dormant"); }); @@ -40,13 +58,6 @@ CameraOnBoardNode::CameraOnBoardNode() dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); detector_params_ = cv::aruco::DetectorParameters::create(); - MARKER_LENGTH_ = this->declare_parameter("marker_length", 0.03); - CLUSTER_RADIUS_ = this->declare_parameter("cluster_radius", 0.3); - BLUE_ID_ = this->declare_parameter("blue_id", 36); - YELLOW_ID_ = this->declare_parameter("yellow_id", 47); - CAMERA_POSITION_ = this->declare_parameter("camera_position", "front"); - SMOOTH_ALPHA_ = this->declare_parameter("smooth_alpha", 0.3); - // configure logic with declared params logic_ = ProcessLogic(MARKER_LENGTH_, BLUE_ID_, YELLOW_ID_, CLUSTER_RADIUS_, CAMERA_POSITION_, SMOOTH_ALPHA_); @@ -59,7 +70,7 @@ CameraOnBoardNode::CameraOnBoardNode() cv::Point3f(-half_len, -half_len, 0) }; - RCLCPP_INFO(this->get_logger(), "Object detector started. cluster_radius: %.3f", CLUSTER_RADIUS_); + RCLCPP_INFO(this->get_logger(), "Object detector [%s] started. cluster_radius: %.3f", CAMERA_POSITION_.c_str(), CLUSTER_RADIUS_); } void CameraOnBoardNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index aab294c..8ff45be 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -15,33 +15,29 @@ fi # Create a new tmux session named 'vision' but don't attach to it yet tmux new-session -d -s $SESSION_NAME -# Send commands to the first pane (pane 0) +# Send commands to the first pane (pane 0) - Camera Node # Enter the running docker container tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m # Wait a brief moment for the container shell to be ready sleep 1 -# Source ROS 2 Humble setup and local workspace setup -# tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_name:=rb_back_camera' C-m -tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py' C-m +# Source ROS 2 Humble setup and launch camera +tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back' C-m # Split the window horizontally to create a second pane tmux split-window -h -t $SESSION_NAME:0 -# Send commands to the second pane (pane 1) -# Enter the running docker container +# Send commands to the second pane (pane 1) - Object Detector Nodes tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m -# Wait a brief moment sleep 1 -# Source ROS 2 Humble setup and local workspace setup -tmux send-keys -t $SESSION_NAME:0.1 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch aruco_object object_detector.launch.py' C-m +tmux send-keys -t $SESSION_NAME:0.1 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch aruco_object detector_multi.launch.py' C-m -# Split the detector pane (pane 1) vertically to create a third pane for the scanner +# Split the detector pane vertically to create a third pane for scanner tmux split-window -v -t $SESSION_NAME:0.1 -# Send commands to the third pane (pane 2) +# Send commands to the third pane (pane 2) - Row Scanner Nodes tmux send-keys -t $SESSION_NAME:0.2 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.2 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch aruco_object scanner.launch.py' C-m +tmux send-keys -t $SESSION_NAME:0.2 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch aruco_object scanner_multi.launch.py' C-m # Create a new window for Rviz tmux new-window -t $SESSION_NAME:1 -n 'rviz' From 85c09424accdf1fee269a62ee88969218ad17582 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 15:54:35 +0800 Subject: [PATCH 05/18] refactor: Replaced simple yaw calculation with a PCA-based wall orientation method for improved accuracy. --- .../aruco_object/src/process_logic.cpp | 75 +++++++++++++++---- 1 file changed, 60 insertions(+), 15 deletions(-) diff --git a/src/aruco_ros/aruco_object/src/process_logic.cpp b/src/aruco_ros/aruco_object/src/process_logic.cpp index 020dabb..6ade200 100644 --- a/src/aruco_ros/aruco_object/src/process_logic.cpp +++ b/src/aruco_ros/aruco_object/src/process_logic.cpp @@ -78,31 +78,76 @@ geometry_msgs::msg::PoseStamped ProcessLogic::compute_perpendicular_pose_from_fl double center_x = sum_x / floor_points.size(); double center_y = sum_y / floor_points.size(); - // Simple Yaw Calculation: No PCA needed + // === PCA-based Wall Orientation === + // Use PCA to find the line direction of the markers (wall). + // The perpendicular to this line gives the wall normal (facing direction). + // Use camera_position to disambiguate which way the normal should point. + + cv::Mat data_pts((int)floor_points.size(), 2, CV_64F); + for (size_t i = 0; i < floor_points.size(); ++i) { + data_pts.at((int)i, 0) = floor_points[i].x; + data_pts.at((int)i, 1) = floor_points[i].y; + } + cv::PCA pca_analysis(data_pts, cv::Mat(), cv::PCA::DATA_AS_ROW); + + // First eigenvector = direction of the line (wall) + double line_dx = pca_analysis.eigenvectors.at(0, 0); + double line_dy = pca_analysis.eigenvectors.at(0, 1); + + // Perpendicular to line = wall normal candidates (two directions: +90° and -90°) + double perp_dx = -line_dy; + double perp_dy = line_dx; + + // === Disambiguate using camera_position === + // The wall normal should point TOWARDS the camera (i.e., away from the wall into the viewing area) + // Based on camera_position, we know which direction the camera is looking: + // - back: camera looks at -Y, so normal should point towards -Y (perp_dy < 0) + // - front: camera looks at +Y, so normal should point towards +Y (perp_dy > 0) + // - left: camera looks at -X, so normal should point towards -X (perp_dx < 0) + // - right: camera looks at +X, so normal should point towards +X (perp_dx > 0) + + if (camera_position_ == "back") { + // Camera looks at -Y, normal should point toward camera (negative Y) + if (perp_dy > 0) { perp_dx = -perp_dx; perp_dy = -perp_dy; } + } else if (camera_position_ == "front") { + // Camera looks at +Y, normal should point toward camera (positive Y) + if (perp_dy < 0) { perp_dx = -perp_dx; perp_dy = -perp_dy; } + } else if (camera_position_ == "left") { + // Camera looks at -X, normal should point toward camera (negative X) + if (perp_dx > 0) { perp_dx = -perp_dx; perp_dy = -perp_dy; } + } else if (camera_position_ == "right") { + // Camera looks at +X, normal should point toward camera (positive X) + if (perp_dx < 0) { perp_dx = -perp_dx; perp_dy = -perp_dy; } + } + + // === Calculate final yaw === // Robot coordinate system: Front = +Y, Right = +X - // The yaw is the angle from robot's +Y axis to the cluster center, - // adjusted by which camera is viewing it. + // The robot should orient so its front (+Y) points OPPOSITE to the wall normal + // (i.e., the robot faces the wall) - // atan2(x, y) gives angle from +Y axis (since front is +Y) - double angle_to_target = std::atan2(center_x, center_y); + // Wall normal angle (from +X axis in standard math coords) + double perp_angle = std::atan2(perp_dy, perp_dx); + // Convert to robot yaw: robot front (+Y) should face OPPOSITE to normal + // Robot yaw is measured from +Y axis, so we add 90° offset and flip double final_yaw; if (camera_position_ == "back") { - // Back camera: robot back (-Y) faces target, so robot front (+Y) points opposite - final_yaw = angle_to_target + M_PI; + // Robot back faces wall, so front points away (opposite to normal + 180°) + final_yaw = perp_angle + M_PI - M_PI_2; // = perp_angle + PI/2 } else if (camera_position_ == "left") { - // Left camera (-X): target is to robot's left - final_yaw = angle_to_target + M_PI_2; + // Robot left faces wall + final_yaw = perp_angle + M_PI_2 - M_PI_2; // = perp_angle } else if (camera_position_ == "right") { - // Right camera (+X): target is to robot's right - final_yaw = angle_to_target - M_PI_2; + // Robot right faces wall + final_yaw = perp_angle - M_PI_2 - M_PI_2; // = perp_angle - PI } else { - // Front camera (+Y): robot front faces target - final_yaw = angle_to_target; + // Robot front faces wall + final_yaw = perp_angle - M_PI_2; } - RCLCPP_INFO(rclcpp::get_logger("ProcessLogic"), "angle_to_target: %.2f deg, final_yaw: %.2f deg", - angle_to_target * (180.0 / M_PI), final_yaw * (180.0 / M_PI)); + RCLCPP_INFO(rclcpp::get_logger("ProcessLogic"), + "PCA perp_angle: %.2f deg, final_yaw: %.2f deg", + perp_angle * (180.0 / M_PI), final_yaw * (180.0 / M_PI)); // Yaw Consistency Check: DISABLED - Trust geometric check only // if (!first_pose_) { From 1a09838057d3b53dceff1ee362a95f90f2902fdd Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 21:29:15 +0800 Subject: [PATCH 06/18] feat: Migrate camera activation to integer-based dock side topic for main program and remove marker publisher from processing logic. --- .../object_detector_node.hpp | 11 ++- .../aruco_cluster_detect/process_logic.hpp | 3 +- .../launch/detector_multi.launch.py | 25 ++--- .../aruco_object/launch/scanner.launch.py | 23 ----- .../launch/scanner_multi.launch.py | 14 ++- .../src/aruco_row_scanner_node.cpp | 96 ++++++++++--------- .../aruco_object/src/object_detector.cpp | 34 ++++--- .../aruco_object/src/process_logic.cpp | 32 +------ start_vision_tmux.sh | 52 +++++++--- 9 files changed, 144 insertions(+), 146 deletions(-) delete mode 100644 src/aruco_ros/aruco_object/launch/scanner.launch.py diff --git a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp b/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp index a20850a..6647eda 100644 --- a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp +++ b/src/aruco_ros/aruco_object/include/aruco_cluster_detect/object_detector_node.hpp @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include #include #include @@ -12,6 +12,7 @@ #include #include #include +#include #include "aruco_cluster_detect/process_logic.hpp" class CameraOnBoardNode : public rclcpp::Node { @@ -26,9 +27,11 @@ class CameraOnBoardNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr camera_info_sub_; rclcpp::Publisher::SharedPtr object_pose_publisher_; rclcpp::Publisher::SharedPtr marker_pub_; - rclcpp::Subscription::SharedPtr active_camera_sub_; + rclcpp::Subscription::SharedPtr dock_side_sub_; - std::string active_camera_ = ""; // Currently active camera (empty = all active) + int16_t my_side_ = -1; // This node's side number + int16_t active_side_ = -1; // Currently active side from /robot/dock_side + std::map position_to_side_; std::shared_ptr tf_broadcaster_; std::unique_ptr tf_buffer_; @@ -44,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_; diff --git a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/process_logic.hpp b/src/aruco_ros/aruco_object/include/aruco_cluster_detect/process_logic.hpp index f7f4f8e..66fd3ec 100644 --- a/src/aruco_ros/aruco_object/include/aruco_cluster_detect/process_logic.hpp +++ b/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 &floor_points, const cv::Point2d &camera_in_base, - const rclcpp::Time &now, - rclcpp::Publisher::SharedPtr marker_pub); + const rclcpp::Time &now); private: double marker_length_; diff --git a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py index c1b8070..6aa4580 100644 --- a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py +++ b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py @@ -8,11 +8,13 @@ def launch_setup(context: LaunchContext): """Launch 4 object_detector nodes""" + # Side mapping: 0=front, 1=right, 2=back, 3=left + position_to_side = {'front': 0, 'right': 1, 'back': 2, 'left': 3} camera_positions = ['front', 'back', 'left', 'right'] - default_camera = LaunchConfiguration('default_active_camera').perform(context) + default_side = int(LaunchConfiguration('default_dock_side').perform(context)) - if default_camera and default_camera not in camera_positions: - raise Exception(f"[ERROR] Invalid camera position: '{default_camera}'") + 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') @@ -31,14 +33,13 @@ def launch_setup(context: LaunchContext): ) nodes.append(node) - # Publish initial active camera - if default_camera: - publish_cmd = ExecuteProcess( - cmd=['bash', '-c', - f'sleep 2 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], - output='screen' - ) - nodes.append(publish_cmd) + # 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 @@ -46,6 +47,6 @@ def launch_setup(context: LaunchContext): def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('log_level', default_value='info'), - DeclareLaunchArgument('default_active_camera', default_value='back'), + DeclareLaunchArgument('default_dock_side', default_value='2'), # 0=front, 1=right, 2=back, 3=left OpaqueFunction(function=launch_setup) ]) diff --git a/src/aruco_ros/aruco_object/launch/scanner.launch.py b/src/aruco_ros/aruco_object/launch/scanner.launch.py deleted file mode 100644 index c53c254..0000000 --- a/src/aruco_ros/aruco_object/launch/scanner.launch.py +++ /dev/null @@ -1,23 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node -import os -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - pkg_name = 'aruco_object' - - config = os.path.join( - get_package_share_directory(pkg_name), - 'config', - 'combined_params.yaml' - ) - - return LaunchDescription([ - Node( - package=pkg_name, - executable='aruco_row_scanner', - name='aruco_row_scanner', - output='screen', - parameters=[config] - ) - ]) diff --git a/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py index 308319c..d43a311 100644 --- a/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py +++ b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py @@ -1,14 +1,17 @@ from launch import LaunchDescription, LaunchContext from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument, OpaqueFunction +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 aruco_row_scanner nodes""" + # Side mapping: 0=front, 1=right, 2=back, 3=left + position_to_side = {'front': 0, 'right': 1, 'back': 2, 'left': 3} camera_positions = ['front', 'back', 'left', 'right'] + default_side = int(LaunchConfiguration('default_dock_side').perform(context)) pkg_share = get_package_share_directory('aruco_object') config_file = os.path.join(pkg_share, 'config', 'combined_params.yaml') @@ -31,6 +34,14 @@ def launch_setup(context: LaunchContext): arguments=['--ros-args', '--log-level', log_level] ) nodes.append(node) + + # Publish initial dock_side after nodes start + 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 @@ -39,5 +50,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('log_level', default_value='info'), DeclareLaunchArgument('team_color', default_value='blue'), + DeclareLaunchArgument('default_dock_side', default_value='2'), # 0=front, 1=right, 2=back, 3=left OpaqueFunction(function=launch_setup) ]) diff --git a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp index 963d732..d5bb1d0 100644 --- a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp +++ b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp @@ -1,7 +1,7 @@ #include #include - #include - #include + #include + #include #include #include #include @@ -16,6 +16,14 @@ // Declare camera_position FIRST (before using it for topics) this->declare_parameter("camera_position", "front"); camera_position_ = this->get_parameter("camera_position").as_string(); + // Build namespaced topic name based on camera position + std::string image_topic = "/" + camera_position_ + "/camera/color/image_rect_raw"; + RCLCPP_INFO(this->get_logger(), "Scanner [%s] (side=%d) subscribing to: %s", camera_position_.c_str(), my_side_, image_topic.c_str()); + + // Map camera position string to dock_side number + // dock_side: 0=front, 1=right, 2=back, 3=left + position_to_side_ = {{"front", 0}, {"right", 1}, {"back", 2}, {"left", 3}}; + my_side_ = position_to_side_[camera_position_]; // Parameters this->declare_parameter("team_color", "yellow"); @@ -37,40 +45,32 @@ dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); detector_params_ = cv::aruco::DetectorParameters::create(); - // Build namespaced topic name based on camera position - std::string image_topic = "/" + camera_position_ + "/camera/color/image_rect_raw"; - RCLCPP_INFO(this->get_logger(), "Scanner [%s] subscribing to: %s", camera_position_.c_str(), image_topic.c_str()); - - // Subscriber with namespaced topic + // Image subscriber with namespaced topic image_sub_ = this->create_subscription( image_topic, 10, std::bind(&ArucoRowScannerNode::image_callback, this, std::placeholders::_1)); - // Publisher - mask_pub_ = this->create_publisher("target_flip_mask", 10); + // Publisher: /robot/vision/hazelnut/flip (Int32MultiArray with 5 elements) + flip_pub_ = this->create_publisher("/robot/vision/hazelnut/flip", 10); - // Subscribe to active camera topic for multi-camera activation control - active_camera_sub_ = this->create_subscription( - "/active_camera", 10, - [this](std_msgs::msg::String::SharedPtr msg) { - active_camera_ = msg->data; - bool is_active = (active_camera_ == camera_position_); - RCLCPP_INFO(this->get_logger(), "Active camera: '%s' -> %s", - msg->data.c_str(), is_active ? "ACTIVE" : "dormant"); + // Subscribe to dock_side topic for multi-camera activation control + dock_side_sub_ = this->create_subscription( + "/robot/dock_side", 10, + [this](std_msgs::msg::Int16::SharedPtr msg) { + active_side_ = msg->data; + bool is_active = (active_side_ == my_side_); + RCLCPP_INFO(this->get_logger(), "Dock side: %d -> %s", msg->data, is_active ? "ACTIVE" : "dormant"); }); - RCLCPP_INFO(this->get_logger(), "Scanner [%s] started for team: %s. Votes needed: %d", - camera_position_.c_str(), team_color_.c_str(), votes_needed_); + RCLCPP_INFO(this->get_logger(), "Scanner [%s] (side=%d) started for team: %s. Votes needed: %d", camera_position_.c_str(), my_side_, team_color_.c_str(), votes_needed_); } private: void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { - // Multi-camera activation: dormant by default until /active_camera matches our position - if (active_camera_ != camera_position_) { + // Multi-camera activation: dormant by default until dock_side matches our side + if (active_side_ != my_side_) { return; } - - if (task_completed_) return; cv_bridge::CvImagePtr cv_ptr; try { @@ -89,6 +89,7 @@ RCLCPP_INFO_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Seen %zu markers (need 4)", ids.size()); return; // Ignore frame } + struct Marker { int id; float y; @@ -107,9 +108,8 @@ std::sort(sorted_markers.begin(), sorted_markers.end(), [](const Marker& a, const Marker& b) { return a.y < b.y; }); - // Generate Instant Array - std::vector current_vote; - bool all_valid_ids = true; + // Generate Instant Array (4 flip values) + std::vector current_vote; // iterate through all marker for (const auto& m : sorted_markers) { @@ -121,11 +121,11 @@ break; } } + // 0 = NO NEED to flip, 1 = NEED to flip current_vote.push_back(is_target ? 1 : 0); } - RCLCPP_ERROR(this->get_logger(), "Current vote: [%d, %d, %d, %d]", - current_vote[0], current_vote[1], current_vote[2], current_vote[3]); + RCLCPP_INFO(this->get_logger(), "Current vote: [%d, %d, %d, %d]", current_vote[0], current_vote[1], current_vote[2], current_vote[3]); // Phase 3: Voting vote_buffer_.push_back(current_vote); @@ -136,13 +136,10 @@ if (vote_buffer_.size() == (size_t)votes_needed_) { if (check_consensus()) { publish_result(vote_buffer_.front()); - // Optional: task_completed_ = true; vote_buffer_.clear(); // Reset buffer after success to avoid rapid re-triggers } else { - // A strict sliding window is better than clearing fully, TODO - if (!check_consensus()) { - vote_buffer_.clear(); - } + // Clear buffer if no consensus + vote_buffer_.clear(); } } } @@ -156,12 +153,19 @@ return true; } - void publish_result(const std::vector& result) { - std_msgs::msg::Int8MultiArray msg; - msg.data = result; - mask_pub_->publish(msg); - RCLCPP_INFO(this->get_logger(), "Consensus reached! Published mask: [%d, %d, %d, %d]", - result[0], result[1], result[2], result[3]); + void publish_result(const std::vector& result) { + // Format: [flip0, flip1, flip2, flip3, side] + std_msgs::msg::Int32MultiArray msg; + msg.data.resize(5); + msg.data[0] = result[0]; // hazelnut 0 + msg.data[1] = result[1]; // hazelnut 1 + msg.data[2] = result[2]; // hazelnut 2 + msg.data[3] = result[3]; // hazelnut 3 + msg.data[4] = my_side_; // which side to do the action + + flip_pub_->publish(msg); + RCLCPP_INFO(this->get_logger(), "Consensus reached! Published flip: [%d, %d, %d, %d, side=%d]", + result[0], result[1], result[2], result[3], my_side_); } std::string team_color_; @@ -170,16 +174,16 @@ cv::Ptr dictionary_; cv::Ptr detector_params_; - + // pub, sub rclcpp::Subscription::SharedPtr image_sub_; - rclcpp::Publisher::SharedPtr mask_pub_; - - std::deque> vote_buffer_; - bool task_completed_ = false; // Could be used if we want single-shot behavior + rclcpp::Publisher::SharedPtr flip_pub_; + std::deque> vote_buffer_; std::string camera_position_; - std::string active_camera_ = ""; - rclcpp::Subscription::SharedPtr active_camera_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 position_to_side_; + rclcpp::Subscription::SharedPtr dock_side_sub_; }; int main(int argc, char **argv) { diff --git a/src/aruco_ros/aruco_object/src/object_detector.cpp b/src/aruco_ros/aruco_object/src/object_detector.cpp index b108510..f21edd5 100644 --- a/src/aruco_ros/aruco_object/src/object_detector.cpp +++ b/src/aruco_ros/aruco_object/src/object_detector.cpp @@ -22,12 +22,18 @@ CameraOnBoardNode::CameraOnBoardNode() CAMERA_POSITION_ = this->declare_parameter("camera_position", "front"); SMOOTH_ALPHA_ = this->declare_parameter("smooth_alpha", 0.3); + // Map camera position string to dock_side number + // dock_side: 0=front, 1=right, 2=back, 3=left + position_to_side_ = {{"front", 0}, {"right", 1}, {"back", 2}, {"left", 3}}; + my_side_ = position_to_side_[CAMERA_POSITION_]; + // Build namespaced topic names based on camera position // e.g., "front" -> "/front/camera/color/image_rect_raw" std::string image_topic = "/" + CAMERA_POSITION_ + "/camera/color/image_rect_raw"; std::string camera_info_topic = "/" + CAMERA_POSITION_ + "/camera/color/camera_info"; - RCLCPP_INFO(this->get_logger(), "Subscribing to: %s", image_topic.c_str()); + RCLCPP_INFO(this->get_logger(), "Detector [%s] (side=%d) subscribing to: %s", + CAMERA_POSITION_.c_str(), my_side_, image_topic.c_str()); subscription_ = this->create_subscription( image_topic, 10, @@ -38,16 +44,15 @@ CameraOnBoardNode::CameraOnBoardNode() std::bind(&CameraOnBoardNode::camera_info_callback, this, _1)); object_pose_publisher_ = this->create_publisher("detected_dock_pose", 10); - marker_pub_ = this->create_publisher("visualization_marker", 10); - // Subscribe to active camera topic for multi-camera activation control - active_camera_sub_ = this->create_subscription( - "/active_camera", 10, - [this](std_msgs::msg::String::SharedPtr msg) { - active_camera_ = msg->data; - bool is_active = (active_camera_ == CAMERA_POSITION_); - RCLCPP_INFO(this->get_logger(), "Active camera: '%s' -> %s", - msg->data.c_str(), is_active ? "ACTIVE" : "dormant"); + // Subscribe to dock_side topic for multi-camera activation control + dock_side_sub_ = this->create_subscription( + "/robot/dock_side", 10, + [this](std_msgs::msg::Int16::SharedPtr msg) { + active_side_ = msg->data; + bool is_active = (active_side_ == my_side_); + RCLCPP_INFO(this->get_logger(), "Dock side: %d -> %s", + msg->data, is_active ? "ACTIVE" : "dormant"); }); @@ -70,12 +75,13 @@ CameraOnBoardNode::CameraOnBoardNode() cv::Point3f(-half_len, -half_len, 0) }; - RCLCPP_INFO(this->get_logger(), "Object detector [%s] started. cluster_radius: %.3f", CAMERA_POSITION_.c_str(), CLUSTER_RADIUS_); + RCLCPP_INFO(this->get_logger(), "Object detector [%s] (side=%d) started. cluster_radius: %.3f", + CAMERA_POSITION_.c_str(), my_side_, CLUSTER_RADIUS_); } void CameraOnBoardNode::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { - // Multi-camera activation: dormant by default until /active_camera matches our position - if (active_camera_ != CAMERA_POSITION_) { + // Multi-camera activation: dormant by default until dock_side matches our side + if (active_side_ != my_side_) { return; } @@ -180,7 +186,7 @@ void CameraOnBoardNode::image_callback(const sensor_msgs::msg::Image::SharedPtr cv::Point2d camera_in_base(cam_to_base.transform.translation.x, cam_to_base.transform.translation.y); geometry_msgs::msg::PoseStamped target_pose = logic_.compute_perpendicular_pose_from_floor_points( - floor_points, camera_in_base, this->get_clock()->now(), marker_pub_); + floor_points, camera_in_base, this->get_clock()->now()); if (!target_pose.header.frame_id.empty()) { // Log key results: number of selected markers, center and yaw diff --git a/src/aruco_ros/aruco_object/src/process_logic.cpp b/src/aruco_ros/aruco_object/src/process_logic.cpp index 6ade200..cd702f7 100644 --- a/src/aruco_ros/aruco_object/src/process_logic.cpp +++ b/src/aruco_ros/aruco_object/src/process_logic.cpp @@ -66,8 +66,7 @@ void ProcessLogic::select_clustered_aruco( geometry_msgs::msg::PoseStamped ProcessLogic::compute_perpendicular_pose_from_floor_points( const std::vector &floor_points, const cv::Point2d &camera_in_base, - const rclcpp::Time &now, - rclcpp::Publisher::SharedPtr marker_pub) { + const rclcpp::Time &now) { geometry_msgs::msg::PoseStamped result_pose; if (floor_points.empty()) return result_pose; @@ -223,34 +222,5 @@ geometry_msgs::msg::PoseStamped ProcessLogic::compute_perpendicular_pose_from_fl // Store for next iteration last_pose_ = result_pose; - // // Publish Visualization Marker (Optional: Show the calculated approach vector) - // if (marker_pub) { - // visualization_msgs::msg::Marker arrow; - // arrow.header.frame_id = result_pose.header.frame_id; - // arrow.header.stamp = now; - // arrow.ns = "approach_vector"; - // arrow.id = 101; - // arrow.type = visualization_msgs::msg::Marker::ARROW; - // arrow.action = visualization_msgs::msg::Marker::ADD; - - // // Start point (Robot/Cluster Center) - // geometry_msgs::msg::Point p1, p2; - // p1.x = result_pose.pose.position.x; - // p1.y = result_pose.pose.position.y; - // p1.z = 0.0; - - // // End point (pointing into the wall) - // double len = 0.5; - // p2.x = result_pose.pose.position.x + perp_dx * len; - // p2.y = result_pose.pose.position.y + perp_dy * len; - // p2.z = 0.0; - - // arrow.points.push_back(p1); - // arrow.points.push_back(p2); - // arrow.scale.x = 0.05; arrow.scale.y = 0.1; arrow.scale.z = 0.1; - // arrow.color.r = 0.0f; arrow.color.g = 1.0f; arrow.color.b = 0.0f; arrow.color.a = 1.0f; - // marker_pub->publish(arrow); - // } - return result_pose; } diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index 8ff45be..19cb69e 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -15,33 +15,59 @@ fi # Create a new tmux session named 'vision' but don't attach to it yet tmux new-session -d -s $SESSION_NAME -# Send commands to the first pane (pane 0) - Camera Node -# Enter the running docker container +# === Pane 0: Camera Node === tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m -# Wait a brief moment for the container shell to be ready sleep 1 -# Source ROS 2 Humble setup and launch camera tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back' C-m # Split the window horizontally to create a second pane tmux split-window -h -t $SESSION_NAME:0 -# Send commands to the second pane (pane 1) - Object Detector Nodes -tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m +# === Pane 1: Watchdog + ArUco Launcher === +# This pane waits for camera topics to be available, then launches detector and scanner +WATCHDOG_CMD=' +export ROS_DOMAIN_ID=13 +source /opt/ros/humble/setup.bash +source install/setup.bash + +echo "=== Waiting for camera topics... ===" + +# Wait for any camera_info topic matching pattern */camera/color/camera_info +while true; do + TOPICS=$(ros2 topic list 2>/dev/null | grep -E "/camera/color/camera_info") + if [ -n "$TOPICS" ]; then + echo "=== Camera topic found: $TOPICS ===" + break + fi + echo "Waiting for camera topics..." + sleep 2 +done + +# Small delay to ensure camera is fully ready +sleep 2 + +echo "=== Launching ArUco Detector ===" +ros2 launch aruco_object detector_multi.launch.py & +DETECTOR_PID=$! + sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch aruco_object detector_multi.launch.py' C-m -# Split the detector pane vertically to create a third pane for scanner -tmux split-window -v -t $SESSION_NAME:0.1 +echo "=== Launching ArUco Scanner ===" +ros2 launch aruco_object scanner_multi.launch.py & +SCANNER_PID=$! + +echo "=== All nodes launched! Detector PID: $DETECTOR_PID, Scanner PID: $SCANNER_PID ===" -# Send commands to the third pane (pane 2) - Row Scanner Nodes -tmux send-keys -t $SESSION_NAME:0.2 'docker exec -it vision-ws bash' C-m +# Wait for both processes +wait $DETECTOR_PID $SCANNER_PID +' + +tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.2 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch aruco_object scanner_multi.launch.py' C-m +tmux send-keys -t $SESSION_NAME:0.1 "$WATCHDOG_CMD" C-m # Create a new window for Rviz tmux new-window -t $SESSION_NAME:1 -n 'rviz' -# Send commands to the new window tmux send-keys -t $SESSION_NAME:1 'docker exec -it vision-ws bash' C-m sleep 1 tmux send-keys -t $SESSION_NAME:1 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash' C-m From c75b69817d3fdeadccc0a256cea4410f028a37fb Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 21:51:52 +0800 Subject: [PATCH 07/18] refactor: Launch ArUco detector and scanner in separate tmux panes, each with its own camera wait, and remove an unused dictionary from the launch file. --- .../launch/detector_multi.launch.py | 1 - start_vision_tmux.sh | 54 +++++++++++-------- 2 files changed, 31 insertions(+), 24 deletions(-) diff --git a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py index 6aa4580..30ff0db 100644 --- a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py +++ b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py @@ -9,7 +9,6 @@ def launch_setup(context: LaunchContext): """Launch 4 object_detector nodes""" # Side mapping: 0=front, 1=right, 2=back, 3=left - position_to_side = {'front': 0, 'right': 1, 'back': 2, 'left': 3} camera_positions = ['front', 'back', 'left', 'right'] default_side = int(LaunchConfiguration('default_dock_side').perform(context)) diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index 19cb69e..e999fff 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -23,48 +23,56 @@ tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/hu # Split the window horizontally to create a second pane tmux split-window -h -t $SESSION_NAME:0 -# === Pane 1: Watchdog + ArUco Launcher === -# This pane waits for camera topics to be available, then launches detector and scanner -WATCHDOG_CMD=' +# === Pane 1: Object Detector (waits for camera) === +DETECTOR_CMD=' export ROS_DOMAIN_ID=13 source /opt/ros/humble/setup.bash source install/setup.bash -echo "=== Waiting for camera topics... ===" - -# Wait for any camera_info topic matching pattern */camera/color/camera_info +echo "=== [Detector] Waiting for camera topics... ===" while true; do TOPICS=$(ros2 topic list 2>/dev/null | grep -E "/camera/color/camera_info") if [ -n "$TOPICS" ]; then - echo "=== Camera topic found: $TOPICS ===" + echo "=== [Detector] Camera found: $TOPICS ===" break fi - echo "Waiting for camera topics..." sleep 2 done +sleep 1 +echo "=== [Detector] Launching ArUco Detector ===" +ros2 launch aruco_object detector_multi.launch.py +' -# Small delay to ensure camera is fully ready -sleep 2 - -echo "=== Launching ArUco Detector ===" -ros2 launch aruco_object detector_multi.launch.py & -DETECTOR_PID=$! - +tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m sleep 1 +tmux send-keys -t $SESSION_NAME:0.1 "$DETECTOR_CMD" C-m -echo "=== Launching ArUco Scanner ===" -ros2 launch aruco_object scanner_multi.launch.py & -SCANNER_PID=$! +# Split pane 1 vertically to create pane 2 +tmux split-window -v -t $SESSION_NAME:0.1 -echo "=== All nodes launched! Detector PID: $DETECTOR_PID, Scanner PID: $SCANNER_PID ===" +# === Pane 2: Row Scanner (waits for camera) === +SCANNER_CMD=' +export ROS_DOMAIN_ID=13 +source /opt/ros/humble/setup.bash +source install/setup.bash -# Wait for both processes -wait $DETECTOR_PID $SCANNER_PID +echo "=== [Scanner] Waiting for camera topics... ===" +while true; do + TOPICS=$(ros2 topic list 2>/dev/null | grep -E "/camera/color/camera_info") + if [ -n "$TOPICS" ]; then + echo "=== [Scanner] Camera found: $TOPICS ===" + break + fi + sleep 2 +done +sleep 1 +echo "=== [Scanner] Launching ArUco Scanner ===" +ros2 launch aruco_object scanner_multi.launch.py ' -tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m +tmux send-keys -t $SESSION_NAME:0.2 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "$WATCHDOG_CMD" C-m +tmux send-keys -t $SESSION_NAME:0.2 "$SCANNER_CMD" C-m # Create a new window for Rviz tmux new-window -t $SESSION_NAME:1 -n 'rviz' From 679f8a12aff64f1ed15588229bd2294343c399ff Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Fri, 6 Feb 2026 22:03:26 +0800 Subject: [PATCH 08/18] refactor: move main robot's code into my codebase --- docker/compose.yaml | 4 +-- .../realsense2_camera/launch/rs_launch.py | 29 ++++++++++--------- 2 files changed, 18 insertions(+), 15 deletions(-) diff --git a/docker/compose.yaml b/docker/compose.yaml index 34b848b..12dae6c 100644 --- a/docker/compose.yaml +++ b/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 \ No newline at end of file + - ../:/home/vision/vision_ws diff --git a/src/realsense_ros/realsense2_camera/launch/rs_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_launch.py index 79a8e29..2dd8ace 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_launch.py @@ -47,7 +47,7 @@ {'name': 'depth_module.infra_format', 'default': 'RGB8', 'description': 'infra0 stream format'}, {'name': 'depth_module.infra1_format', 'default': 'Y8', 'description': 'infra1 stream format'}, {'name': 'depth_module.infra2_format', 'default': 'Y8', 'description': 'infra2 stream format'}, - {'name': 'depth_module.color_profile', 'default': '0,0,0', 'description': 'Depth module color stream profile for d405'}, + {'name': 'depth_module.color_profile', 'default': '848,480,60', 'description': 'Depth module color stream profile for d405'}, {'name': 'depth_module.color_format', 'default': 'RGB8', 'description': 'color stream format for d405'}, {'name': 'depth_module.exposure', 'default': '8500', 'description': 'Depth module manual exposure value'}, {'name': 'depth_module.gain', 'default': '16', 'description': 'Depth module manual gain value'}, @@ -139,19 +139,22 @@ def launch_setup(context, params, param_name_suffix=''): ) ] +def launch_map_transform_publisher_node(context: LaunchContext): + node = launch_ros.actions.Node( + name='map_transform_publisher', + package="tf2_ros", + executable="static_transform_publisher", + arguments=[ + # 新的紅黑機數值 + '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571', + 'base_footprint', + context.launch_configurations['camera_name'] + '_link' + ] + ) + return [node] + def generate_launch_description(): return LaunchDescription(declare_configurable_parameters(configurable_parameters) + [ OpaqueFunction(function=launch_setup, kwargs = {'params' : set_configurable_parameters(configurable_parameters)}), - launch_ros.actions.Node( - name='map_transform_publisher', - package="tf2_ros", - executable="static_transform_publisher", - arguments=[ - # 新的紅黑機 back camera - '0.0', '-0.125', '0.30', # x, y, z - '-1.571', '0.7854', '-1.571', # yaw, pitch, roll - 'base_footprint', - [LaunchConfiguration('camera_name'), '_link'] - ] - ) + OpaqueFunction(function=launch_map_transform_publisher_node) # Uncomment to enable static transform publisher ]) From 1c86890739bc80202fa442c28603a8a3401f82a7 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Mon, 9 Feb 2026 13:35:59 +0800 Subject: [PATCH 09/18] docs: changes for testing right camera --- src/aruco_ros/aruco_object/launch/detector_multi.launch.py | 2 +- src/aruco_ros/aruco_object/launch/multi_camera.launch.py | 2 +- src/aruco_ros/aruco_object/launch/scanner_multi.launch.py | 2 +- start_vision_tmux.sh | 3 ++- 4 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py index 30ff0db..c304e49 100644 --- a/src/aruco_ros/aruco_object/launch/detector_multi.launch.py +++ b/src/aruco_ros/aruco_object/launch/detector_multi.launch.py @@ -46,6 +46,6 @@ def launch_setup(context: LaunchContext): def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('log_level', default_value='info'), - DeclareLaunchArgument('default_dock_side', default_value='2'), # 0=front, 1=right, 2=back, 3=left + DeclareLaunchArgument('default_dock_side', default_value='1'), # 0=front, 1=right, 2=back, 3=left OpaqueFunction(function=launch_setup) ]) diff --git a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py index acddeb2..7251350 100644 --- a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py +++ b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py @@ -85,7 +85,7 @@ def generate_launch_description(): ), DeclareLaunchArgument( 'default_active_camera', - default_value='back', + default_value='right', description='Camera to activate at startup (front/back/left/right, empty for dormant)' ), DeclareLaunchArgument( diff --git a/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py index d43a311..b44636c 100644 --- a/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py +++ b/src/aruco_ros/aruco_object/launch/scanner_multi.launch.py @@ -50,6 +50,6 @@ def generate_launch_description(): return LaunchDescription([ DeclareLaunchArgument('log_level', default_value='info'), DeclareLaunchArgument('team_color', default_value='blue'), - DeclareLaunchArgument('default_dock_side', default_value='2'), # 0=front, 1=right, 2=back, 3=left + DeclareLaunchArgument('default_dock_side', default_value='1'), # 0=front, 1=right, 2=back, 3=left OpaqueFunction(function=launch_setup) ]) diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index e999fff..ff69bfc 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -18,7 +18,8 @@ tmux new-session -d -s $SESSION_NAME # === Pane 0: Camera Node === tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back' C-m +# 換預設相機要記得換這個 +tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right' C-m # Split the window horizontally to create a second pane tmux split-window -h -t $SESSION_NAME:0 From adad6e797ac6fca7a36b117a1f2bf9df1adb1356 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Mon, 9 Feb 2026 14:12:46 +0800 Subject: [PATCH 10/18] feat Update tmux script and multi launch script to launch four cameras with bandwidth-safe settings and delay detector/scanner nodes until all cameras are active. --- .../multi_camera.launch.cpython-312.pyc | Bin 0 -> 4112 bytes .../launch/multi_camera.launch.py | 48 ++++++++-- start_vision_tmux.sh | 84 ++++++++++++------ 3 files changed, 96 insertions(+), 36 deletions(-) create mode 100644 src/aruco_ros/aruco_object/launch/__pycache__/multi_camera.launch.cpython-312.pyc diff --git a/src/aruco_ros/aruco_object/launch/__pycache__/multi_camera.launch.cpython-312.pyc b/src/aruco_ros/aruco_object/launch/__pycache__/multi_camera.launch.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..1412b99c0d5c1d457045d48ef9a7125b43802b4c GIT binary patch literal 4112 zcmbtXOKcm*8J;DVujN~!p0?$+Wm%>pin8M-hLa|8Z8esiD2elE!qj0`oDsG1KG@l% zZLt)f76$5|1sb1ZAPFD@J*2P?Ir^AmfnKO8fr+h*1ZXb3F_C}*IrX2}U0Sviv_V(m z>^%OR|1tmnfB*a~nUoMbf2Z%M@Ae|}Pd4zMfY;e>KxYkMgar-hLP^jArGUW30WGMD zC6V`oT1XFakKxmr9ZzFU9plDZ$r5T2fDyQoJA5(xo))6VWnywv>gr zCcP@=RO($0=i~okg5(2E_C=*`R2I(?t3uV9t(u0Dbf?akhE1;7dBG9q zOibY2+g(^tt97a{>U_xQJ4-5>LW#S2npW#NF>EI_bB$E$HhG1b6=GRV>gAg9>pJ;4 zu;Xk7XN`(h$KGeV1&F_{>V%%=WWaXcE6etxX*?$AYi6BR$Q)4YsS>+fQ!1C0Dk)ow zAWIpmlvHe!E<2Hmt4KxgY|mZ^_yys&{V}v_s0!!sv9Kc8T^by??e&)!-6KyE`Kv2| zW`OoL15M$IKzrVa@PF_*CL(*N ztApH3+&R#-Na_bHl6oK~M8ufAr~CauJcS zuYLsaSSb6wM-rMUH!HoiHRP zNT!Wk#nk6jgJ4KwRMD-;WkuJVm}XYX8o5F=N3w~cmn)`bQqW4P6DPb(G|jx)*#EfX z4`@N??*z#SyY^}%s8x2mleOiB^wf!|i4!NMCXW3C48}yuN1TkScG&>CSddeR6W=w{ z7^KyCPxwhLO}R$R1yv(Xf*8uY2AP-C?8WX#H5RDiB)v60I`;7JI3)0lD^$6Ao=&f8 zIZSMx#>)mkkdtu@2`s!>C+*4RACeOW93#YNNQDA5tpaRlHR6Q=7_z_(nGZV=4=Egp zaD-w~J0jE9k+}6?cWOCdvu@YwwiA0GRMr)WnpewKMKKIQou0?0%J~320?toa1XbNA z$V~&(hHHeXCUzps^Gw5H-f1m10)$xq9N56d9-aoIL&YDKQq=jGx4Tm z9!|gnIcf@=n7CxB24yfq89F*4)h0TaL8;YfrqQO%wF}D*&{rOgYdb98Jy}sFvzS+NyZ}weWo%<$=qW!lfTT{~;!_!-- zf!6T!XCtlYS2{>|RhVo~KG!Nd-+KOyPEZ*7g|Ll;^jiWf#oiLW&OGtS+-ByP)!D6F zu{CvOGk13NrLCUa+W5`!wTYV(x0XLS(mHW|v**IlZdNQ};3_H!>&h4s2#l z!)Mt;>%ZL09>148wUIq__vy{-nbq^1FdCNE2U~+jTVh{3l5Gtg-H06Jay@hR{M~1P z_cU&r__6?_pf{r9qH_c{X2)yh};r~J&uXc1aM6EwsQyD0~77u(e-odr&`&`uQGeu z*@NrnTA4y86dYt+b9>tZqwAMGoo)3$x6>+;afN87z)>x=89R(6W*d}!kC`HjpAKX&IG*ja9;d)k@9JE34-c=aU^E0p;C+;8XJ z|LN!Afp5a#8(;SyYG(&-<=VY(w@Qnxx7C(vwz4%aeU=Je_3qh;hy&7%ct`S0^PA73 zqhG~)R_C_C3sHqN{u6@^D<`+FL$ih+6o+=VFvbE7;NTt6FS~ZFgrEe9eD4aBsaV>z z(k-2BuOvd~PPfdeihe-=#Xw??rwc~}I58Ws7tN~d=JtRCLGPkFBoV;KI7GgGuc0}R zpR%&AaqbLHAdqEco?R82$chcQRiO}#@)2G<6j|+1WTiuqR}K>%xI-G1F{Dz(u=6}) z(P6gd>DK|EWe$n*5eCpj7oYQTL1uZ1>HzPSe05Q+EXt?p8x>QYJ?qvjz!okr7T{oX z!Mh#hMary8*sP53~p8cTWv+s&~3m1Ak6@9xZeZWJki=D4U3MP&O3d zlH_xAA24ub6`|+!czF#_Y|_Iy4joR;%Y9aT-m+D@&LPkRr>g+xg9gG!5WYlD-bbVN(TOk7)i2Q1`{=?yQ0zWB(+Mkr(0i+}jo5qp KT(C!QZS)^*0bM@; literal 0 HcmV?d00001 diff --git a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py index 7251350..af1d853 100644 --- a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py +++ b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py @@ -1,7 +1,8 @@ from launch import LaunchDescription, LaunchContext from launch_ros.actions import Node from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction +from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, IncludeLaunchDescription, TimerAction +from launch.launch_description_sources import PythonLaunchDescriptionSource import os from ament_index_python.packages import get_package_share_directory @@ -21,6 +22,7 @@ def launch_setup(context: LaunchContext): raise Exception(f"\n\n[ERROR] Invalid camera position: '{default_camera}'. \n" f"Valid options are: {camera_positions} or an empty string.\n") pkg_share = get_package_share_directory('aruco_object') + pkg_realsense = get_package_share_directory('realsense2_camera') config_file = os.path.join(pkg_share, 'config', 'combined_params.yaml') log_level = LaunchConfiguration('log_level').perform(context) @@ -32,7 +34,28 @@ def launch_setup(context: LaunchContext): raise Exception(f"\n\n[ERROR] Invalid team_color: '{team_color}'. \n" f"Valid options are: {valid_team_colors}\n") - nodes = [] + actions = [] + + # ========== PHASE 1: Launch 4 cameras with bandwidth-safe settings ========== + for position in camera_positions: + camera = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + os.path.join(pkg_realsense, 'launch', 'rs_launch.py') + ), + launch_arguments={ + 'camera_namespace': position, + 'camera_name': position, + 'rgb_camera.color_profile': '640,480,15', # Low bandwidth: 640x480 @ 15fps + 'enable_depth': 'false', # Disable depth to save USB bandwidth + 'enable_infra': 'false', + 'enable_infra1': 'false', + 'enable_infra2': 'false', + }.items() + ) + actions.append(camera) + + # ========== PHASE 2: Delayed detector/scanner nodes (wait for cameras) ========== + detector_scanner_nodes = [] # Launch 4 object_detector nodes for position in camera_positions: @@ -47,7 +70,7 @@ def launch_setup(context: LaunchContext): output='screen', arguments=['--ros-args', '--log-level', log_level] ) - nodes.append(node) + detector_scanner_nodes.append(node) # Launch 4 aruco_row_scanner nodes for position in camera_positions: @@ -62,18 +85,26 @@ def launch_setup(context: LaunchContext): output='screen', arguments=['--ros-args', '--log-level', log_level] ) - nodes.append(node) + detector_scanner_nodes.append(node) + + # Wrap detector/scanner nodes in TimerAction to delay 5 seconds after cameras start + delayed_nodes = TimerAction( + period=5.0, + actions=detector_scanner_nodes + ) + actions.append(delayed_nodes) - # Publish initial active camera after a short delay (to let nodes start) + # ========== PHASE 3: Publish initial active camera ========== if default_camera: + # Additional delay to ensure detectors are ready before publishing active camera publish_cmd = ExecuteProcess( cmd=['bash', '-c', - f'sleep 2 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], + f'sleep 7 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], output='screen' ) - nodes.append(publish_cmd) + actions.append(publish_cmd) - return nodes + return actions def generate_launch_description(): @@ -95,4 +126,3 @@ def generate_launch_description(): ), OpaqueFunction(function=launch_setup) ]) - diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index ff69bfc..a1619c5 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -15,72 +15,102 @@ fi # Create a new tmux session named 'vision' but don't attach to it yet tmux new-session -d -s $SESSION_NAME -# === Pane 0: Camera Node === +# Camera positions and bandwidth-safe settings +CAMERA_POSITIONS=("front" "back" "left" "right") +CAMERA_SETTINGS='rgb_camera.color_profile:=640,480,15 enable_depth:=false enable_infra:=false' + +# === Window 0: Cameras (4 panes, one for each camera) === +# Pane 0: First camera (front) tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m sleep 1 -# 換預設相機要記得換這個 -tmux send-keys -t $SESSION_NAME:0 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right' C-m +tmux send-keys -t $SESSION_NAME:0 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=front camera_name:=front $CAMERA_SETTINGS" C-m -# Split the window horizontally to create a second pane +# Split horizontally and launch second camera (back) tmux split-window -h -t $SESSION_NAME:0 +tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m +sleep 1 +tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back camera_name:=back $CAMERA_SETTINGS" C-m + +# Split pane 0 vertically for third camera (left) +tmux split-window -v -t $SESSION_NAME:0.0 +tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m +sleep 1 +tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=left camera_name:=left $CAMERA_SETTINGS" C-m + +# Split pane 2 vertically for fourth camera (right) +tmux split-window -v -t $SESSION_NAME:0.2 +tmux send-keys -t $SESSION_NAME:0.3 'docker exec -it vision-ws bash' C-m +sleep 1 +tmux send-keys -t $SESSION_NAME:0.3 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right camera_name:=right $CAMERA_SETTINGS" C-m + +# === Window 1: Detector & Scanner === +tmux new-window -t $SESSION_NAME:1 -n 'nodes' -# === Pane 1: Object Detector (waits for camera) === +# === Pane 0: Object Detector (waits for all 4 cameras) === DETECTOR_CMD=' export ROS_DOMAIN_ID=13 source /opt/ros/humble/setup.bash source install/setup.bash -echo "=== [Detector] Waiting for camera topics... ===" +echo "=== [Detector] Waiting for all 4 camera topics... ===" while true; do - TOPICS=$(ros2 topic list 2>/dev/null | grep -E "/camera/color/camera_info") - if [ -n "$TOPICS" ]; then - echo "=== [Detector] Camera found: $TOPICS ===" + FRONT=$(ros2 topic list 2>/dev/null | grep -E "^/front/color/camera_info$") + BACK=$(ros2 topic list 2>/dev/null | grep -E "^/back/color/camera_info$") + LEFT=$(ros2 topic list 2>/dev/null | grep -E "^/left/color/camera_info$") + RIGHT=$(ros2 topic list 2>/dev/null | grep -E "^/right/color/camera_info$") + if [ -n "$FRONT" ] && [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then + echo "=== [Detector] All 4 cameras found! ===" break fi + echo "[Detector] Waiting... (front:${FRONT:-missing} back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})" sleep 2 done -sleep 1 +sleep 2 echo "=== [Detector] Launching ArUco Detector ===" ros2 launch aruco_object detector_multi.launch.py ' -tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m +tmux send-keys -t $SESSION_NAME:1 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "$DETECTOR_CMD" C-m +tmux send-keys -t $SESSION_NAME:1 "$DETECTOR_CMD" C-m -# Split pane 1 vertically to create pane 2 -tmux split-window -v -t $SESSION_NAME:0.1 +# Split for Scanner pane +tmux split-window -h -t $SESSION_NAME:1 -# === Pane 2: Row Scanner (waits for camera) === +# === Pane 1: Row Scanner (waits for all 4 cameras) === SCANNER_CMD=' export ROS_DOMAIN_ID=13 source /opt/ros/humble/setup.bash source install/setup.bash -echo "=== [Scanner] Waiting for camera topics... ===" +echo "=== [Scanner] Waiting for all 4 camera topics... ===" while true; do - TOPICS=$(ros2 topic list 2>/dev/null | grep -E "/camera/color/camera_info") - if [ -n "$TOPICS" ]; then - echo "=== [Scanner] Camera found: $TOPICS ===" + FRONT=$(ros2 topic list 2>/dev/null | grep -E "^/front/color/camera_info$") + BACK=$(ros2 topic list 2>/dev/null | grep -E "^/back/color/camera_info$") + LEFT=$(ros2 topic list 2>/dev/null | grep -E "^/left/color/camera_info$") + RIGHT=$(ros2 topic list 2>/dev/null | grep -E "^/right/color/camera_info$") + if [ -n "$FRONT" ] && [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then + echo "=== [Scanner] All 4 cameras found! ===" break fi + echo "[Scanner] Waiting... (front:${FRONT:-missing} back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})" sleep 2 done -sleep 1 +sleep 2 echo "=== [Scanner] Launching ArUco Scanner ===" ros2 launch aruco_object scanner_multi.launch.py ' -tmux send-keys -t $SESSION_NAME:0.2 'docker exec -it vision-ws bash' C-m +tmux send-keys -t $SESSION_NAME:1.1 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.2 "$SCANNER_CMD" C-m +tmux send-keys -t $SESSION_NAME:1.1 "$SCANNER_CMD" C-m -# Create a new window for Rviz -tmux new-window -t $SESSION_NAME:1 -n 'rviz' -tmux send-keys -t $SESSION_NAME:1 'docker exec -it vision-ws bash' C-m +# === Window 2: RViz === +tmux new-window -t $SESSION_NAME:2 -n 'rviz' +tmux send-keys -t $SESSION_NAME:2 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:1 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash' C-m -tmux send-keys -t $SESSION_NAME:1 'ros2 run rviz2 rviz2' C-m +tmux send-keys -t $SESSION_NAME:2 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash' C-m +tmux send-keys -t $SESSION_NAME:2 'ros2 run rviz2 rviz2' C-m # Function to kill the session when script exits cleanup() { From 2bb005b5197d4b110f0d1dea51540e17c4d977a4 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Mon, 9 Feb 2026 15:42:34 +0800 Subject: [PATCH 11/18] docs: update README.md for the newest multi camera launch --- README.md | 90 +++++++++++++++++++++++++++++++++---------------------- 1 file changed, 54 insertions(+), 36 deletions(-) diff --git a/README.md b/README.md index aadb85c..94650c8 100644 --- a/README.md +++ b/README.md @@ -1,58 +1,76 @@ # 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 -``` -bash start_vision_tmux.sh -``` +### 2. Launch Vision System -### Run Object Detector with Parameters -You can run the object detector node with customizable parameters using the launch file: +**Option A: tmux (recommended for debugging)** ```bash -ros2 launch aruco_object object_detector.launch.py +./start_vision_tmux.sh ``` -This will load parameters from `src/aruco_ros/aruco_object/config/params.yaml`. +- Window 0: 4 camera panes (front, back, left, right) +- Window 1: Detector + Scanner nodes +- Window 2: RViz -You can also override parameters from the command line: +**Option B: Single launch file** ```bash -ros2 launch aruco_object object_detector.launch.py cluster_radius:=0.5 +# Inside Docker container +source /opt/ros/humble/setup.bash +colcon build +source install/setup.bash +ros2 launch aruco_object multi_camera.launch.py ``` -### Run rviz -Before going in the container, run this in terminal to allow allow GUI window inside container -``` -xhost +local:docker -``` -Open RViz to visualize camera images and TF frames: -```bash -ros2 run rviz2 rviz2 -``` +## 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 | -### switch to another camera when using multiple cameras -Since only one camera is active at a time, you can switch to another camera by publishing to /active_camera topic. -#### Switch to front camera +Example: ```bash -ros2 topic pub /active_camera std_msgs/msg/String "data: 'front'" -1 +ros2 launch aruco_object multi_camera.launch.py default_active_camera:=front team_color:=yellow ``` -#### Switch to back camera + +## 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 /active_camera std_msgs/msg/String "data: 'back'" -1 +ros2 topic pub /robot/dock_side std_msgs/msg/Int16 "data: 2" -1 # Switch to back ``` -#### Switch to left camera + +## RViz + ```bash -ros2 topic pub /active_camera std_msgs/msg/String "data: 'left'" -1 +xhost +local:docker # Run BEFORE entering container +ros2 run rviz2 rviz2 ``` -#### Switch to right camera -```bash -ros2 topic pub /active_camera std_msgs/msg/String "data: 'right'" -1 + +## Architecture + ``` -#### Make all cameras dormant (empty string) -```bash -ros2 topic pub /active_camera std_msgs/msg/String "data: ''" -1 +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) ``` + +All nodes subscribe to `/robot/dock_side` - only the matching camera side processes images. From e7287b8067a629a3baaa6132501312ed0ce7a15e Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Mon, 9 Feb 2026 19:52:28 +0800 Subject: [PATCH 12/18] docs: update camera launch --- README.md | 35 +++++++++++++++++++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/README.md b/README.md index 94650c8..5366cf2 100644 --- a/README.md +++ b/README.md @@ -74,3 +74,38 @@ start_vision_tmux.sh / multi_camera.launch.py ``` 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) +```bash +ros2 launch realsense2_camera rs_launch.py \ + camera_namespace:=front camera_name:=front \ + rgb_camera.color_profile:=640,480,15 enable_depth:=false +``` +Replace `front` with: `front`, `back`, `left`, or `right` + +### Object Detector Node +```bash +ros2 run aruco_object aruco_detector_node --ros-args \ + -p camera_position:=front +``` + +### 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 +``` + From e4650357acdaa4d82bf1c25c83abbbf771ad3d6e Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Mon, 9 Feb 2026 20:59:47 +0800 Subject: [PATCH 13/18] fix: correct the topic names for multi camera launch --- src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp | 4 +++- src/aruco_ros/aruco_object/src/object_detector.cpp | 7 ++++--- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp index d5bb1d0..f9f7ed1 100644 --- a/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp +++ b/src/aruco_ros/aruco_object/src/aruco_row_scanner_node.cpp @@ -17,7 +17,9 @@ this->declare_parameter("camera_position", "front"); camera_position_ = this->get_parameter("camera_position").as_string(); // Build namespaced topic name based on camera position - std::string image_topic = "/" + camera_position_ + "/camera/color/image_rect_raw"; + // Topic format: /{camera_namespace}/{camera_name}/color/image_rect_raw + // Both camera_namespace and camera_name are set to camera_position in launch + std::string image_topic = "/" + camera_position_ + "/" + camera_position_ + "/color/image_rect_raw"; RCLCPP_INFO(this->get_logger(), "Scanner [%s] (side=%d) subscribing to: %s", camera_position_.c_str(), my_side_, image_topic.c_str()); // Map camera position string to dock_side number diff --git a/src/aruco_ros/aruco_object/src/object_detector.cpp b/src/aruco_ros/aruco_object/src/object_detector.cpp index f21edd5..c12bc52 100644 --- a/src/aruco_ros/aruco_object/src/object_detector.cpp +++ b/src/aruco_ros/aruco_object/src/object_detector.cpp @@ -28,9 +28,10 @@ CameraOnBoardNode::CameraOnBoardNode() my_side_ = position_to_side_[CAMERA_POSITION_]; // Build namespaced topic names based on camera position - // e.g., "front" -> "/front/camera/color/image_rect_raw" - std::string image_topic = "/" + CAMERA_POSITION_ + "/camera/color/image_rect_raw"; - std::string camera_info_topic = "/" + CAMERA_POSITION_ + "/camera/color/camera_info"; + // Topic format: /{camera_namespace}/{camera_name}/color/... + // Both camera_namespace and camera_name are set to CAMERA_POSITION_ in launch + std::string image_topic = "/" + CAMERA_POSITION_ + "/" + CAMERA_POSITION_ + "/color/image_rect_raw"; + std::string camera_info_topic = "/" + CAMERA_POSITION_ + "/" + CAMERA_POSITION_ + "/color/camera_info"; RCLCPP_INFO(this->get_logger(), "Detector [%s] (side=%d) subscribing to: %s", CAMERA_POSITION_.c_str(), my_side_, image_topic.c_str()); From 0b5a64c569c6a6708de44abf6df099b00378e93e Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Tue, 10 Feb 2026 01:47:05 +0800 Subject: [PATCH 14/18] feat: Correct yaw calculation after real robot test. Also, enhanced multi-camera launch with serial number support and update tmux script for camera initialization. --- README.md | 43 ++++++++++++++++--- .../launch/multi_camera.launch.py | 13 ++++-- .../aruco_object/src/object_detector.cpp | 3 +- .../aruco_object/src/process_logic.cpp | 6 +-- .../realsense2_camera/launch/rs_launch.py | 4 +- .../launch/rs_multi_camera_launch.py | 41 ++++++++++++------ start_vision_tmux.sh | 15 +++++-- 7 files changed, 94 insertions(+), 31 deletions(-) diff --git a/README.md b/README.md index 5366cf2..975e903 100644 --- a/README.md +++ b/README.md @@ -80,17 +80,16 @@ All nodes subscribe to `/robot/dock_side` - only the matching camera side proces Launch individual camera, detector, or scanner nodes with camera position: ### Camera Node (RealSense) -```bash -ros2 launch realsense2_camera rs_launch.py \ - camera_namespace:=front camera_name:=front \ - rgb_camera.color_profile:=640,480,15 enable_depth:=false + +start docker first +``` +docker exec -it vision-ws bash ``` -Replace `front` with: `front`, `back`, `left`, or `right` ### Object Detector Node ```bash ros2 run aruco_object aruco_detector_node --ros-args \ - -p camera_position:=front + -p camera_position:=left ``` ### Row Scanner Node @@ -109,3 +108,35 @@ ros2 launch aruco_object detector_multi.launch.py default_dock_side:=1 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 +``` diff --git a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py index af1d853..b212db1 100644 --- a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py +++ b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py @@ -10,8 +10,14 @@ def launch_setup(context: LaunchContext): """Called after launch arguments are resolved""" - # Define valid camera positions - camera_positions = ['front', 'back', 'left', 'right'] + # Define valid camera positions and their serial numbers + camera_configs = { + 'front': '313522070126', + 'back': '419122270813', + 'left': '218622276534', + 'right': '218622276687' + } + camera_positions = list(camera_configs.keys()) # 1. Grab the value default_camera = LaunchConfiguration('default_active_camera').perform(context) @@ -37,7 +43,7 @@ def launch_setup(context: LaunchContext): actions = [] # ========== PHASE 1: Launch 4 cameras with bandwidth-safe settings ========== - for position in camera_positions: + for position, serial_no in camera_configs.items(): camera = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_realsense, 'launch', 'rs_launch.py') @@ -45,6 +51,7 @@ def launch_setup(context: LaunchContext): launch_arguments={ 'camera_namespace': position, 'camera_name': position, + 'serial_no': serial_no, 'rgb_camera.color_profile': '640,480,15', # Low bandwidth: 640x480 @ 15fps 'enable_depth': 'false', # Disable depth to save USB bandwidth 'enable_infra': 'false', diff --git a/src/aruco_ros/aruco_object/src/object_detector.cpp b/src/aruco_ros/aruco_object/src/object_detector.cpp index c12bc52..7b8a7f4 100644 --- a/src/aruco_ros/aruco_object/src/object_detector.cpp +++ b/src/aruco_ros/aruco_object/src/object_detector.cpp @@ -109,7 +109,8 @@ void CameraOnBoardNode::image_callback(const sensor_msgs::msg::Image::SharedPtr // Optimize TF lookup: Get transform ONCE for this frame timestamp geometry_msgs::msg::TransformStamped cam_to_base; try { - cam_to_base = tf_buffer_->lookupTransform("base_footprint", "camera_color_optical_frame", + std::string camera_frame = CAMERA_POSITION_ + "_color_optical_frame"; + cam_to_base = tf_buffer_->lookupTransform("base_footprint", camera_frame, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); } catch (tf2::TransformException &ex) { diff --git a/src/aruco_ros/aruco_object/src/process_logic.cpp b/src/aruco_ros/aruco_object/src/process_logic.cpp index cd702f7..7e732f4 100644 --- a/src/aruco_ros/aruco_object/src/process_logic.cpp +++ b/src/aruco_ros/aruco_object/src/process_logic.cpp @@ -132,13 +132,13 @@ geometry_msgs::msg::PoseStamped ProcessLogic::compute_perpendicular_pose_from_fl double final_yaw; if (camera_position_ == "back") { // Robot back faces wall, so front points away (opposite to normal + 180°) - final_yaw = perp_angle + M_PI - M_PI_2; // = perp_angle + PI/2 + final_yaw = perp_angle + M_PI_2; // = perp_angle + PI/2 } else if (camera_position_ == "left") { // Robot left faces wall - final_yaw = perp_angle + M_PI_2 - M_PI_2; // = perp_angle + final_yaw = perp_angle + M_PI; // = perp_angle } else if (camera_position_ == "right") { // Robot right faces wall - final_yaw = perp_angle - M_PI_2 - M_PI_2; // = perp_angle - PI + final_yaw = perp_angle; // = perp_angle - PI } else { // Robot front faces wall final_yaw = perp_angle - M_PI_2; diff --git a/src/realsense_ros/realsense2_camera/launch/rs_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_launch.py index 2dd8ace..4936a75 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_launch.py @@ -139,6 +139,8 @@ def launch_setup(context, params, param_name_suffix=''): ) ] +# 2: '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571' +# 3: '-0.125', '0.0', '0.30', '-3.142', '0.7854', '-1.571' def launch_map_transform_publisher_node(context: LaunchContext): node = launch_ros.actions.Node( name='map_transform_publisher', @@ -146,7 +148,7 @@ def launch_map_transform_publisher_node(context: LaunchContext): executable="static_transform_publisher", arguments=[ # 新的紅黑機數值 - '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571', + '-0.125', '0.0', '0.30', '-3.142', '0.7854', '-1.571', 'base_footprint', context.launch_configurations['camera_name'] + '_link' ] diff --git a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py index d7a2205..4476a8f 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py @@ -35,10 +35,12 @@ sys.path.append(str(pathlib.Path(__file__).parent.absolute())) import rs_launch -local_parameters = [{'name': 'camera_name1', 'default': 'camera1', 'description': 'camera1 unique name'}, - {'name': 'camera_name2', 'default': 'camera2', 'description': 'camera2 unique name'}, - {'name': 'camera_namespace1', 'default': 'camera1', 'description': 'camera1 namespace'}, - {'name': 'camera_namespace2', 'default': 'camera2', 'description': 'camera2 namespace'}, +local_parameters = [{'name': 'camera_name1', 'default': 'back', 'description': 'camera1 unique name'}, + {'name': 'camera_name2', 'default': 'left', 'description': 'camera2 unique name'}, + {'name': 'camera_namespace1', 'default': 'back', 'description': 'camera1 namespace'}, + {'name': 'camera_namespace2', 'default': 'left', 'description': 'camera2 namespace'}, + {'name': 'serial_no1', 'default': '_419122270813', 'description': 'camera1 serial number'}, + {'name': 'serial_no2', 'default': '_218622276534', 'description': 'camera2 serial number'}, ] def yaml_to_dict(path_to_yaml): @@ -67,16 +69,29 @@ def launch_static_transform_publisher_node(context : LaunchContext): log_message = "Launching as LifecycleNode" if use_lifecycle_node else "Launching as Normal ROS Node" # dummy static transformation from camera1 to camera2 - node = node_action( - namespace="", - name="tf2_static_transform_publisher", - package = "tf2_ros", - executable = "static_transform_publisher", - arguments = ["0", "0", "0", "0", "0", "0", - context.launch_configurations['camera_name1'] + "_link", - context.launch_configurations['camera_name2'] + "_link"] + node2 = node_action( + namespace="", + name="tf2_static_transform_publisher", + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571', + 'base_footprint', + context.launch_configurations['camera_name1'] + '_link' + ] ) - return [LogInfo(msg=f"🚀 {log_message}"), node] + node3 = node_action( + namespace="", + name="tf2_static_transform_publisher", + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '-0.125', '0.0', '0.30', '-3.142', '0.7854', '-1.571', + 'base_footprint', + context.launch_configurations['camera_name2'] + '_link' + ] + ) + return [LogInfo(msg=f"🚀 {log_message}"), node2, node3] def generate_launch_description(): params1 = duplicate_params(rs_launch.configurable_parameters, '1') diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index a1619c5..65fe1b5 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -15,33 +15,40 @@ fi # Create a new tmux session named 'vision' but don't attach to it yet tmux new-session -d -s $SESSION_NAME +# Camera positions and bandwidth-safe settings # Camera positions and bandwidth-safe settings CAMERA_POSITIONS=("front" "back" "left" "right") CAMERA_SETTINGS='rgb_camera.color_profile:=640,480,15 enable_depth:=false enable_infra:=false' +# Camera Serial Numbers +FRONT_SERIAL="313522070126" +BACK_SERIAL="419122270813" +LEFT_SERIAL="218622276534" +RIGHT_SERIAL="218622276687" + # === Window 0: Cameras (4 panes, one for each camera) === # Pane 0: First camera (front) tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=front camera_name:=front $CAMERA_SETTINGS" C-m +tmux send-keys -t $SESSION_NAME:0 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=front camera_name:=front serial_no:=$FRONT_SERIAL $CAMERA_SETTINGS" C-m # Split horizontally and launch second camera (back) tmux split-window -h -t $SESSION_NAME:0 tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back camera_name:=back $CAMERA_SETTINGS" C-m +tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back camera_name:=back serial_no:=$BACK_SERIAL $CAMERA_SETTINGS" C-m # Split pane 0 vertically for third camera (left) tmux split-window -v -t $SESSION_NAME:0.0 tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=left camera_name:=left $CAMERA_SETTINGS" C-m +tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=left camera_name:=left serial_no:=$LEFT_SERIAL $CAMERA_SETTINGS" C-m # Split pane 2 vertically for fourth camera (right) tmux split-window -v -t $SESSION_NAME:0.2 tmux send-keys -t $SESSION_NAME:0.3 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:0.3 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right camera_name:=right $CAMERA_SETTINGS" C-m +tmux send-keys -t $SESSION_NAME:0.3 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right camera_name:=right serial_no:=$RIGHT_SERIAL $CAMERA_SETTINGS" C-m # === Window 1: Detector & Scanner === tmux new-window -t $SESSION_NAME:1 -n 'nodes' From e5c4be49f415950533d55886cccce553b75c956f Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Wed, 11 Feb 2026 19:22:19 +0800 Subject: [PATCH 15/18] feat: add white_back_camera.sh script for managing tmux sessions and launching camera nodes --- .../realsense2_camera/launch/rs_launch.py | 2 +- white_back_camera.sh | 67 +++++++++++++++++++ 2 files changed, 68 insertions(+), 1 deletion(-) create mode 100644 white_back_camera.sh diff --git a/src/realsense_ros/realsense2_camera/launch/rs_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_launch.py index 4936a75..1d2d0de 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_launch.py @@ -148,7 +148,7 @@ def launch_map_transform_publisher_node(context: LaunchContext): executable="static_transform_publisher", arguments=[ # 新的紅黑機數值 - '-0.125', '0.0', '0.30', '-3.142', '0.7854', '-1.571', + '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571', 'base_footprint', context.launch_configurations['camera_name'] + '_link' ] diff --git a/white_back_camera.sh b/white_back_camera.sh new file mode 100644 index 0000000..32d69a2 --- /dev/null +++ b/white_back_camera.sh @@ -0,0 +1,67 @@ +#!/bin/bash + +SESSION="white_back_camera_test" + +# Check if session exists and kill it if it does +tmux has-session -t $SESSION 2>/dev/null +if [ $? -eq 0 ]; then + echo "Session $SESSION already exists. Killing it." + tmux kill-session -t $SESSION +fi + +# Create a new detached session +tmux new-session -d -s $SESSION + +# Get the window ID to be robust against base-index settings +WIN_ID=$(tmux list-windows -t $SESSION -F "#{window_index}" | head -n 1) + +# Split the window into 4 panes in a grid +# Start with Pane 0 (Whole screen) + +# 1. Split vertically: Top (0) and Bottom (1) +tmux split-window -v -t ${SESSION}:${WIN_ID}.0 + +# 2. Split Top (0) horizontally: TL (0) and TR (1). Old Bottom becomes 2. +tmux split-window -h -t ${SESSION}:${WIN_ID}.0 + +# 3. Split Bottom (2) horizontally: BL (2) and BR (3). +tmux split-window -h -t ${SESSION}:${WIN_ID}.2 + +# Ensure tiled layout +tmux select-layout -t ${SESSION}:${WIN_ID} tiled + +# Common commands for ALL panes +for i in {0..3}; do + TARGET="${SESSION}:${WIN_ID}.${i}" + tmux send-keys -t $TARGET "docker exec -it vision-ws bash" C-m + tmux send-keys -t $TARGET "export ROS_DOMAIN_ID=11" C-m + tmux send-keys -t $TARGET "colcon build" C-m + tmux send-keys -t $TARGET "source install/setup.bash" C-m + tmux send-keys -t $TARGET "clear" C-m +done + +# Upper Left (Pane 0) +tmux send-keys -t ${SESSION}:${WIN_ID}.0 "ros2 launch realsense2_camera rs_launch.py \ + camera_namespace:=back \ + camera_name:=back \ + serial_no:=\"'218622278918'\" \ + rgb_camera.color_profile:=640,480,15 \ + enable_depth:=false" C-m + +# Upper Right (Pane 1) +tmux send-keys -t ${SESSION}:${WIN_ID}.1 "ros2 run aruco_object aruco_row_scanner --ros-args \ + -p camera_position:=back \ + -p team_color:=yellow" C-m + +# Lower Left (Pane 2) +tmux send-keys -t ${SESSION}:${WIN_ID}.2 "ros2 run aruco_object aruco_detector_node --ros-args \ + -p camera_position:=back" C-m + +# Lower Right (Pane 3) +tmux send-keys -t ${SESSION}:${WIN_ID}.3 "ros2 topic hz /detected_dock_pose" C-m + +# Attach to session +tmux attach -t $SESSION + +# Kill session when detached (script continues here after user detaches) +tmux kill-session -t $SESSION From 2dc4d00b210362b2d0b0fb1c9f0b23d6eb3b67a6 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Wed, 11 Feb 2026 23:16:34 +0800 Subject: [PATCH 16/18] refactor: update multi-camera launch scripts for improved organization and clarity. Also, closed depth and other modalities for better bandwidth --- .../launch/multi_camera.launch.py | 135 ------------------ .../realsense2_camera/launch/rs_launch.py | 2 + .../launch/rs_multi_camera_launch.py | 33 ++++- start_docker.sh | 5 +- start_vision_tmux.sh | 121 +++++++--------- 5 files changed, 93 insertions(+), 203 deletions(-) delete mode 100644 src/aruco_ros/aruco_object/launch/multi_camera.launch.py diff --git a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py b/src/aruco_ros/aruco_object/launch/multi_camera.launch.py deleted file mode 100644 index b212db1..0000000 --- a/src/aruco_ros/aruco_object/launch/multi_camera.launch.py +++ /dev/null @@ -1,135 +0,0 @@ -from launch import LaunchDescription, LaunchContext -from launch_ros.actions import Node -from launch.substitutions import LaunchConfiguration -from launch.actions import DeclareLaunchArgument, ExecuteProcess, OpaqueFunction, IncludeLaunchDescription, TimerAction -from launch.launch_description_sources import PythonLaunchDescriptionSource -import os -from ament_index_python.packages import get_package_share_directory - - -def launch_setup(context: LaunchContext): - """Called after launch arguments are resolved""" - - # Define valid camera positions and their serial numbers - camera_configs = { - 'front': '313522070126', - 'back': '419122270813', - 'left': '218622276534', - 'right': '218622276687' - } - camera_positions = list(camera_configs.keys()) - - # 1. Grab the value - default_camera = LaunchConfiguration('default_active_camera').perform(context) - - # 2. VALIDATION LOGIC - # Allow empty string if 'dormant' is intended, otherwise check the list - if default_camera and default_camera not in camera_positions: - raise Exception(f"\n\n[ERROR] Invalid camera position: '{default_camera}'. \n" - f"Valid options are: {camera_positions} or an empty string.\n") - pkg_share = get_package_share_directory('aruco_object') - pkg_realsense = get_package_share_directory('realsense2_camera') - config_file = os.path.join(pkg_share, 'config', 'combined_params.yaml') - - log_level = LaunchConfiguration('log_level').perform(context) - team_color = LaunchConfiguration('team_color').perform(context) - - # Validate team_color - valid_team_colors = ['blue', 'yellow'] - if team_color not in valid_team_colors: - raise Exception(f"\n\n[ERROR] Invalid team_color: '{team_color}'. \n" - f"Valid options are: {valid_team_colors}\n") - - actions = [] - - # ========== PHASE 1: Launch 4 cameras with bandwidth-safe settings ========== - for position, serial_no in camera_configs.items(): - camera = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - os.path.join(pkg_realsense, 'launch', 'rs_launch.py') - ), - launch_arguments={ - 'camera_namespace': position, - 'camera_name': position, - 'serial_no': serial_no, - 'rgb_camera.color_profile': '640,480,15', # Low bandwidth: 640x480 @ 15fps - 'enable_depth': 'false', # Disable depth to save USB bandwidth - 'enable_infra': 'false', - 'enable_infra1': 'false', - 'enable_infra2': 'false', - }.items() - ) - actions.append(camera) - - # ========== PHASE 2: Delayed detector/scanner nodes (wait for cameras) ========== - detector_scanner_nodes = [] - - # Launch 4 object_detector 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] - ) - detector_scanner_nodes.append(node) - - # Launch 4 aruco_row_scanner nodes - for position in camera_positions: - node = Node( - package='aruco_object', - executable='aruco_row_scanner', - name=f'aruco_row_scanner_{position}', - parameters=[ - config_file, - {'camera_position': position, 'team_color': team_color} - ], - output='screen', - arguments=['--ros-args', '--log-level', log_level] - ) - detector_scanner_nodes.append(node) - - # Wrap detector/scanner nodes in TimerAction to delay 5 seconds after cameras start - delayed_nodes = TimerAction( - period=5.0, - actions=detector_scanner_nodes - ) - actions.append(delayed_nodes) - - # ========== PHASE 3: Publish initial active camera ========== - if default_camera: - # Additional delay to ensure detectors are ready before publishing active camera - publish_cmd = ExecuteProcess( - cmd=['bash', '-c', - f'sleep 7 && ros2 topic pub /active_camera std_msgs/msg/String "data: \'{default_camera}\'" -1'], - output='screen' - ) - actions.append(publish_cmd) - - return actions - - -def generate_launch_description(): - return LaunchDescription([ - DeclareLaunchArgument( - 'log_level', - default_value='info', - description='Log level' - ), - DeclareLaunchArgument( - 'default_active_camera', - default_value='right', - description='Camera to activate at startup (front/back/left/right, empty for dormant)' - ), - DeclareLaunchArgument( - 'team_color', - default_value='blue', - description='Team color (blue/yellow) - determines which ArUco IDs are targets' - ), - OpaqueFunction(function=launch_setup) - ]) diff --git a/src/realsense_ros/realsense2_camera/launch/rs_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_launch.py index 1d2d0de..2ae96a5 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_launch.py @@ -139,6 +139,8 @@ def launch_setup(context, params, param_name_suffix=''): ) ] +# 0: '0.0', '0.125', '0.30', '1.571', '0.7854', '-1.571' 前面相機z應該是錯的 +# 1: '0.125', '0.0', '0.30', '0.0', '0.7854', '-1.571' # 2: '0.0', '-0.125', '0.30', '-1.571', '0.7854', '-1.571' # 3: '-0.125', '0.0', '0.30', '-3.142', '0.7854', '-1.571' def launch_map_transform_publisher_node(context: LaunchContext): diff --git a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py index 4476a8f..23242c3 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py @@ -41,6 +41,9 @@ {'name': 'camera_namespace2', 'default': 'left', 'description': 'camera2 namespace'}, {'name': 'serial_no1', 'default': '_419122270813', 'description': 'camera1 serial number'}, {'name': 'serial_no2', 'default': '_218622276534', 'description': 'camera2 serial number'}, + {'name': 'camera_name3', 'default': 'right', 'description': 'camera3 unique name'}, + {'name': 'camera_namespace3', 'default': 'right', 'description': 'camera3 namespace'}, + {'name': 'serial_no3', 'default': '_218622276687', 'description': 'camera3 serial number'}, ] def yaml_to_dict(path_to_yaml): @@ -55,6 +58,18 @@ def duplicate_params(general_params, posix): for param in local_params: param['original_name'] = param['name'] param['name'] += posix + if param['original_name'] == 'enable_depth': + param['default'] = 'false' + elif param['original_name'] == 'enable_infra': + param['default'] = 'false' + elif param['original_name'] == 'enable_infra1': + param['default'] = 'false' + elif param['original_name'] == 'enable_infra2': + param['default'] = 'false' + elif param['original_name'] == 'enable_gyro': + param['default'] = 'false' + elif param['original_name'] == 'enable_accel': + param['default'] = 'false' return local_params def launch_static_transform_publisher_node(context : LaunchContext): @@ -91,15 +106,28 @@ def launch_static_transform_publisher_node(context : LaunchContext): context.launch_configurations['camera_name2'] + '_link' ] ) - return [LogInfo(msg=f"🚀 {log_message}"), node2, node3] + node4 = node_action( + namespace="", + name="tf2_static_transform_publisher", + package = "tf2_ros", + executable = "static_transform_publisher", + arguments=[ + '0.125', '0.0', '0.30', '0.0', '0.7854', '-1.571', + 'base_footprint', + context.launch_configurations['camera_name3'] + '_link' + ] + ) + return [LogInfo(msg=f"🚀 {log_message}"), node2, node3, node4] def generate_launch_description(): params1 = duplicate_params(rs_launch.configurable_parameters, '1') params2 = duplicate_params(rs_launch.configurable_parameters, '2') + params3 = duplicate_params(rs_launch.configurable_parameters, '3') return LaunchDescription( rs_launch.declare_configurable_parameters(local_parameters) + rs_launch.declare_configurable_parameters(params1) + rs_launch.declare_configurable_parameters(params2) + + rs_launch.declare_configurable_parameters(params3) + [ OpaqueFunction(function=rs_launch.launch_setup, kwargs = {'params' : set_configurable_parameters(params1), @@ -107,5 +135,8 @@ def generate_launch_description(): OpaqueFunction(function=rs_launch.launch_setup, kwargs = {'params' : set_configurable_parameters(params2), 'param_name_suffix': '2'}), + OpaqueFunction(function=rs_launch.launch_setup, + kwargs = {'params' : set_configurable_parameters(params3), + 'param_name_suffix': '3'}), OpaqueFunction(function=launch_static_transform_publisher_node) ]) diff --git a/start_docker.sh b/start_docker.sh index c0d830e..c933b4b 100644 --- a/start_docker.sh +++ b/start_docker.sh @@ -2,5 +2,8 @@ cd docker/ || { echo "Directory 'docker/' not found"; exit 1; } -# Build AND then Up +# Take down running containers and networks first +docker compose down + +# Build and start fresh docker compose build && docker compose up -d \ No newline at end of file diff --git a/start_vision_tmux.sh b/start_vision_tmux.sh index 65fe1b5..68b1234 100644 --- a/start_vision_tmux.sh +++ b/start_vision_tmux.sh @@ -12,112 +12,101 @@ if [ $? == 0 ]; then tmux kill-session -t $SESSION_NAME fi -# Create a new tmux session named 'vision' but don't attach to it yet +# Create a new detached session tmux new-session -d -s $SESSION_NAME -# Camera positions and bandwidth-safe settings -# Camera positions and bandwidth-safe settings -CAMERA_POSITIONS=("front" "back" "left" "right") -CAMERA_SETTINGS='rgb_camera.color_profile:=640,480,15 enable_depth:=false enable_infra:=false' +# 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) -# Camera Serial Numbers -FRONT_SERIAL="313522070126" -BACK_SERIAL="419122270813" -LEFT_SERIAL="218622276534" -RIGHT_SERIAL="218622276687" +# === Window 0: Vision System (2x2 grid) === +# Start with Pane 0 (Top Left) -# === Window 0: Cameras (4 panes, one for each camera) === -# Pane 0: First camera (front) -tmux send-keys -t $SESSION_NAME:0 'docker exec -it vision-ws bash' C-m -sleep 1 -tmux send-keys -t $SESSION_NAME:0 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && colcon build && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=front camera_name:=front serial_no:=$FRONT_SERIAL $CAMERA_SETTINGS" C-m +# 1. Split vertically: Top (0) and Bottom (1) +tmux split-window -v -t ${SESSION_NAME}:${WIN_ID} -# Split horizontally and launch second camera (back) -tmux split-window -h -t $SESSION_NAME:0 -tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m -sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=back camera_name:=back serial_no:=$BACK_SERIAL $CAMERA_SETTINGS" C-m +# 2. Split Top (0) horizontally: TL (0) and TR (1). Old Bottom becomes 2. +tmux split-window -h -t ${SESSION_NAME}:${WIN_ID}.0 -# Split pane 0 vertically for third camera (left) -tmux split-window -v -t $SESSION_NAME:0.0 -tmux send-keys -t $SESSION_NAME:0.1 'docker exec -it vision-ws bash' C-m -sleep 1 -tmux send-keys -t $SESSION_NAME:0.1 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=left camera_name:=left serial_no:=$LEFT_SERIAL $CAMERA_SETTINGS" C-m +# 3. Split Bottom (2) horizontally: BL (2) and BR (3). +tmux split-window -h -t ${SESSION_NAME}:${WIN_ID}.2 -# Split pane 2 vertically for fourth camera (right) -tmux split-window -v -t $SESSION_NAME:0.2 -tmux send-keys -t $SESSION_NAME:0.3 'docker exec -it vision-ws bash' C-m -sleep 1 -tmux send-keys -t $SESSION_NAME:0.3 "export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash && ros2 launch realsense2_camera rs_launch.py camera_namespace:=right camera_name:=right serial_no:=$RIGHT_SERIAL $CAMERA_SETTINGS" C-m +# Ensure tiled layout +tmux select-layout -t ${SESSION_NAME}:${WIN_ID} tiled + +# Common setup for all panes +# Note: Panes are indexed 0 (TL), 1 (TR), 2 (BL), 3 (BR) *after* the splits and select-layout tiled? +# Let's double check standard behavior or rely on targeting. +# With "tiled", indices might shift or be stable. +# Layout logic: +# 0 -> TL +# 1 -> TR +# 2 -> BL +# 3 -> BR +# Let's verify targeting. -# === Window 1: Detector & Scanner === -tmux new-window -t $SESSION_NAME:1 -n 'nodes' +# Pane 0 (Top Left): All Cameras +tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.0 'docker exec -it vision-ws bash' C-m +sleep 1 +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 0: Object Detector (waits for all 4 cameras) === +# Pane 1 (Top Right): Detector +tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.1 'docker exec -it vision-ws bash' C-m +sleep 1 DETECTOR_CMD=' export ROS_DOMAIN_ID=13 source /opt/ros/humble/setup.bash source install/setup.bash -echo "=== [Detector] Waiting for all 4 camera topics... ===" +echo "=== [Detector] Waiting for BACK, LEFT, RIGHT camera topics... ===" while true; do - FRONT=$(ros2 topic list 2>/dev/null | grep -E "^/front/color/camera_info$") - BACK=$(ros2 topic list 2>/dev/null | grep -E "^/back/color/camera_info$") - LEFT=$(ros2 topic list 2>/dev/null | grep -E "^/left/color/camera_info$") - RIGHT=$(ros2 topic list 2>/dev/null | grep -E "^/right/color/camera_info$") - if [ -n "$FRONT" ] && [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then - echo "=== [Detector] All 4 cameras found! ===" + 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... (front:${FRONT:-missing} back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})" + 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 -tmux send-keys -t $SESSION_NAME:1 'docker exec -it vision-ws bash' C-m +# Pane 2 (Bottom Left): Scanner +tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.2 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:1 "$DETECTOR_CMD" C-m - -# Split for Scanner pane -tmux split-window -h -t $SESSION_NAME:1 - -# === Pane 1: Row Scanner (waits for all 4 cameras) === SCANNER_CMD=' export ROS_DOMAIN_ID=13 source /opt/ros/humble/setup.bash source install/setup.bash -echo "=== [Scanner] Waiting for all 4 camera topics... ===" +echo "=== [Scanner] Waiting for BACK, LEFT, RIGHT camera topics... ===" while true; do - FRONT=$(ros2 topic list 2>/dev/null | grep -E "^/front/color/camera_info$") - BACK=$(ros2 topic list 2>/dev/null | grep -E "^/back/color/camera_info$") - LEFT=$(ros2 topic list 2>/dev/null | grep -E "^/left/color/camera_info$") - RIGHT=$(ros2 topic list 2>/dev/null | grep -E "^/right/color/camera_info$") - if [ -n "$FRONT" ] && [ -n "$BACK" ] && [ -n "$LEFT" ] && [ -n "$RIGHT" ]; then - echo "=== [Scanner] All 4 cameras found! ===" + 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... (front:${FRONT:-missing} back:${BACK:-missing} left:${LEFT:-missing} right:${RIGHT:-missing})" + 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 -tmux send-keys -t $SESSION_NAME:1.1 'docker exec -it vision-ws bash' C-m -sleep 1 -tmux send-keys -t $SESSION_NAME:1.1 "$SCANNER_CMD" C-m - -# === Window 2: RViz === -tmux new-window -t $SESSION_NAME:2 -n 'rviz' -tmux send-keys -t $SESSION_NAME:2 'docker exec -it vision-ws bash' C-m +# Pane 3 (Bottom Right): Shell / Utils +tmux send-keys -t ${SESSION_NAME}:${WIN_ID}.3 'docker exec -it vision-ws bash' C-m sleep 1 -tmux send-keys -t $SESSION_NAME:2 'export ROS_DOMAIN_ID=13 && source /opt/ros/humble/setup.bash && source install/setup.bash' C-m -tmux send-keys -t $SESSION_NAME:2 'ros2 run rviz2 rviz2' C-m +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() { @@ -128,5 +117,5 @@ cleanup() { trap cleanup EXIT # Attach to the tmux session to view the output -tmux select-window -t $SESSION_NAME:0 +tmux select-window -t ${SESSION_NAME}:${WIN_ID} tmux attach-session -t $SESSION_NAME From 51597b4bc5e0f3f19b19ef365f0d4c3a2e5973f2 Mon Sep 17 00:00:00 2001 From: kylevirtuous1211 Date: Wed, 11 Feb 2026 23:49:31 +0800 Subject: [PATCH 17/18] feat: add inside_start_vision_tmux.sh script for launching vision sessions in the container --- inside_start_vision_tmux.sh | 101 ++++++++++++++++++++++++++++++++++++ 1 file changed, 101 insertions(+) create mode 100644 inside_start_vision_tmux.sh diff --git a/inside_start_vision_tmux.sh b/inside_start_vision_tmux.sh new file mode 100644 index 0000000..abb99c9 --- /dev/null +++ b/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 From 562a41f2abb65d71a3c789eef65555e6a8c016f9 Mon Sep 17 00:00:00 2001 From: yuhsiang1117 Date: Sat, 28 Feb 2026 17:28:17 +0800 Subject: [PATCH 18/18] fix: adjust param for object feedback - Adjusted the parameter for object feedback to improve accuracy. - Adjusted camera param for object feedback --- src/object_feedback/CMakeLists.txt | 6 +++--- src/object_feedback/launch/obj_fb_bringup.launch.py | 3 ++- src/object_feedback/src/feedback_all.cpp | 2 +- .../realsense2_camera/launch/rs_multi_camera_launch.py | 4 ++++ 4 files changed, 10 insertions(+), 5 deletions(-) diff --git a/src/object_feedback/CMakeLists.txt b/src/object_feedback/CMakeLists.txt index f57b52c..8f03a38 100644 --- a/src/object_feedback/CMakeLists.txt +++ b/src/object_feedback/CMakeLists.txt @@ -59,9 +59,9 @@ install(TARGETS ) # Install headers -install(DIRECTORY include/ - DESTINATION include -) +# install(DIRECTORY include/ +# DESTINATION include +# ) # Install launch and config install(DIRECTORY launch diff --git a/src/object_feedback/launch/obj_fb_bringup.launch.py b/src/object_feedback/launch/obj_fb_bringup.launch.py index 0d86131..632d8b6 100644 --- a/src/object_feedback/launch/obj_fb_bringup.launch.py +++ b/src/object_feedback/launch/obj_fb_bringup.launch.py @@ -61,7 +61,7 @@ ), DeclareLaunchArgument( "distance_threshold_mm", - default_value="150", + default_value="190", description="Distance threshold in mm for feedback_all_node to determine success", ), ] @@ -118,6 +118,7 @@ def generate_launch_description(): {"right_dist_topic": LaunchConfiguration("right_dist_topic")}, {"back_dist_topic": LaunchConfiguration("back_dist_topic")}, {"left_dist_topic": LaunchConfiguration("left_dist_topic")}, + {"distance_threshold_mm": LaunchConfiguration("distance_threshold_mm")}, {"success_topic": LaunchConfiguration("success_topic")}, ], ) diff --git a/src/object_feedback/src/feedback_all.cpp b/src/object_feedback/src/feedback_all.cpp index d89ef6b..789bc6f 100644 --- a/src/object_feedback/src/feedback_all.cpp +++ b/src/object_feedback/src/feedback_all.cpp @@ -12,7 +12,7 @@ class FeedbackAll : public rclcpp::Node { this->declare_parameter("front_topic", "/front/front/obj_distance"); this->declare_parameter("back_topic", "/back/back/obj_distance"); this->declare_parameter("success_topic", "/robot/vision/onTakeSuccess"); - this->declare_parameter("distance_threshold_mm", 500); + this->declare_parameter("distance_threshold_mm", 100); left_topic_ = this->get_parameter("left_topic").as_string(); right_topic_ = this->get_parameter("right_topic").as_string(); diff --git a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py index 23242c3..5c484dd 100644 --- a/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py +++ b/src/realsense_ros/realsense2_camera/launch/rs_multi_camera_launch.py @@ -59,7 +59,11 @@ def duplicate_params(general_params, posix): param['original_name'] = param['name'] param['name'] += posix if param['original_name'] == 'enable_depth': + param['default'] = 'true' + elif param['original_name'] == 'initial_reset': param['default'] = 'false' + elif param['original_name'] == 'enable_color': + param['default'] = 'true' elif param['original_name'] == 'enable_infra': param['default'] = 'false' elif param['original_name'] == 'enable_infra1':