merge plus new code

Author: redto0

include/polynomial_planner/backend.hpp

@@ -42,7 +42,6 @@ class Polynomial {
 };
 
 namespace backend {
-
 // std::vector<double> FitPolynomial(const std::vector<double>& x, const std::vector<double>& y, int degree);
 
 std::vector<cv::Point2d> getLeftContour(std::vector<double> message);

src/PolynomialPlannerAi_node.cpp

@@ -21,6 +21,9 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N
     // RGB_INFO PARAMETER DO NOT DELETE
     this->declare_parameter("camera_frame", "mid_cam_link");
 
+    // Frame params
+    this->declare_parameter("path_frame", "odom");
+
     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); });
@@ -71,18 +74,34 @@ void PolynomialPlannerAi::polynomial_cb(phnx_msgs::msg::Contours::SharedPtr msg,
 
         //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(cv_points_left, cv_points_right, 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();
+            path.header.stamp = this->get_clock()->now();
+
+            try {
+                // Transform path to odom. While the path is in the same location, its now WRT where the kart started, which
+                // means that as the kart moves and this path is transformed back WRT the kart, the path will have moved location.
+                auto trans = this->tf2_buffer->lookupTransform(this->get_parameter("path_frame").as_string(),
+                                                               path.header.frame_id, rclcpp::Time{});
+                for (auto& pose : path.poses) {
+                    tf2::doTransform(pose, pose, trans);
+                    pose.header.frame_id = this->get_parameter("path_frame").as_string();
+                    pose.header.stamp = this->get_clock()->now();
+                }
+                path.header.frame_id = this->get_parameter("path_frame").as_string();
+            } catch (tf2::LookupException& e) {
+                RCLCPP_INFO(this->get_logger(), "Could not look up odom!");
+                return;
+            }
+
             this->path_pub->publish(path);  // error invalid operator *path
                                             // Extract and print coefficients
             // RCLCPP_INFO(this->get_logger(), "Received Polynomial Coefficients:");

src/PolynomialPlanner_node.cpp

@@ -1,3 +1,4 @@
+
 #include "polynomial_planner/PolynomialPlanner_node.hpp"
 
 // Required for doTransform
@@ -85,6 +86,8 @@ void polynomial_pb(std_msgs::msg::Float32MultiArray::SharedPtr msg, image_geomet
         for (int i = 0; i < no_coeff; i++) {
             coeff.push_back(msg->data[i]);
         }
+        // TODO fix params
+        nav_msgs::msg::Path path = backend::create_path(coeff, camera_rgb, frame_id);
 
         std::string p = std::to_string(camera_rgb.cx());
         RCLCPP_INFO(this->get_logger(), p.c_str());
@@ -127,48 +130,3 @@ void polynomial_pb(std_msgs::msg::Float32MultiArray::SharedPtr msg, image_geomet
         return;
     }
 }
-
-/* not so obvious reference:
-geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vector<cv::Point2d>& object_locations,
-                                                                  const cv::Mat& depth) {
-    geometry_msgs::msg::PoseArray poses{};
-    poses.header.frame_id = this->get_parameter(std::string{"camera_frame"}).as_string();
-
-    // Rotation that rotates left 90 and backwards 90.
-    // This converts from camera coordinates in OpenCV to ROS coordinates
-    tf2::Quaternion optical_to_ros{};
-    optical_to_ros.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
-
-    for (const cv::Point2d& center : object_locations) {
-        if (!this->rgb_model.initialized()) {
-            continue;
-        }
-
-        // Project pixel to camera space
-        auto ray = this->rgb_model.projectPixelTo3dRay(center);
-        // The oak-d uses shorts in mm, sim uses f32 in m
-        float dist = depth.type() == 2 ? (float)depth.at<short>(center) / 1000 : depth.at<float>(center);
-
-        // If depth unavailable, then skip
-        if (dist == INFINITY || dist >= 10 || dist <= 0) {
-            continue;
-        }
-
-        // Just resize the ray to be a vector at the distance of the depth pixel. This is in camera space
-        auto point_3d = dist / cv::norm(ray) * ray;
-
-        // Convert from camera space to ros coordinates ("World" but wrt camera mount)
-        tf2::Vector3 tf_vec{point_3d.x, point_3d.y, point_3d.z};
-        auto world_vec = tf2::quatRotate(optical_to_ros, tf_vec);
-
-        geometry_msgs::msg::Pose p{};
-        p.position.x = world_vec.x();
-        p.position.y = world_vec.y();
-        p.position.z = world_vec.z();
-        poses.poses.push_back(p);
-    }
-
-    poses.header.stamp = this->get_clock()->now();
-    return poses;
-}
-    */

src/backend.cpp

@@ -13,7 +13,7 @@
 
 std::optional<nav_msgs::msg::Path> backend::create_path(std::vector<cv::Point2d>& left_contours,
                                                         std::vector<cv::Point2d>& right_contours,
-                                                        image_geometry::PinholeCameraModel& rgb_info_sub,
+                                                        image_geometry::PinholeCameraModel& camera_info,
                                                         std::string_view frame_id) {
     // take in contours
     // DO NOT THE Polynomials
@@ -30,8 +30,8 @@ std::optional<nav_msgs::msg::Path> backend::create_path(std::vector<cv::Point2d>
     std::vector<cv::Point2d> cam_path;     // this is the vector of path plannign points in camera space
     // ground_path.emplace_back(cv::Point2d(0, 0));
 
-    int width = rgb_info_sub.fullResolution().width;    // camera space sizes!
-    int height = rgb_info_sub.fullResolution().height;  // Camera space sizes!
+    int width = camera_info.fullResolution().width;    // camera space sizes!
+    int height = camera_info.fullResolution().height;  // Camera space sizes!
 
     bool is_right_valid = true;  // stores if Polynomial was intizatized!
     bool is_left_valid = true;   // left and right respectively
@@ -51,11 +51,11 @@ std::optional<nav_msgs::msg::Path> backend::create_path(std::vector<cv::Point2d>
     bool is_left_bigger = left_contours.size() > right_contours.size();
 
     if (is_left_bigger) {
-        bigger_array = cameraPixelToGroundPos(left_contours, rgb_info_sub);
-        smaller_array = cameraPixelToGroundPos(right_contours, rgb_info_sub);
+        bigger_array = cameraPixelToGroundPos(left_contours, camera_info);
+        smaller_array = cameraPixelToGroundPos(right_contours, camera_info);
     } else {
-        smaller_array = cameraPixelToGroundPos(left_contours, rgb_info_sub);
-        bigger_array = cameraPixelToGroundPos(right_contours, rgb_info_sub);
+        smaller_array = cameraPixelToGroundPos(left_contours, camera_info);
+        bigger_array = cameraPixelToGroundPos(right_contours, camera_info);
     }
 
     for (int i = 0; i < smaller_array.size(); i++) {
@@ -89,7 +89,7 @@ std::optional<nav_msgs::msg::Path> backend::create_path(std::vector<cv::Point2d>
 
         // convert from camera to ground
         // done further ahead already
-        // std::vector<cv::Point2d> ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub);
+        // std::vector<cv::Point2d> ground_path = cameraPixelToGroundPos(cam_path, camera_info);
 
         nav_msgs::msg::Path msg{};
         // msg.header.frame_id = frame;
@@ -162,59 +162,3 @@ std::vector<cv::Point2d> backend::cameraPixelToGroundPos(std::vector<cv::Point2d
 
     return rwpoints;
 }
-
-// // why are the pointer things the way they are
-// // TODO: make it not die when z is too small
-// //       or make z not too small
-// std::vector<cv::Point2d> backend::cameraPixelToGroundPos(std::vector<cv::Point2d>& pixels,
-//                                                          image_geometry::PinholeCameraModel rgb_info_sub, int i) {
-//     // Rotation that rotates left 90 and backwards 90.
-//     // This converts from camera coordinates in OpenCV to ROS coordinates
-//     tf2::Quaternion optical_to_ros{};
-//     // set the Roll Pitch YAW
-//     /// optical_to_ros.setRPY(0.0, 0.0, 0.0);
-//     optical_to_ros.setRPY(-M_PI / 2, 0.0, -M_PI / 2);
-
-//     std::vector<cv::Point2d> rwpoints;
-//     const double camera_height = 0.6;  // meters
-
-//     for (cv::Point2d& pixel : pixels) {
-//         // gotta rectify the pixel before we raycast
-//         // pixel.y += 120;
-//         // pixel.x += 320;
-//         // cv::Point2d rectPixel = rgb_info_sub.rectifyPoint(pixel);
-//         cv::Point3d ray = rgb_info_sub.projectPixelTo3dRay(pixel);
-
-//         // safety check
-//         if (fabs(ray.y) < 1e-6) {  // Near zero check
-//             // RCLCPP_WARN(rclcpp::get_logger("backend"), "Invalid ray projection (y near zero)");
-//             continue;  // Skip this point
-//         }
-
-//         // -- CAMERA COORDINATES --
-//         //      positive x = +X TO CAMERA
-//         //      positive y = STRAIGHT TO GROUND
-//         //      positive z = OUT OF CAMERA
-//         //      hopefully
-
-//         // ask zach for the trig, extend ray to the floor.
-//         double divisor = ray.y / camera_height;  // divide by how high off the ground the camera is!
-//         ray.x = ray.x / divisor;
-//         ray.y = ray.y / divisor;
-//         ray.z = ray.z / divisor;
-//         // ray /= ray.z / -0.6;
-//         // ray.z = (ray.z * -1);  // we don't really care abt z, since it -will- *should* always just be cameraHeight
-//         // tf2::Vector3 tf_vec{ray.z, -ray.x, -ray.y};
-//         tf2::Vector3 tf_vec{ray.x, ray.y, ray.z};
-//         tf_vec = tf2::quatRotate(optical_to_ros, tf_vec);
-
-//         //return type world_vec, use this is
-
-//         cv::Point2d dvector(tf_vec.x(), tf_vec.y());
-
-//         // push back vectors
-//         rwpoints.push_back(dvector);
-//     }
-
-//     return rwpoints;
-// }

tests/unit.cpp

@@ -2,7 +2,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
-#include "polynomial_planner/PolynomialPlanner_node.hpp"
+// #include "polynomial_planner/PolynomialPlanner_node.hpp"
 
 TEST(PolynomialPlanner, Test1) {}