Simple Distance Target Selection

src/damian_package/src/damian_node.cpp

@@ -0,0 +1,55 @@
+#include <rclcpp/rclcpp.hpp>
+#include <cv_bridge/cv_bridge.h>
+#include <opencv2/opencv.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+#include <opencv2/highgui/highgui.hpp>
+
+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<DamianNode>();
+  rclcpp::shutdown();
+  return 0;
+}

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 <limits>
+#include <cmath>
+
+class TargetSelectorNode : public rclcpp::Node
+{
+public:
+    TargetSelectorNode() : Node("target_selector_node")
+    {
+        detection_sub_ = this->create_subscription<vision_msgs::msg::Detection2DArray>(
+            "/detections", 10,
+            std::bind(&TargetSelectorNode::detection_callback, this, std::placeholders::_1));
+
+        target_pub_ = this->create_publisher<vision_msgs::msg::Detection2D>("/target", 10);
+
+        marker_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/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<vision_msgs::msg::Detection2DArray>::SharedPtr detection_sub_;
+    rclcpp::Publisher<vision_msgs::msg::Detection2D>::SharedPtr target_pub_;
+    rclcpp::Publisher<visualization_msgs::msg::Marker>::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<double>::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<TargetSelectorNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

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()

src/target_selector_pkg/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>target_selector_pkg</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="[email protected]">admin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>visualization_msgs</depend>
+  <depend>vision_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

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 <limits>
+#include <cmath>
+
+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<vision_msgs::msg::Detection2DArray>(
+            "/detections", 10,
+            std::bind(&TargetSelectorNode::detection_callback, this, std::placeholders::_1));
+
+        target_pub_ = this->create_publisher<vision_msgs::msg::Detection2D>("/target", 10);
+
+        RCLCPP_INFO(this->get_logger(), "Target Selector Node initialized with resolution %.0fx%.0f", image_width_, image_height_);
+    }
+
+private:
+    rclcpp::Subscription<vision_msgs::msg::Detection2DArray>::SharedPtr detection_sub_;
+    rclcpp::Publisher<vision_msgs::msg::Detection2D>::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<double>::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<TargetSelectorNode>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}