Planner node

packages/camera/include/camera/camera_node.h

@@ -1,20 +1,21 @@
 #pragma once
 
-#include "detection/detection.h"
-#include "camera/params.h"
-#include <wbb_msgs/msg/image_pose.hpp>
-#include <wbb_msgs/msg/image_marker_pos.hpp>
-#include <wbb_msgs/msg/image_marker_pos_array.hpp>
-
 #include <cv_bridge/cv_bridge.h>
+
 #include <foxglove_msgs/msg/image_marker_array.hpp>
+#include <memory>
 #include <opencv2/aruco.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/compressed_image.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <visualization_msgs/msg/image_marker.hpp>
+#include <wbb_msgs/msg/image_marker_pos.hpp>
+#include <wbb_msgs/msg/image_marker_pos_array.hpp>
+#include <wbb_msgs/msg/image_pixel_scale.hpp>
+#include <wbb_msgs/msg/image_pose.hpp>
 
-#include <memory>
+#include "camera/params.h"
+#include "detection/detection.h"
 
 namespace wbb {
 
@@ -37,6 +38,7 @@ class CameraNode : public rclcpp::Node {
     void publishImageCorners(const std::vector<Marker>& markers);
     void publishImageBorder(const std::vector<cv::Point2f>& border);
     void publishBotEgo(const std::optional<BotPose>& ego, const rclcpp::Time& capturing_time);
+    void publishPixelScale(const std::vector<Marker>& markers);
 
     void publishPreview(const cv::Mat& warped_image, const rclcpp::Time& capturing_time);
     void publishPreviewCorners(
@@ -46,6 +48,7 @@ class CameraNode : public rclcpp::Node {
     rclcpp::TimerBase::SharedPtr timer_;
     rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr image_publisher_ = nullptr;
     rclcpp::Publisher<sensor_msgs::msg::CompressedImage>::SharedPtr preview_publisher_ = nullptr;
+    rclcpp::Publisher<wbb_msgs::msg::ImagePixelScale>::SharedPtr pixel_scale_publisher_ = nullptr;
 
     rclcpp::Publisher<wbb_msgs::msg::ImagePose>::SharedPtr robot_ego_publisher_ = nullptr;
     rclcpp::Publisher<wbb_msgs::msg::ImageMarkerPosArray>::SharedPtr image_corners_publisher_ =

packages/camera/src/camera_node.cpp

@@ -1,19 +1,22 @@
 #include "camera/camera_node.h"
-#include "detection/detection.h"
 
-#include <std_msgs/msg/header.hpp>
+#include <stdio.h>
 
 #include <chrono>
 #include <cmath>
 #include <optional>
-#include <stdio.h>
+#include <std_msgs/msg/header.hpp>
+
+#include "detection/detection.h"
 
 namespace wbb {
 
 namespace {
 
-const int kBotMarkerId = 4;  // Values ​​from 0 to 3 are the serial numbers of markers on the corners of the board
+const int kBotMarkerId =
+    4;  // Values ​​from 0 to 3 are the serial numbers of markers on the corners of the board
 const float kBotMarkerSize = 0.1f;
+const float kCornerMarkerSize = 0.057f;
 const float kBaseLength = 0.2f;
 const float kBaseWidth = 0.15f;
 const int kCornersMsgId = 0;
@@ -56,6 +59,9 @@ CameraNode::CameraNode() : Node("CameraNode") {
     robot_border_publisher_ =
         this->create_publisher<wbb_msgs::msg::ImageMarkerPos>("/board/image/ego", 10);
 
+    pixel_scale_publisher_ =
+        this->create_publisher<wbb_msgs::msg::ImagePixelScale>("/board/scale", 10);
+
     preview_corners_publisher_ =
         this->create_publisher<foxglove_msgs::msg::ImageMarkerArray>("/board/preview/corners", 10);
 
@@ -218,6 +224,22 @@ void CameraNode::publishImageCorners(const std::vector<Marker>& markers) {
     image_corners_publisher_->publish(image_corners_msg);
 }
 
+void CameraNode::publishPixelScale(const std::vector<Marker>& markers) {
+    wbb_msgs::msg::ImagePixelScale msg;
+    if (markers.empty()) {
+        msg.scale_x = 1;
+        msg.scale_y = 1;
+    } else {
+        int pixels_x = markers[0].corners[1].x - markers[0].corners[0].x;
+        int pixels_y = markers[0].corners[3].y - markers[0].corners[0].y;
+
+        msg.scale_x = kCornerMarkerSize / pixels_x;
+        msg.scale_y = kCornerMarkerSize / pixels_y;
+    }
+
+    pixel_scale_publisher_->publish(msg);
+}
+
 void CameraNode::publishPreview(const cv::Mat& warped_image, const rclcpp::Time& captured_time) {
     sensor_msgs::msg::CompressedImage compressed_msg;
     cv_bridge::CvImage(msg::makeHeader(captured_time), "bgr8", warped_image)
@@ -231,17 +253,20 @@ void CameraNode::publishPreviewCorners(
     for (auto marker : markers) {
         coords.push_back(marker.corners);
     }
-    const auto preview_corners_msg = msg::makeLineStripArray(kCornersMsgId, coords, captured_time);
+    const auto preview_corners_msg =
+        msg::makeLineStripArray(kCornersMsgId, coords, Color(1, 0, 0, 1), 2, captured_time);
     preview_corners_publisher_->publish(preview_corners_msg);
 }
 
 void CameraNode::publishBotBox(
     const std::vector<cv::Point2f>& bot_box, const rclcpp::Time& captured_time) {
     visualization_msgs::msg::ImageMarker bot_box_msg;
     if (bot_box.size() == 0) {
-        bot_box_msg = msg::makeLineStrip(kBotMsgId, bot_box, captured_time, false, false);
+        bot_box_msg = msg::makeLineStrip(
+            kBotMsgId, bot_box, Color(1, 0, 0, 1), 2, captured_time, false, false);
     } else {
-        bot_box_msg = msg::makeLineStrip(kBotMsgId, bot_box, captured_time);
+        bot_box_msg =
+            msg::makeLineStrip(kBotMsgId, bot_box, Color(1, 0, 0, 1), 2, captured_time, true, true);
     }
 
     preview_border_publisher_->publish(bot_box_msg);
@@ -292,6 +317,7 @@ void CameraNode::handleCameraOnTimer() {
 
     publishImage(warped_image, captured_time);
     publishImageCorners(transformed_detection.corners);
+    publishPixelScale(transformed_detection.corners);
     publishImageBorder(bot_box);
     publishBotEgo(bot_pose, captured_time);
 

packages/detection/include/detection/detection.h

@@ -17,6 +17,23 @@ struct BotPose {
     float theta = 0.;
 };
 
+struct SegmentationGrid {
+    int resolution = 0;
+    int width = 0;
+    int height = 0;
+    std::vector<uint8_t> data{};
+};
+
+struct Color {
+    Color() = default;
+    Color(double r, double g, double b, double a) : r(r), g(g), b(b), a(a) {}
+
+    double r = 0.;
+    double g = 0.;
+    double b = 0.;
+    double a = 1.;
+};
+
 struct Marker {
     Marker() = default;
     Marker(int id, const std::vector<cv::Point2f>& corners) : id(id), corners(corners) {}
@@ -41,13 +58,16 @@ namespace msg {
 std_msgs::msg::Header makeHeader(const rclcpp::Time& capturing_time);
 
 visualization_msgs::msg::ImageMarker makeLineStrip(
-    int id, const std::vector<cv::Point2f>& coords, const rclcpp::Time& capturing_time,
-    bool close = true, bool add = true);
+    int id, const std::vector<cv::Point2f>& coords, const Color& color, float scale,
+    const rclcpp::Time& capturing_time, bool close = true, bool add = true);
 
 foxglove_msgs::msg::ImageMarkerArray makeLineStripArray(
-    int id, const std::vector<std::vector<cv::Point2f>>& markers,
+    int id, const std::vector<std::vector<cv::Point2f>>& markers, const Color& color, float scale,
     const rclcpp::Time& capturing_time, bool close = true, bool add = true);
 
+visualization_msgs::msg::ImageMarker makePoints(
+    int id, const std::vector<cv::Point2f>& coords, const Color& color, float scale);
+
 visualization_msgs::msg::ImageMarker makePolygon(const std::vector<std::pair<int, int>>& coords);
 
 foxglove_msgs::msg::ImageMarkerArray makePolygonArray(

packages/detection/src/detection.cpp

@@ -50,8 +50,8 @@ std_msgs::msg::Header makeHeader(const rclcpp::Time& captured_time) {
 }
 
 visualization_msgs::msg::ImageMarker makeLineStrip(
-    int id, const std::vector<cv::Point2f>& coords, const rclcpp::Time& captured_time, bool close,
-    bool add) {
+    int id, const std::vector<cv::Point2f>& coords, const Color& color, float scale,
+    const rclcpp::Time& captured_time, bool close, bool add) {
     visualization_msgs::msg::ImageMarker msg;
 
     msg.header = makeHeader(captured_time);
@@ -66,24 +66,24 @@ visualization_msgs::msg::ImageMarker makeLineStrip(
 
     msg.action = visualization_msgs::msg::ImageMarker::ADD;
 
+    geometry_msgs::msg::Point point;
+    std_msgs::msg::ColorRGBA outline_color;
+
+    outline_color.r = color.r;
+    outline_color.g = color.g;
+    outline_color.b = color.b;
+    outline_color.a = color.a;
+
+    msg.outline_color = outline_color;
+    msg.scale = scale;
+
     size_t amount = coords.size();
     if (close) {
         amount += 1;
     }
 
-    geometry_msgs::msg::Point point;
-    std_msgs::msg::ColorRGBA color;
-
-    color.r = 1.0;
-    color.g = 0.0;
-    color.b = 0.0;
-    color.a = 1.0;
-
-    msg.outline_color = color;
-    msg.scale = 2;
-
     for (size_t i = 0; i < amount; ++i) {
-        msg.outline_colors.push_back(color);
+        msg.outline_colors.push_back(outline_color);
         point.x = coords[i % coords.size()].x;
         point.y = coords[i % coords.size()].y;
         msg.points.push_back(point);
@@ -93,18 +93,48 @@ visualization_msgs::msg::ImageMarker makeLineStrip(
 }
 
 foxglove_msgs::msg::ImageMarkerArray makeLineStripArray(
-    int id, const std::vector<std::vector<cv::Point2f>>& markers, const rclcpp::Time& captured_time,
-    bool close, bool add) {
+    int id, const std::vector<std::vector<cv::Point2f>>& markers, const Color& color, float scale,
+    const rclcpp::Time& captured_time, bool close, bool add) {
     foxglove_msgs::msg::ImageMarkerArray msg;
 
     for (const auto& marker : markers) {
-        const auto marker_msg = makeLineStrip(id, marker, captured_time, close, add);
+        const auto marker_msg = makeLineStrip(id, marker, color, scale, captured_time, close, add);
         msg.markers.push_back(marker_msg);
     }
 
     return msg;
 }
 
+visualization_msgs::msg::ImageMarker makePoints(
+    int id, const std::vector<cv::Point2f>& coords, const Color& color, float scale) {
+    visualization_msgs::msg::ImageMarker msg;
+
+    msg.ns = "";
+    msg.id = id;
+    msg.type = visualization_msgs::msg::ImageMarker::POINTS;
+    msg.action = visualization_msgs::msg::ImageMarker::ADD;
+
+    geometry_msgs::msg::Point point;
+    std_msgs::msg::ColorRGBA outline_color;
+
+    outline_color.r = color.r;
+    outline_color.g = color.g;
+    outline_color.b = color.b;
+    outline_color.a = color.a;
+
+    msg.outline_color = outline_color;
+    msg.scale = scale;
+
+    for (const auto& point_coords : coords) {
+        msg.outline_colors.push_back(outline_color);
+        point.x = point_coords.x;
+        point.y = point_coords.y;
+        msg.points.push_back(point);
+    }
+
+    return msg;
+}
+
 visualization_msgs::msg::ImageMarker makePolygon(const std::vector<std::pair<int, int>>& coords) {
     visualization_msgs::msg::ImageMarker msg;
 

packages/planner/CMakeLists.txt

@@ -0,0 +1,39 @@
+cmake_minimum_required(VERSION 3.8)
+project(planner)
+
+add_compile_options(-Wall -Wextra -Wpedantic)
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(Boost REQUIRED)
+find_package(foxglove_msgs REQUIRED)
+find_package(OpenCV REQUIRED)
+find_package(rclcpp REQUIRED)
+
+find_package(detection REQUIRED)
+find_package(wbb_msgs REQUIRED)
+
+add_executable(planner src/main.cpp src/planner_node.cpp src/graph_helper.cpp)
+
+target_include_directories(${PROJECT_NAME}  PUBLIC
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include/${PROJECT_NAME}>")
+
+ament_target_dependencies(
+    ${PROJECT_NAME} 
+    Boost
+    foxglove_msgs
+    OpenCV
+    rclcpp
+    detection
+    wbb_msgs
+)
+
+install(TARGETS ${PROJECT_NAME} 
+  DESTINATION lib/${PROJECT_NAME})
+
+  install(
+    DIRECTORY launch
+    DESTINATION share/${PROJECT_NAME})
+
+ament_package()

packages/planner/include/planner/graph_helper.h

@@ -0,0 +1,26 @@
+#pragma once
+
+#include "detection/detection.h"
+
+#include <boost/graph/adjacency_list.hpp>
+#include <opencv2/core.hpp>
+
+#include <iostream>
+#include <vector>
+#include <utility>
+
+namespace wbb::graph {
+
+cv::Point getPointFromSegmentation(int index, int width);
+
+using Graph = boost::adjacency_list<
+    boost::setS, boost::listS, boost::undirectedS, boost::property<boost::vertex_index_t, int>,
+    boost::property<boost::edge_weight_t, int>, boost::no_property>;
+
+using VertexDescriptor = Graph::vertex_descriptor;
+
+int getDist(const cv::Point& first, const cv::Point& second);
+
+std::vector<cv::Point> getPath(const BotPose& bot_pose, const SegmentationGrid& segmentation);
+
+}  // namespace wbb::graph