From 5c0c6bd0f119d1b3fae79f1488af7875cbf4f3db Mon Sep 17 00:00:00 2001 From: Damian Date: Fri, 28 Mar 2025 22:25:15 +0000 Subject: [PATCH 1/4] Add damian_node.cpp with initial code --- src/damian_package/src/damian_node.cpp | 55 ++++++++++++++++++++++++++ 1 file changed, 55 insertions(+) create mode 100644 src/damian_package/src/damian_node.cpp diff --git a/src/damian_package/src/damian_node.cpp b/src/damian_package/src/damian_node.cpp new file mode 100644 index 0000000..bdd384b --- /dev/null +++ b/src/damian_package/src/damian_node.cpp @@ -0,0 +1,55 @@ +#include +#include +#include +#include +#include + +class DamianNode : public rclcpp::Node { +public: + DamianNode() : Node("damian_node") { + RCLCPP_INFO(this->get_logger(), "DamianNode started!"); + processImage(); // Directly process `image.jpg` + } + +private: + void processImage() { + // Load the image + cv::Mat image = cv::imread("image.jpg"); + + if (image.empty()) { + RCLCPP_ERROR(this->get_logger(), "Failed to load image.jpg!"); + return; + } + + RCLCPP_INFO(this->get_logger(), "Processing image.jpg..."); + + // Save the original image + cv::imwrite("original_image.jpg", image); + + // Apply grayscale conversion + cv::Mat processed_image; + cv::cvtColor(image, processed_image, cv::COLOR_BGR2GRAY); + + // Save processed image + cv::imwrite("processed_image.jpg", processed_image); + + // Display images + RCLCPP_INFO(this->get_logger(), "Saving images (GUI disabled)..."); + + cv::imwrite("original_image.jpg", image); + cv::imwrite("processed_image.jpg", processed_image); + + RCLCPP_INFO(this->get_logger(), "Images saved successfully!"); + + + RCLCPP_INFO(this->get_logger(), "Image processing completed!"); + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::shutdown(); + return 0; +} From f7e1e24b0ba101d654633425acadd38d394af90b Mon Sep 17 00:00:00 2001 From: Damian Date: Fri, 11 Apr 2025 17:28:10 -0500 Subject: [PATCH 2/4] Add initial implementation of TargetSelectorNode --- .../src/target_selector_node.cpp | 115 ++++++++++++++++++ 1 file changed, 115 insertions(+) create mode 100644 src/damian_package/src/target_selector_node.cpp diff --git a/src/damian_package/src/target_selector_node.cpp b/src/damian_package/src/target_selector_node.cpp new file mode 100644 index 0000000..594149e --- /dev/null +++ b/src/damian_package/src/target_selector_node.cpp @@ -0,0 +1,115 @@ +#include "rclcpp/rclcpp.hpp" +#include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection2_d.hpp" +#include "std_msgs/msg/header.hpp" +#include "visualization_msgs/msg/marker.hpp" +#include +#include + +class TargetSelectorNode : public rclcpp::Node +{ +public: + TargetSelectorNode() : Node("target_selector_node") + { + detection_sub_ = this->create_subscription( + "/detections", 10, + std::bind(&TargetSelectorNode::detection_callback, this, std::placeholders::_1)); + + target_pub_ = this->create_publisher("/target", 10); + + marker_pub_ = this->create_publisher("/target_marker", 10); + + // Set your actual camera resolution here (update if different) + image_width_ = 1280.0; + image_height_ = 720.0; + center_x_ = image_width_ / 2.0; + center_y_ = image_height_ / 2.0; + + RCLCPP_INFO(this->get_logger(), "Target Selector Node initialized."); + } + +private: + rclcpp::Subscription::SharedPtr detection_sub_; + rclcpp::Publisher::SharedPtr target_pub_; + rclcpp::Publisher::SharedPtr marker_pub_; + + double image_width_; + double image_height_; + double center_x_; + double center_y_; + + void detection_callback(const vision_msgs::msg::Detection2DArray::SharedPtr msg) + { + if (msg->detections.empty()) + { + RCLCPP_DEBUG(this->get_logger(), "No detections received."); + return; + } + + double min_distance = std::numeric_limits::max(); + vision_msgs::msg::Detection2D best_detection; + + for (const auto &detection : msg->detections) + { + double dx = detection.bbox.center.position.x - center_x_; + double dy = detection.bbox.center.position.y - center_y_; + double distance = dx * dx + dy * dy; + + if (distance < min_distance) + { + min_distance = distance; + best_detection = detection; + } + } + + // Set header stamp and frame ID + best_detection.header.stamp = this->get_clock()->now(); + best_detection.header.frame_id = msg->header.frame_id; + + target_pub_->publish(best_detection); + + RCLCPP_INFO(this->get_logger(), "Published best detection at (%.2f, %.2f)", + best_detection.bbox.center.position.x, + best_detection.bbox.center.position.y); + + publish_marker(best_detection); + } + + void publish_marker(const vision_msgs::msg::Detection2D &detection) + { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = detection.header.frame_id; + marker.header.stamp = this->get_clock()->now(); + marker.ns = "target_selector"; + marker.id = 0; + marker.type = visualization_msgs::msg::Marker::SPHERE; + marker.action = visualization_msgs::msg::Marker::ADD; + + marker.pose.position.x = detection.bbox.center.position.x; + marker.pose.position.y = detection.bbox.center.position.y; + marker.pose.position.z = 0.0; + marker.pose.orientation.w = 1.0; + + marker.scale.x = 20.0; + marker.scale.y = 20.0; + marker.scale.z = 1.0; + + marker.color.r = 1.0f; + marker.color.g = 0.0f; + marker.color.b = 0.0f; + marker.color.a = 1.0f; + + marker.lifetime = rclcpp::Duration::from_seconds(0.5); + + marker_pub_->publish(marker); + } +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 3fdeb848e9fb72530a243209852ffb997e6b1a46 Mon Sep 17 00:00:00 2001 From: Damian Date: Fri, 25 Apr 2025 21:10:40 +0000 Subject: [PATCH 3/4] Refactor target selector node: removed RViz markers, added ROS params for camera resolution --- src/target_selector_pkg/CMakeLists.txt | 54 ++++++++++++ src/target_selector_pkg/package.xml | 23 ++++++ .../target_selector_node.cpp | 82 +++++++++++++++++++ 3 files changed, 159 insertions(+) create mode 100644 src/target_selector_pkg/CMakeLists.txt create mode 100644 src/target_selector_pkg/package.xml create mode 100644 src/target_selector_pkg/target_selector_node.cpp diff --git a/src/target_selector_pkg/CMakeLists.txt b/src/target_selector_pkg/CMakeLists.txt new file mode 100644 index 0000000..b76bd7e --- /dev/null +++ b/src/target_selector_pkg/CMakeLists.txt @@ -0,0 +1,54 @@ +cmake_minimum_required(VERSION 3.8) +project(target_selector_pkg) + + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Find dependencies +find_package(visualization_msgs REQUIRED) +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(cv_bridge REQUIRED) +find_package(image_transport REQUIRED) +find_package(OpenCV REQUIRED) # Ensure OpenCV is found +find_package(vision_msgs REQUIRED) + +# Declare Executables +add_executable(target_selector_node target_selector_node.cpp) + + + +# Link Dependencies for target_selector_node +ament_target_dependencies(target_selector_node + rclcpp + std_msgs + vision_msgs + visualization_msgs +) + +# OpenCV linking +include_directories(${OpenCV_INCLUDE_DIRS}) + +# Compiler Features +target_compile_features(target_selector_node PUBLIC c_std_99 cxx_std_17) + +# Install Executables + + +install(TARGETS target_selector_node + DESTINATION lib/${PROJECT_NAME} +) + + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/target_selector_pkg/package.xml b/src/target_selector_pkg/package.xml new file mode 100644 index 0000000..994fc84 --- /dev/null +++ b/src/target_selector_pkg/package.xml @@ -0,0 +1,23 @@ + + + + target_selector_pkg + 0.0.0 + TODO: Package description + admin + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + visualization_msgs + vision_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/target_selector_pkg/target_selector_node.cpp b/src/target_selector_pkg/target_selector_node.cpp new file mode 100644 index 0000000..6071eff --- /dev/null +++ b/src/target_selector_pkg/target_selector_node.cpp @@ -0,0 +1,82 @@ +#include "rclcpp/rclcpp.hpp" +#include "vision_msgs/msg/detection2_d_array.hpp" +#include "vision_msgs/msg/detection2_d.hpp" +#include "std_msgs/msg/header.hpp" +#include +#include + +class TargetSelectorNode : public rclcpp::Node +{ +public: + TargetSelectorNode() : Node("target_selector_node") + { + // Declare and get camera resolution parameters + this->declare_parameter("image_width", 1280.0); + this->declare_parameter("image_height", 720.0); + this->get_parameter("image_width", image_width_); + this->get_parameter("image_height", image_height_); + center_x_ = image_width_ / 2.0; + center_y_ = image_height_ / 2.0; + + // Set up publishers and subscribers + detection_sub_ = this->create_subscription( + "/detections", 10, + std::bind(&TargetSelectorNode::detection_callback, this, std::placeholders::_1)); + + target_pub_ = this->create_publisher("/target", 10); + + RCLCPP_INFO(this->get_logger(), "Target Selector Node initialized with resolution %.0fx%.0f", image_width_, image_height_); + } + +private: + rclcpp::Subscription::SharedPtr detection_sub_; + rclcpp::Publisher::SharedPtr target_pub_; + + double image_width_; + double image_height_; + double center_x_; + double center_y_; + + void detection_callback(const vision_msgs::msg::Detection2DArray::SharedPtr msg) + { + if (msg->detections.empty()) + { + RCLCPP_DEBUG(this->get_logger(), "No detections received."); + return; + } + + double min_distance = std::numeric_limits::max(); + vision_msgs::msg::Detection2D best_detection; + + for (const auto &detection : msg->detections) + { + double dx = detection.bbox.center.position.x - center_x_; + double dy = detection.bbox.center.position.y - center_y_; + double distance = dx * dx + dy * dy; + + if (distance < min_distance) + { + min_distance = distance; + best_detection = detection; + } + } + + best_detection.header.stamp = this->get_clock()->now(); + best_detection.header.frame_id = msg->header.frame_id; + + target_pub_->publish(best_detection); + + RCLCPP_INFO(this->get_logger(), "Published best detection at (%.2f, %.2f)", + best_detection.bbox.center.position.x, + best_detection.bbox.center.position.y); + } +}; + +int main(int argc, char *argv[]) +{ + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From d47f9d7f5246b9e3c6f1449b1b686cd231faced5 Mon Sep 17 00:00:00 2001 From: Damian Date: Fri, 25 Apr 2025 23:17:48 +0000 Subject: [PATCH 4/4] Remove damian_package and old node files --- src/damian_package/src/damian_node.cpp | 55 --------- .../src/target_selector_node.cpp | 115 ------------------ 2 files changed, 170 deletions(-) delete mode 100644 src/damian_package/src/damian_node.cpp delete mode 100644 src/damian_package/src/target_selector_node.cpp diff --git a/src/damian_package/src/damian_node.cpp b/src/damian_package/src/damian_node.cpp deleted file mode 100644 index bdd384b..0000000 --- a/src/damian_package/src/damian_node.cpp +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include -#include -#include -#include - -class DamianNode : public rclcpp::Node { -public: - DamianNode() : Node("damian_node") { - RCLCPP_INFO(this->get_logger(), "DamianNode started!"); - processImage(); // Directly process `image.jpg` - } - -private: - void processImage() { - // Load the image - cv::Mat image = cv::imread("image.jpg"); - - if (image.empty()) { - RCLCPP_ERROR(this->get_logger(), "Failed to load image.jpg!"); - return; - } - - RCLCPP_INFO(this->get_logger(), "Processing image.jpg..."); - - // Save the original image - cv::imwrite("original_image.jpg", image); - - // Apply grayscale conversion - cv::Mat processed_image; - cv::cvtColor(image, processed_image, cv::COLOR_BGR2GRAY); - - // Save processed image - cv::imwrite("processed_image.jpg", processed_image); - - // Display images - RCLCPP_INFO(this->get_logger(), "Saving images (GUI disabled)..."); - - cv::imwrite("original_image.jpg", image); - cv::imwrite("processed_image.jpg", processed_image); - - RCLCPP_INFO(this->get_logger(), "Images saved successfully!"); - - - RCLCPP_INFO(this->get_logger(), "Image processing completed!"); - } -}; - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::shutdown(); - return 0; -} diff --git a/src/damian_package/src/target_selector_node.cpp b/src/damian_package/src/target_selector_node.cpp deleted file mode 100644 index 594149e..0000000 --- a/src/damian_package/src/target_selector_node.cpp +++ /dev/null @@ -1,115 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "vision_msgs/msg/detection2_d_array.hpp" -#include "vision_msgs/msg/detection2_d.hpp" -#include "std_msgs/msg/header.hpp" -#include "visualization_msgs/msg/marker.hpp" -#include -#include - -class TargetSelectorNode : public rclcpp::Node -{ -public: - TargetSelectorNode() : Node("target_selector_node") - { - detection_sub_ = this->create_subscription( - "/detections", 10, - std::bind(&TargetSelectorNode::detection_callback, this, std::placeholders::_1)); - - target_pub_ = this->create_publisher("/target", 10); - - marker_pub_ = this->create_publisher("/target_marker", 10); - - // Set your actual camera resolution here (update if different) - image_width_ = 1280.0; - image_height_ = 720.0; - center_x_ = image_width_ / 2.0; - center_y_ = image_height_ / 2.0; - - RCLCPP_INFO(this->get_logger(), "Target Selector Node initialized."); - } - -private: - rclcpp::Subscription::SharedPtr detection_sub_; - rclcpp::Publisher::SharedPtr target_pub_; - rclcpp::Publisher::SharedPtr marker_pub_; - - double image_width_; - double image_height_; - double center_x_; - double center_y_; - - void detection_callback(const vision_msgs::msg::Detection2DArray::SharedPtr msg) - { - if (msg->detections.empty()) - { - RCLCPP_DEBUG(this->get_logger(), "No detections received."); - return; - } - - double min_distance = std::numeric_limits::max(); - vision_msgs::msg::Detection2D best_detection; - - for (const auto &detection : msg->detections) - { - double dx = detection.bbox.center.position.x - center_x_; - double dy = detection.bbox.center.position.y - center_y_; - double distance = dx * dx + dy * dy; - - if (distance < min_distance) - { - min_distance = distance; - best_detection = detection; - } - } - - // Set header stamp and frame ID - best_detection.header.stamp = this->get_clock()->now(); - best_detection.header.frame_id = msg->header.frame_id; - - target_pub_->publish(best_detection); - - RCLCPP_INFO(this->get_logger(), "Published best detection at (%.2f, %.2f)", - best_detection.bbox.center.position.x, - best_detection.bbox.center.position.y); - - publish_marker(best_detection); - } - - void publish_marker(const vision_msgs::msg::Detection2D &detection) - { - visualization_msgs::msg::Marker marker; - marker.header.frame_id = detection.header.frame_id; - marker.header.stamp = this->get_clock()->now(); - marker.ns = "target_selector"; - marker.id = 0; - marker.type = visualization_msgs::msg::Marker::SPHERE; - marker.action = visualization_msgs::msg::Marker::ADD; - - marker.pose.position.x = detection.bbox.center.position.x; - marker.pose.position.y = detection.bbox.center.position.y; - marker.pose.position.z = 0.0; - marker.pose.orientation.w = 1.0; - - marker.scale.x = 20.0; - marker.scale.y = 20.0; - marker.scale.z = 1.0; - - marker.color.r = 1.0f; - marker.color.g = 0.0f; - marker.color.b = 0.0f; - marker.color.a = 1.0f; - - marker.lifetime = rclcpp::Duration::from_seconds(0.5); - - marker_pub_->publish(marker); - } -}; - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -}