Dev

CMakeLists.txt

@@ -9,36 +9,17 @@ endif ()
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(image_geometry REQUIRED)
 # Messages TODO_EXTRA
-find_package(ackermann_msgs REQUIRED)
+# find_package(ackermann_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(nav_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(tf2_geometry_msgs REQUIRED)
-#find_package(std_msgs REQUIRED)
 # OpenCV TODO_EXTRA
 find_package(cv_bridge REQUIRED)
 find_package(OpenCV 4.2.0 REQUIRED)
 
-#for ros listeners
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(rclpy REQUIRED)
-# define the publisher and subscriper nodes with the node names
-add_executable(talker src/publisher_member_function.cpp)
-ament_target_dependencies(talker rclcpp std_msgs)
-install(TARGETS
-  talker
-  DESTINATION lib/${PROJECT_NAME})
-  #define project name
-#end ros listners includes
-
-# Add source for node executable (link non-ros dependencies here)
-add_executable(polynomial_planner src/PolynomialPlanner.cpp src/PolynomialPlanner_node.cpp)
-target_include_directories(polynomial_planner PUBLIC
-        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
-        $<INSTALL_INTERFACE:include>)
-target_compile_features(polynomial_planner PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
 
 # Node 2
 add_executable(polynomial_planner_ai src/PolynomialPlannerAi.cpp src/PolynomialPlannerAi_node.cpp)
@@ -51,7 +32,7 @@ target_compile_features(polynomial_planner_ai PUBLIC c_std_99 cxx_std_17)  # Req
 set(dependencies
         rclcpp
         # Messages TODO_EXTRA
-        ackermann_msgs
+        # ackermann_msgs
         sensor_msgs
         std_msgs
         nav_msgs
@@ -60,22 +41,19 @@ set(dependencies
         # OpenCv TODO_EXTRA
         cv_bridge
         OpenCV
+        # for camera space transformation
+        image_geometry
 )
 
 # Link ros dependencies
-ament_target_dependencies(
-        polynomial_planner
-        ${dependencies}
-)
+
 
 # Link ros dependencies
 ament_target_dependencies(
         polynomial_planner_ai
         ${dependencies}
 )
 
-install(TARGETS polynomial_planner
-        DESTINATION lib/${PROJECT_NAME})
 
 install(TARGETS polynomial_planner_ai
         DESTINATION lib/${PROJECT_NAME})
@@ -97,7 +75,7 @@ if (BUILD_TESTING)
     ament_add_gtest(${PROJECT_NAME}-test
             tests/unit.cpp
             # Remember to add node source files
-            src/PolynomialPlanner_node.cpp
+
             src/PolynomialPlannerAi_node.cpp
     )
     ament_target_dependencies(${PROJECT_NAME}-test ${dependencies})

include/polynomial_planner/PolynomialPlannerAi_node.hpp

@@ -1,17 +1,41 @@
 #pragma once
 
+#include <opencv2/core/types.hpp>
+#include <string>
+#include <vector>
+
+#include "image_geometry/pinhole_camera_model.h"
+#include "nav_msgs/msg/path.hpp"
 #include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/camera_info.hpp"
+#include "std_msgs/msg/float32_multi_array.hpp"
 #include "std_msgs/msg/string.hpp"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include "tf2_ros/buffer.h"
+#include "tf2_ros/transform_listener.h"
 
 class PolynomialPlannerAi : public rclcpp::Node {
 private:
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
+    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
+
+    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr poly_sub;
+
+    // Camera info sub & model vars
+    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr rgb_info_sub;
+    image_geometry::PinholeCameraModel rgb_model;
+
+    // std::optional<nav_msgs::msg::Path>
+    std::unique_ptr<std::optional<nav_msgs::msg::Path>> Backend;
 
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub;
+    // TF2 stuff
+    std::unique_ptr<tf2_ros::TransformListener> tf2_listener;
+    std::unique_ptr<tf2_ros::Buffer> tf2_buffer;
 
 public:
     PolynomialPlannerAi(const rclcpp::NodeOptions& options);
 
     /// subscriber callback
-    void sub_cb(std_msgs::msg::String::SharedPtr msg);
+    /// 'PolynomialPlannerAi::' is unnesscary
+    void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, image_geometry::PinholeCameraModel camera_rgb);
+    void evaluate_polynomial(const std::vector<float>& coeffs, const std::vector<float>& x_values);
 };

include/polynomial_planner/PolynomialPlanner_node.hpp

@@ -1,17 +1,39 @@
-#pragma once
-
-#include "rclcpp/rclcpp.hpp"
-#include "std_msgs/msg/string.hpp"
-
-class PolynomialPlanner : public rclcpp::Node {
-private:
-    rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub;
-
-    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub;
-
-public:
-    PolynomialPlanner(const rclcpp::NodeOptions& options);
-
-    /// subscriber callback
-    void sub_cb(std_msgs::msg::String::SharedPtr msg);
-};
+//#pragma once
+//
+//#include <opencv2/core/types.hpp>
+//#include <vector>
+//
+//#include "image_geometry/pinhole_camera_model.h"
+//#include "nav_msgs/msg/path.hpp"
+//#include "rclcpp/rclcpp.hpp"
+//#include "sensor_msgs/msg/camera_info.hpp"
+//#include "std_msgs/msg/string.hpp"
+//#include "tf2_ros/buffer.h"
+//#include "tf2_ros/transform_listener.h"
+//
+//class PolynomialPlanner : public rclcpp::Node {
+//private:
+//    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_pub;
+//
+//    rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr sub;
+//
+//    // Camera info sub & model vars
+//    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr rgb_info_sub;
+//    image_geometry::PinholeCameraModel rgb_model;
+//
+//    // std::optional<nav_msgs::msg::Path>
+//    std::unique_ptr<std::optional<nav_msgs::msg::Path>> Backend;
+//
+//    // TF2 stuff
+//    std::unique_ptr<tf2_ros::TransformListener> tf2_listener;
+//    std::unique_ptr<tf2_ros::Buffer> tf2_buffer;
+//
+//public:
+//    PolynomialPlanner(const rclcpp::NodeOptions& options);
+//
+//    /// subscriber callback
+//    void sub_cb(std_msgs::msg::String::SharedPtr msg);
+//
+//    // camera transform
+//    nav_msgs::msg::Path cameraPixelToGroundPos(std::vector<cv::Point2d>& pixels);
+//};

include/polynomial_planner/backend.hpp

@@ -2,41 +2,49 @@
 #include <fstream>
 #include <iostream>
 #include <optional>
-#include <vector>
+#include <sensor_msgs/msg/camera_info.hpp>
 #include <string_view>
-#include "nav_msgs/msgs/Path.hpp"
+#include <vector>
+
+#include "image_geometry/pinhole_camera_model.h"
+#include "nav_msgs/msg/path.hpp"
 
 class Polynomial {
 public:
     // TODO what does std::pmr::vec mean compared to std::vec ???
-    Polynomial(std::pmr::vector<float>& vect) { this->vect = vect; }
+    Polynomial(std::vector<float>& vect) { this->vect = vect; }
     Polynomial() = default;
     // store the vector
 private:
-    std::pmr::vector<float> vect;
+    std::vector<float> vect;
 
 public:
     // returns the y value at a given X.
     float poly(float x) {
         float result = 0;
         for (int i = 0; i < vect.size(); i++) {
-            int power = vect.size() - i;
+            int power = vect.size() - i - 1;
             // TODO this pow function might be wrong
+            // inproper way of accessing vect index
             result += vect[i] * pow(x, power);  // a[n] * x ^ n
         }
         return result;
     }
     float polyDirvative(float x) {
         float result = 0;
         for (int i = 0; i < vect.size() - 1; i++) {
-            int power = vect.size() - i - 1;
+            int power = vect.size() - i - 2;
             // todo finish?
-            result += vect[i] * i * pow(x, power);  // a[n] * n * x ^ (n - 1)
+            result += vect[i] * (power + 1) * pow(x, power);  // a[n] * n * x ^ (n - 1)
         }
+        return result;
     }
 };
 
 namespace backend {
-std::optional<nav_msgs::msg::Path> create_path(const std::vector& leftPoly,
-                                               const std::vector& rightPoly std::string_view frame);
-}
+
+std::vector<cv::Point2d> cameraPixelToGroundPos(std::vector<cv::Point2d>& pixels,
+                                                image_geometry::PinholeCameraModel rgb_info_sub);
+std::optional<nav_msgs::msg::Path> create_path(std::vector<float>& leftPoly,
+                                               image_geometry::PinholeCameraModel rgb_info_sub, std::string_view frame);
+}  // namespace backend

package.xml

@@ -20,6 +20,7 @@
     <depend>nav_msgs</depend>
     <depend>geometry_msgs</depend>
     <depend>tf2_geometry_msgs</depend>
+    <depend>image_geometry</depend>
 
     <!--OpenCv Things TODO_EXTRA-->
     <depend>cv_bridge</depend>

src/PolynomialPlannerAi_node.cpp

@@ -1,15 +1,89 @@
 #include "polynomial_planner/PolynomialPlannerAi_node.hpp"
 
+#include <string>
+
+#include "backend.cpp"
+
 // Required for doTransform
-#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+#include <tf2/LinearMath/Quaternion.h>
+
+#include <vector>
+
+#include "geometry_msgs/msg/pose_stamped.hpp"
+#include "image_geometry/pinhole_camera_model.h"
 
-// For _1
 using namespace std::placeholders;
 
 PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) {
+    // RGB_INFO PARAMETER DO NOT DELETE
+    this->declare_parameter("camera_frame", "mid_cam_link");
+
+    this->rgb_info_sub = this->create_subscription<sensor_msgs::msg::CameraInfo>(
+        "/camera/mid/rgb/camera_info", 1,
+        [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); });
+
+    this->poly_sub = this->create_subscription<std_msgs::msg::Float32MultiArray>(
+        "/road/polynomial", 1, [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
+            this->polynomial_cb(msg, this->rgb_model);
+        });  ///  TODO fix paras
+
+    this->path_pub = this->create_publisher<nav_msgs::msg::Path>("/path", 5);
 
+    RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data...");
+    // TF2 things
+    this->tf2_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
+    this->tf2_listener = std::make_unique<tf2_ros::TransformListener>(*this->tf2_buffer);
 }
 
-void PolynomialPlannerAi::sub_cb(std_msgs::msg::String::SharedPtr msg) {
+void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg,
+                                        image_geometry::PinholeCameraModel camera_rgb) {
+    // fix msg->empty
+    if (false) {
+        RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)");
+        return;
 
-}
+    } else {
+        std::vector<float> coeff{};
+        int no_coeff = msg->data.size();
+
+        for (int i = 0; i < no_coeff; i++) {
+            coeff.push_back(msg->data[i]);
+        }
+
+
+        //std::string frame_id = this->get_parameter("camera_frame").as_string();
+        //std::string frame_id = "notemptystring";
+        // TODO camera frame_id is wrong
+        auto frame_id = this->get_parameter(std::string("camera_frame")).as_string();
+        std::optional<nav_msgs::msg::Path> path_optional = backend::create_path(coeff, camera_rgb, frame_id);
+        nav_msgs::msg::Path path;
+
+
+        if (path_optional.has_value()) {
+            path = path_optional.value();
+            std::string p = std::to_string(path.poses.size());
+            RCLCPP_INFO(this->get_logger(), p.c_str());
+            path.header.frame_id = this->get_parameter(std::string("camera_frame")).as_string();
+            this->path_pub->publish(path);  // error invalid operator *path
+                                            // Extract and print coefficients
+            // RCLCPP_INFO(this->get_logger(), "Received Polynomial Coefficients:");
+            for (size_t i = 0; i < msg->data.size(); i++) {
+                // RCLCPP_INFO(this->get_logger(), "Coefficient[%zu] = %.15e", i, msg->data[i]);
+            }
+        } else {
+            std::string p = " error no path ";
+            RCLCPP_INFO(this->get_logger(), p.c_str());
+        }
+        return;
+    }
+}
+
+void PolynomialPlannerAi::evaluate_polynomial(const std::vector<float>& coeffs, const std::vector<float>& x_values) {
+    for (float x : x_values) {
+        float y = 0.0;
+        for (size_t i = 0; i < coeffs.size(); i++) {
+            y += coeffs[i] * std::pow(x, i);
+        }
+        // RCLCPP_INFO(this->get_logger(), "P(%f) = %f", x, y);
+    }
+}