From 2c9d4a76be10285ddae600e7f0d11d292e571823 Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Sun, 10 May 2026 04:34:11 +0000 Subject: [PATCH 1/2] Add aruco pose estimate to gstreamer pipeline --- src/Cameras/video_streaming/CMakeLists.txt | 11 ++ .../video_streaming/aruco_pose_estimate.hpp | 44 +++++++ .../include/video_streaming/detect_node.hpp | 15 ++- .../include/video_streaming/input_node.hpp | 4 +- src/Cameras/video_streaming/package.xml | 5 + .../src/aruco_pose_estimate.cpp | 113 ++++++++++++++++++ .../video_streaming/src/detect_node.cpp | 101 ++++++++++++++-- .../video_streaming/src/input_node.cpp | 13 +- 8 files changed, 288 insertions(+), 18 deletions(-) create mode 100644 src/Cameras/video_streaming/include/video_streaming/aruco_pose_estimate.hpp create mode 100644 src/Cameras/video_streaming/src/aruco_pose_estimate.cpp diff --git a/src/Cameras/video_streaming/CMakeLists.txt b/src/Cameras/video_streaming/CMakeLists.txt index ffc67e18..be1878a7 100644 --- a/src/Cameras/video_streaming/CMakeLists.txt +++ b/src/Cameras/video_streaming/CMakeLists.txt @@ -12,6 +12,11 @@ find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) find_package(interfaces REQUIRED) find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(OpenCV REQUIRED) find_package(PkgConfig REQUIRED) pkg_check_modules(GSTREAMER REQUIRED IMPORTED_TARGET gstreamer-1.0) @@ -27,6 +32,7 @@ add_library(${PROJECT_NAME} SHARED src/base_video_node.cpp src/input_node.cpp src/detect_node.cpp + src/aruco_pose_estimate.cpp src/webrtc_node.cpp src/srt_node.cpp src/srt_node_client.cpp @@ -55,12 +61,17 @@ target_link_libraries(${PROJECT_NAME} PkgConfig::GSTREAMER PkgConfig::GST_APP PkgConfig::GST_VIDEO + ${OpenCV_LIBRARIES} ) ament_target_dependencies(${PROJECT_NAME} rclcpp rclcpp_components interfaces std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport ) # If pkg-config provided extra compile flags (e.g., -D, -pthread) diff --git a/src/Cameras/video_streaming/include/video_streaming/aruco_pose_estimate.hpp b/src/Cameras/video_streaming/include/video_streaming/aruco_pose_estimate.hpp new file mode 100644 index 00000000..4a549935 --- /dev/null +++ b/src/Cameras/video_streaming/include/video_streaming/aruco_pose_estimate.hpp @@ -0,0 +1,44 @@ +#pragma once + +#include +#include +#include +#include +#include + +// --- OpenCV 4.8.0 Specific Headers --- +#include +#include +#include +#include +#include +// ------------------------------------- + +#include + +struct CameraIntrinsics { + cv::Mat camera_matrix; + cv::Mat dist_coeffs; +}; + +class ArucoPoseEstimator { +public: + ArucoPoseEstimator(rclcpp::Node *node); + void processImage(const cv::Mat &frame, const std::string &camera_name, + const rclcpp::Time &stamp); + +private: + rclcpp::Node *parent_node_; + rclcpp::Publisher::SharedPtr drive_pub_; + rclcpp::Publisher::SharedPtr ee_pub_; + + cv::aruco::DetectorParameters detector_params_; + cv::aruco::Dictionary dictionary_; + + double marker_length_; + std::vector marker_obj_points_; + + // Camera routing and calibration mapping + std::unordered_map camera_to_frame_id_; + std::unordered_map calibrations_; +}; \ No newline at end of file diff --git a/src/Cameras/video_streaming/include/video_streaming/detect_node.hpp b/src/Cameras/video_streaming/include/video_streaming/detect_node.hpp index 7d52b20f..d54e5bd9 100644 --- a/src/Cameras/video_streaming/include/video_streaming/detect_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/detect_node.hpp @@ -1,7 +1,12 @@ #pragma once +#include "aruco_pose_estimate.hpp" #include "base_video_node.hpp" +#include #include +#include +#include #include +#include class DetectNode : public BaseVideoNode { public: @@ -18,12 +23,20 @@ class DetectNode : public BaseVideoNode { private: DetectionType detection_type_; + std::shared_ptr aruco_estimator_; rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; rclcpp::Publisher::SharedPtr marker_pub_; rclcpp::Publisher::SharedPtr object_detected_pub_; + rclcpp::Subscription::SharedPtr active_camera_sub_; + + std::string active_camera_{"Drive"}; + + void active_camera_callback(const std_msgs::msg::String::SharedPtr msg); rcl_interfaces::msg::SetParametersResult on_parameter_change(const std::vector ¶meters); DetectionType string_to_detection_type(const std::string &type_str); std::string detection_type_to_string() const; -}; + + static GstFlowReturn on_new_sample(GstAppSink *sink, gpointer user_data); +}; \ No newline at end of file diff --git a/src/Cameras/video_streaming/include/video_streaming/input_node.hpp b/src/Cameras/video_streaming/include/video_streaming/input_node.hpp index 1b21dcc8..6f9e3e73 100644 --- a/src/Cameras/video_streaming/include/video_streaming/input_node.hpp +++ b/src/Cameras/video_streaming/include/video_streaming/input_node.hpp @@ -2,6 +2,7 @@ #include "base_video_node.hpp" #include #include +#include // Added to fix the compiler error class InputNode : public BaseVideoNode { public: @@ -30,7 +31,8 @@ class InputNode : public BaseVideoNode { private: std::map source_map_; + rclcpp::Publisher::SharedPtr active_camera_pub_; rclcpp::Service::SharedPtr video_service_; rclcpp::Service::SharedPtr get_cam_service_; std::shared_ptr current_video_request_; -}; +}; \ No newline at end of file diff --git a/src/Cameras/video_streaming/package.xml b/src/Cameras/video_streaming/package.xml index 84955559..302b672a 100644 --- a/src/Cameras/video_streaming/package.xml +++ b/src/Cameras/video_streaming/package.xml @@ -13,6 +13,11 @@ rclcpp rclcpp_components std_msgs + sensor_msgs + geometry_msgs + cv_bridge + image_transport + opencv gstreamer-1.0 glib-2.0 libssl-dev diff --git a/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp b/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp new file mode 100644 index 00000000..4a261999 --- /dev/null +++ b/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp @@ -0,0 +1,113 @@ +#include "aruco_pose_estimate.hpp" +#include + +ArucoPoseEstimator::ArucoPoseEstimator(rclcpp::Node *node) + : parent_node_(node) { + drive_pub_ = parent_node_->create_publisher( + "/computer_vision/drive_nav_markers", 10); + ee_pub_ = parent_node_->create_publisher( + "/computer_vision/ee_nav_markers", 10); + + dictionary_ = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50); + detector_params_ = cv::aruco::DetectorParameters(); + + marker_length_ = + parent_node_->declare_parameter("marker_size", 0.15); // 15cm default + + float half_l = marker_length_ / 2.0f; + marker_obj_points_ = { + cv::Point3f(-half_l, half_l, 0), cv::Point3f(half_l, half_l, 0), + cv::Point3f(half_l, -half_l, 0), cv::Point3f(-half_l, -half_l, 0)}; + + // ------------------------------------------------------------- + // Define frame IDs map based on active camera string + // ------------------------------------------------------------- + camera_to_frame_id_["Drive"] = "DriveCamera"; + camera_to_frame_id_["EndEffector"] = "EndEffector"; + + // ------------------------------------------------------------- + // Define Intrinsic Calibrations + // ------------------------------------------------------------- + + // Drive Camera (High Distortion) + CameraIntrinsics drive_calib; + drive_calib.camera_matrix = (cv::Mat_(3, 3) << 234.2702, 0, 346.4237, + 0, 255.8694, 287.5284, 0, 0, 1); + drive_calib.dist_coeffs = (cv::Mat_(1, 8) << 0.6030, 0.4734, -0.0011, + -0.0004, -0.0708, 0.3530, 0.6553, -0.0759); + calibrations_["Drive"] = drive_calib; + + // End Effector Camera (High Distortion) + // NOTE: Replace these values with the actual EE calibration when available + CameraIntrinsics ee_calib; + ee_calib.camera_matrix = (cv::Mat_(3, 3) << 234.2702, 0, 346.4237, 0, + 255.8694, 287.5284, 0, 0, 1); + ee_calib.dist_coeffs = (cv::Mat_(1, 8) << 0.6030, 0.4734, -0.0011, + -0.0004, -0.0708, 0.3530, 0.6553, -0.0759); + calibrations_["EndEffector"] = ee_calib; +} + +void ArucoPoseEstimator::processImage(const cv::Mat &frame, + const std::string &camera_name, + const rclcpp::Time &stamp) { + try { + // Resolve Frame ID + std::string frame_id = camera_name; // Fallback + if (camera_to_frame_id_.find(camera_name) != camera_to_frame_id_.end()) { + frame_id = camera_to_frame_id_[camera_name]; + } + + // Fetch correct calibration + if (calibrations_.find(camera_name) == calibrations_.end()) { + RCLCPP_WARN_THROTTLE( + parent_node_->get_logger(), *parent_node_->get_clock(), 2000, + "No calibration found for camera: %s. Skipping Aruco.", + camera_name.c_str()); + return; + } + + cv::Mat cam_mat = calibrations_[camera_name].camera_matrix; + cv::Mat dist = calibrations_[camera_name].dist_coeffs; + + cv::Mat undistorted; + cv::undistort(frame, undistorted, cam_mat, dist); + + cv::aruco::ArucoDetector detector(dictionary_, detector_params_); + std::vector markerIds; + std::vector> markerCorners, rejectedCandidates; + detector.detectMarkers(undistorted, markerCorners, markerIds, + rejectedCandidates); + + interfaces::msg::ArucoMarkers msg; + msg.header.stamp = stamp; + msg.header.frame_id = frame_id; + + if (!markerIds.empty()) { + for (size_t i = 0; i < markerIds.size(); i++) { + msg.marker_ids.push_back(markerIds[i]); + + cv::Vec3d rvec, tvec; + bool success = + cv::solvePnP(marker_obj_points_, markerCorners[i], cam_mat, dist, + rvec, tvec, false, cv::SOLVEPNP_IPPE_SQUARE); + + geometry_msgs::msg::Point pt; + if (success) { + pt.x = tvec[0]; + pt.y = tvec[1]; + pt.z = tvec[2]; + } + msg.points.push_back(pt); + msg.is_moving.push_back(false); + } + } + + if (camera_name == "EndEffector") { + ee_pub_->publish(msg); + } else { + drive_pub_->publish(msg); + } + } catch (const std::exception &e) { + RCLCPP_WARN(parent_node_->get_logger(), "Aruco logic failed: %s", e.what()); + } +} \ No newline at end of file diff --git a/src/Cameras/video_streaming/src/detect_node.cpp b/src/Cameras/video_streaming/src/detect_node.cpp index a3d851eb..a53f2189 100644 --- a/src/Cameras/video_streaming/src/detect_node.cpp +++ b/src/Cameras/video_streaming/src/detect_node.cpp @@ -13,6 +13,9 @@ metadata_probe_callback(GstPad *pad, GstPadProbeInfo *info, gpointer user_data); DetectNode::DetectNode(const rclcpp::NodeOptions &options) : BaseVideoNode("detect_node", options), detection_type_(DetectionType::NONE) { + + aruco_estimator_ = std::make_shared(this); + this->declare_parameter("bottle_config", "config/bottle/bottle.txt"); this->declare_parameter("detection_type", "NONE"); @@ -21,13 +24,22 @@ DetectNode::DetectNode(const rclcpp::NodeOptions &options) this->declare_parameter("rockpick_config", "config/rockpick/rockpick.txt"); this->declare_parameter("listen_to", "input"); + + active_camera_sub_ = this->create_subscription( + "/active_camera", 10, + std::bind(&DetectNode::active_camera_callback, this, + std::placeholders::_1)); + param_callback_handle_ = this->add_on_set_parameters_callback( std::bind(&DetectNode::on_parameter_change, this, std::placeholders::_1)); + marker_pub_ = this->create_publisher( "marker_detected", rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); + object_detected_pub_ = this->create_publisher( "object_detected", rclcpp::QoS(rclcpp::KeepLast(10)).reliable()); + start_pipeline(); } @@ -47,8 +59,11 @@ bool DetectNode::create_pipeline() { << this->get_parameter("listen_to").as_string() << " is-live=true " "allow-renegotiation=true name=src ! "; + + bool add_terminator = true; detection_type_ = string_to_detection_type( this->get_parameter("detection_type").as_string()); + switch (detection_type_) { case DetectionType::WATER_BOTTLE: desc_ss << get_detection_pipeline_str( @@ -63,15 +78,24 @@ bool DetectNode::create_pipeline() { this->get_parameter("rockpick_config").as_string()); break; case DetectionType::ARUCO: - desc_ss << "videoconvert ! queue ! videoconvert ! arucomarker " - "detect-every=10 " - "name=aruco_detector ! queue ! videoconvert ! "; + // Added by Gemini: ! videoconvert ! video/x-raw,format=RGB + desc_ss << "videoconvert ! queue ! videoconvert ! " + "arucomarker detect-every=10 name=aruco_detector ! " + "tee name=aruco_tee " + "aruco_tee. ! queue ! videoconvert ! nvvidconv ! interpipesink " + "name=detect " + "aruco_tee. ! queue ! videoconvert ! video/x-raw,format=RGB ! " + "appsink name=aruco_cam_sink sync=false max-buffers=1 drop=true"; + add_terminator = false; break; case DetectionType::NONE: desc_ss << "identity ! "; break; } - desc_ss << "nvvidconv ! interpipesink name=detect"; + + if (add_terminator) { + desc_ss << "nvvidconv ! interpipesink name=detect"; + } RCLCPP_INFO(this->get_logger(), "Creating pipeline: %s", desc_ss.str().c_str()); @@ -100,19 +124,31 @@ bool DetectNode::create_pipeline() { pipeline_ = p; return true; } + bool DetectNode::start_pipeline() { if (!BaseVideoNode::start_pipeline()) { return false; } + if (detection_type_ == DetectionType::ARUCO) { GstElement *aruco = get_element("aruco_detector"); - if (!aruco) { - RCLCPP_ERROR(this->get_logger(), "Failed to get arucomarker element."); - return false; + if (aruco) { + g_signal_connect(aruco, "marker-detected", G_CALLBACK(on_marker_detected), + marker_pub_.get()); + gst_object_unref(aruco); + } + + GstElement *appsink = get_element("aruco_cam_sink"); + if (appsink) { + GstAppSinkCallbacks callbacks = {}; + callbacks.new_sample = on_new_sample; + gst_app_sink_set_callbacks(GST_APP_SINK(appsink), &callbacks, this, + nullptr); + gst_object_unref(appsink); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get aruco_cam_sink element."); } - g_signal_connect(aruco, "marker-detected", G_CALLBACK(on_marker_detected), - marker_pub_.get()); - gst_object_unref(aruco); + } else if (detection_type_ == DetectionType::MALLET || detection_type_ == DetectionType::WATER_BOTTLE) { GstElement *osd = get_element("osd"); @@ -128,9 +164,49 @@ bool DetectNode::start_pipeline() { return true; } +void DetectNode::active_camera_callback( + const std_msgs::msg::String::SharedPtr msg) { + if (msg->data != active_camera_) { + active_camera_ = msg->data; + RCLCPP_INFO(this->get_logger(), "Active camera changed to %s", + active_camera_.c_str()); + } +} + +GstFlowReturn DetectNode::on_new_sample(GstAppSink *sink, gpointer user_data) { + auto *self = static_cast(user_data); + GstSample *sample = gst_app_sink_pull_sample(sink); + if (!sample) + return GST_FLOW_ERROR; + + GstCaps *caps = gst_sample_get_caps(sample); + GstBuffer *buffer = gst_sample_get_buffer(sample); + + gint width = 0, height = 0; + if (caps) { + GstStructure *s = gst_caps_get_structure(caps, 0); + gst_structure_get_int(s, "width", &width); + gst_structure_get_int(s, "height", &height); + } + + GstMapInfo map; + if (gst_buffer_map(buffer, &map, GST_MAP_READ)) { + + // 1. Process PnP Math + auto stamp = self->now(); + cv::Mat frame(height, width, CV_8UC3, map.data); + if (self->aruco_estimator_) { + self->aruco_estimator_->processImage(frame, self->active_camera_, stamp); + } + + gst_buffer_unmap(buffer, &map); + } + gst_sample_unref(sample); + return GST_FLOW_OK; +} + GstPadProbeReturn metadata_probe_callback(GstPad *pad, GstPadProbeInfo *info, gpointer user_data) { - // TODO: (ERIK) Create whatever API you want for autonomy DetectNode *self = static_cast(user_data); GstBuffer *buffer = GST_PAD_PROBE_INFO_BUFFER(info); if (!buffer) { @@ -242,6 +318,7 @@ rcl_interfaces::msg::SetParametersResult DetectNode::on_parameter_change( result.successful = true; result.reason = "success"; bool needs_restart = false; + for (const auto ¶m : parameters) { if (param.get_name() == "bottle_config" || param.get_name() == "mallet_config" || @@ -274,4 +351,4 @@ rcl_interfaces::msg::SetParametersResult DetectNode::on_parameter_change( return result; } -RCLCPP_COMPONENTS_REGISTER_NODE(DetectNode) +RCLCPP_COMPONENTS_REGISTER_NODE(DetectNode) \ No newline at end of file diff --git a/src/Cameras/video_streaming/src/input_node.cpp b/src/Cameras/video_streaming/src/input_node.cpp index c9377b98..8e7f7af4 100644 --- a/src/Cameras/video_streaming/src/input_node.cpp +++ b/src/Cameras/video_streaming/src/input_node.cpp @@ -6,6 +6,8 @@ InputNode::InputNode(const rclcpp::NodeOptions &options) : BaseVideoNode("input_node", options) { RCLCPP_INFO(this->get_logger(), "InputNode constructed."); declare_parameters(); + active_camera_pub_ = + this->create_publisher("/active_camera", 10); video_service_ = this->create_service( "start_video", std::bind(&InputNode::video_cb, this, std::placeholders::_1, std::placeholders::_2)); @@ -40,8 +42,6 @@ void InputNode::declare_parameters() { bool InputNode::create_pipeline() { std::stringstream desc_stream; - // Hack: Use a constant videotestsrc for the sink_0 to ensure the compositor - // always has a source pad of the correct size desc_stream << "videotestsrc is-live=true pattern=black ! video/x-raw,width=" << this->get_parameter("out_width").as_int() << ",height=" << this->get_parameter("out_height").as_int() @@ -90,7 +90,6 @@ bool InputNode::create_pipeline() { if (cam_framerate > 0) { desc_stream << "framerate=" << cam_framerate << "/1,"; } - // Remove trailing comma std::string current_desc = desc_stream.str(); if (current_desc.back() == ',') { current_desc.pop_back(); @@ -212,7 +211,13 @@ void InputNode::video_cb( } if (response->success) { current_video_request_ = request; + + if (!request->sources.empty()) { + std_msgs::msg::String msg; + msg.data = request->sources[0].name; + active_camera_pub_->publish(msg); + } } } -RCLCPP_COMPONENTS_REGISTER_NODE(InputNode) +RCLCPP_COMPONENTS_REGISTER_NODE(InputNode) \ No newline at end of file From d336f2cd5679b39b0bcd7712569663069ffe650e Mon Sep 17 00:00:00 2001 From: Erik Caldwell Date: Sun, 10 May 2026 11:55:36 -0400 Subject: [PATCH 2/2] Drive camera calibration added --- .../video_streaming/src/aruco_pose_estimate.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp b/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp index 4a261999..350022e7 100644 --- a/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp +++ b/src/Cameras/video_streaming/src/aruco_pose_estimate.cpp @@ -29,16 +29,19 @@ ArucoPoseEstimator::ArucoPoseEstimator(rclcpp::Node *node) // Define Intrinsic Calibrations // ------------------------------------------------------------- - // Drive Camera (High Distortion) + // Drive Camera CameraIntrinsics drive_calib; - drive_calib.camera_matrix = (cv::Mat_(3, 3) << 234.2702, 0, 346.4237, - 0, 255.8694, 287.5284, 0, 0, 1); - drive_calib.dist_coeffs = (cv::Mat_(1, 8) << 0.6030, 0.4734, -0.0011, - -0.0004, -0.0708, 0.3530, 0.6553, -0.0759); + drive_calib.camera_matrix = (cv::Mat_(3, 3) << 1208.455865000, 0, + 953.045426800, 0, 1201.369845000, + 590.105458700, 0, 0, 1); + drive_calib.dist_coeffs = + (cv::Mat_(1, 8) + << -1.041143259e-01, -8.638070390e-03, 1.363747596e-03, + -2.526459586e-05, 4.666582799e-02, 1.310149040e-04, + -5.928740585e-04, 4.767364821e-02); calibrations_["Drive"] = drive_calib; // End Effector Camera (High Distortion) - // NOTE: Replace these values with the actual EE calibration when available CameraIntrinsics ee_calib; ee_calib.camera_matrix = (cv::Mat_(3, 3) << 234.2702, 0, 346.4237, 0, 255.8694, 287.5284, 0, 0, 1);