From 2a391be0198754a5cc7105e138ef557698a30c74 Mon Sep 17 00:00:00 2001 From: redtoo Date: Thu, 13 Feb 2025 17:07:09 -0500 Subject: [PATCH 01/46] formated --- CMakeLists.txt | 12 ++-- .../PolynomialPlanner_node.hpp | 8 +++ include/polynomial_planner/backend.hpp | 3 +- src/PolynomialPlannerAi_node.cpp | 8 +-- src/PolynomialPlanner_node.cpp | 13 +++- src/backend.cpp | 64 +++++++++---------- src/publisher_member_function.cpp | 44 ++++++------- src/subscriber_member_function.cpp | 33 ++++------ 8 files changed, 92 insertions(+), 93 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 48d7af3..51d891f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -10,9 +10,9 @@ endif () find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) # Messages TODO_EXTRA -find_package(ackermann_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(nav_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) @@ -51,10 +51,10 @@ target_compile_features(polynomial_planner_ai PUBLIC c_std_99 cxx_std_17) # Req set(dependencies rclcpp # Messages TODO_EXTRA - ackermann_msgs - sensor_msgs + # ackermann_msgs + # sensor_msgs std_msgs - nav_msgs + # nav_msgs geometry_msgs tf2_geometry_msgs # OpenCv TODO_EXTRA diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 686bc3e..11fdc15 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,5 +1,7 @@ #pragma once +#include + #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" @@ -9,6 +11,12 @@ class PolynomialPlanner : public rclcpp::Node { rclcpp::Subscription::SharedPtr sub; + // std::optional + std::unique_ptr> Backend; + + // TF2 stuff + // std::unique_ptr tf2_listener; + // std::unique_ptr tf2_buffer; public: PolynomialPlanner(const rclcpp::NodeOptions& options); diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 41306da..df5f4e4 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -2,8 +2,9 @@ #include #include #include -#include #include +#include + #include "nav_msgs/msgs/Path.hpp" class Polynomial { diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 4b28ec9..3342486 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -6,10 +6,6 @@ // For _1 using namespace std::placeholders; -PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { +PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) {} -} - -void PolynomialPlannerAi::sub_cb(std_msgs::msg::String::SharedPtr msg) { - -} \ No newline at end of file +void PolynomialPlannerAi::sub_cb(std_msgs::msg::String::SharedPtr msg) {} \ No newline at end of file diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index e5ffd1a..17e0a9e 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -1,15 +1,22 @@ #include "polynomial_planner/PolynomialPlanner_node.hpp" // Required for doTransform +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // For _1 using namespace std::placeholders; PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node("polynomial_planner", options) { + // delcare params + auto random_int = this->declare_parameter("random_int", 0); -} + pub = this->create_publisher("topic", 10); -void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) { + // TF2 things + // this->tf2_buffer = std::make_unique(this->get_clock()); + // this->tf2_listener = std::make_unique(*this->tf2_buffer); +} -} \ No newline at end of file +void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} \ No newline at end of file diff --git a/src/backend.cpp b/src/backend.cpp index 44c4d06..d1e2d76 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -1,52 +1,51 @@ #include "polynomial_planner/backend.hpp" -#include "geometry_msgs/msgs/PoseStamped.hpp" #include +#include "geometry_msgs/msgs/PoseStamped.hpp" + std::optional backend::create_path(const std::vector& leftPoly, - const std::vector& rightPoly - std::string_view frame) { -// std::string_view is a string lol + const std::vector& rightPoly std::string_view frame) { + // std::string_view is a string lol std::vector path; // this is the array for cone points // this will make the // Start path from kart - path.emplace_back(0, 0); + path.emplace_back(0, 0); // take in Polynomial // ros polynoial take in code... // transfer to Polynomial class; float is_right_valid = false; float is_left_valid = false; - for (int i = 0; i < /* listenerArry.length*/; i++){ + for (int i = 0; i < /* listenerArry.length*/; i++) { + is_left_valid = (leftPoly == NULL) ? is_left_valid : (leftPoly[i] != 0) ? true : is_left_valid; - is_left_valid = ( leftPoly == NULL) ? is_left_valid : ( leftPoly[i] != 0 ) ? true : is_left_valid; - - is_right_valid = ( rightPoly == NULL) ? is_right_valid : ( rightPoly[i] != 0 ) ? true : is_right_valid; + is_right_valid = (rightPoly == NULL) ? is_right_valid : (rightPoly[i] != 0) ? true : is_right_valid; } - leftPoly = (is_left_valid) ? new Polynomial( /* vector from ros listener */) : null; - if(is_left_valid){ - Polynomial leftPoly = new Polynomial( /* vector from ros listener */); - }else { + leftPoly = (is_left_valid) ? new Polynomial(/* vector from ros listener */) : null; + if (is_left_valid) { + Polynomial leftPoly = new Polynomial(/* vector from ros listener */); + } else { // TODO this is lazy and bad fix please Polynomial leftPoly = null; } - rightPoly = (is_right_valid) ? new Polynomial( /* vector from ros listener */) : null; - if(is_right_valid){ - Polynomial rightPoly = new Polynomial( /* vector from ros listener */); + rightPoly = (is_right_valid) ? new Polynomial(/* vector from ros listener */) : null; + if (is_right_valid) { + Polynomial rightPoly = new Polynomial(/* vector from ros listener */); } else { // TODO this is lazy and bad fix please Polynomial rightPoly = null; } // interval for polynomial - float max = 10; - float interval = 0.25; - float start = 0; + float max = 10; + float interval = 0.25; + float start = 0; std::vector nav_points; - for (float i = start; i < max; i += interval){ + for (float i = start; i < max; i += interval) { // generate points nav_points.pushback(); - if (leftPoly != null){ + if (leftPoly != null) { // do left poly math; // float dx = leftPoly.polyDirvative(x); @@ -54,28 +53,28 @@ std::optional backend::create_path(const std::vector& leftP // becuase I dont want to use a wrapper class float dy = 1; // set dy to 1 - float l = std::sqrt( dx*dx + dy*dy); + float l = std::sqrt(dx * dx + dy * dy); // find the magnitude - dx = dx/l; // normalize - dy = dy/l; // normalize + dx = dx / l; // normalize + dy = dy / l; // normalize cv::Point2d temp; - float x = i; // define x - float y = leftPoly.poly(x); // define y - temp.x = (x + 7 * dy); // project x - temp.y = (y - 7 * dx); // project y + float x = i; // define x + float y = leftPoly.poly(x); // define y + temp.x = (x + 7 * dy); // project x + temp.y = (y - 7 * dx); // project y nav_points[i].pushback(temp); // return vector as < y, -x > } - if (rightPoly != null){ + if (rightPoly != null) { // do right poly math // <-y, x> // same steps different numbers float dx = rightPoly.polyDirvative(x); float dy = 1; - float l = std::sqrt( dx*dx + dy*dy); + float l = std::sqrt(dx * dx + dy * dy); l = std::hypot(dx, dy); - dx = dx/l; - dy = dy/l; + dx = dx / l; + dy = dy / l; cv::Point2d temp; float x = i; float y = leftPoly.poly(x); @@ -85,7 +84,6 @@ std::optional backend::create_path(const std::vector& leftP // return vector as < -y , x > } // rememebr to return - } if (path.empty()) { diff --git a/src/publisher_member_function.cpp b/src/publisher_member_function.cpp index b211a5e..632881b 100644 --- a/src/publisher_member_function.cpp +++ b/src/publisher_member_function.cpp @@ -25,34 +25,28 @@ using namespace std::chrono_literals; /* This example creates a subclass of Node and uses std::bind() to register a * member function as a callback from the timer. */ -class MinimalPublisher : public rclcpp::Node -{ +class MinimalPublisher : public rclcpp::Node { public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } + MinimalPublisher() : Node("minimal_publisher"), count_(0) { + publisher_ = this->create_publisher("topic", 10); + timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this)); + } private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; + void timer_callback() { + auto message = std_msgs::msg::String(); + message.data = "Hello, world! " + std::to_string(count_++); + RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); + publisher_->publish(message); + } + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Publisher::SharedPtr publisher_; + size_t count_; }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } diff --git a/src/subscriber_member_function.cpp b/src/subscriber_member_function.cpp index 40f27f7..cd557d5 100644 --- a/src/subscriber_member_function.cpp +++ b/src/subscriber_member_function.cpp @@ -20,28 +20,23 @@ using std::placeholders::_1; -class MinimalSubscriber : public rclcpp::Node -{ +class MinimalSubscriber : public rclcpp::Node { public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } + MinimalSubscriber() : Node("minimal_subscriber") { + subscription_ = this->create_subscription( + "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); + } private: - void topic_callback(const std_msgs::msg::String & msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; + void topic_callback(const std_msgs::msg::String& msg) const { + RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); + } + rclcpp::Subscription::SharedPtr subscription_; }; -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; +int main(int argc, char* argv[]) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } From de710b4d109d2bffa4c52ad7a5430bf1dc930ce1 Mon Sep 17 00:00:00 2001 From: redto0 <=> Date: Thu, 13 Feb 2025 17:36:06 -0500 Subject: [PATCH 02/46] added includes --- .../polynomial_planner/PolynomialPlanner_node.hpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 11fdc15..449990c 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,9 +1,15 @@ #pragma once -#include +// #include +#include "opencv2/core/types.hpp" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" + +#include "tf2_ros/buffer.h" +#include "tf2_ros/transform_listener.h" + +#include "rclcpp/rclcpp.hpp" +// #include "std_msgs/msg/string.hpp" class PolynomialPlanner : public rclcpp::Node { private: @@ -15,11 +21,12 @@ class PolynomialPlanner : public rclcpp::Node { std::unique_ptr> Backend; // TF2 stuff - // std::unique_ptr tf2_listener; - // std::unique_ptr tf2_buffer; + td::unique_ptr tf2_listener; + td::unique_ptr tf2_buffer; public: PolynomialPlanner(const rclcpp::NodeOptions& options); /// subscriber callback void sub_cb(std_msgs::msg::String::SharedPtr msg); }; + From 0330eea6270a7c9d042c2988506b18b11bbb5d83 Mon Sep 17 00:00:00 2001 From: redto0 <=> Date: Thu, 13 Feb 2025 20:00:43 -0500 Subject: [PATCH 03/46] reformat for build --- CMakeLists.txt | 4 ++++ include/polynomial_planner/PolynomialPlanner_node.hpp | 5 +---- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 51d891f..ce53b41 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -26,7 +26,11 @@ 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) +add_executable(talkerCV src/PolynomialPlanner_node.cpp) +add_executable(talkerAI src/PolynomialPlanner_node.cpp) ament_target_dependencies(talker rclcpp std_msgs) +ament_target_dependencies(talkerCV rclcpp std_msgs) +ament_target_dependencies(talkerAI rclcpp std_msgs) install(TARGETS talker DESTINATION lib/${PROJECT_NAME}) diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 449990c..52f81af 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -4,11 +4,8 @@ #include "opencv2/core/types.hpp" #include "rclcpp/rclcpp.hpp" - #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" - -#include "rclcpp/rclcpp.hpp" // #include "std_msgs/msg/string.hpp" class PolynomialPlanner : public rclcpp::Node { @@ -23,10 +20,10 @@ class PolynomialPlanner : public rclcpp::Node { // TF2 stuff td::unique_ptr tf2_listener; td::unique_ptr tf2_buffer; + public: PolynomialPlanner(const rclcpp::NodeOptions& options); /// subscriber callback void sub_cb(std_msgs::msg::String::SharedPtr msg); }; - From 70394efc7d080181e4b42c0bbba06333c43432d4 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Fri, 14 Feb 2025 15:49:57 -0500 Subject: [PATCH 04/46] fixed include paths to allow to compile --- CMakeLists.txt | 17 ++++++++--------- .../PolynomialPlanner_node.hpp | 10 +++++----- include/polynomial_planner/backend.hpp | 2 +- src/PolynomialPlanner_node.cpp | 2 +- src/backend.cpp | 7 ++++--- 5 files changed, 19 insertions(+), 19 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index ce53b41..4eac80d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -11,11 +11,10 @@ find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) # Messages TODO_EXTRA # find_package(ackermann_msgs REQUIRED) -# find_package(sensor_msgs REQUIRED) -# find_package(nav_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) @@ -26,11 +25,11 @@ 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) -add_executable(talkerCV src/PolynomialPlanner_node.cpp) -add_executable(talkerAI src/PolynomialPlanner_node.cpp) +# add_executable(talkerCV src/PolynomialPlanner_node.cpp) +# add_executable(talkerAI src/PolynomialPlanner_node.cpp) ament_target_dependencies(talker rclcpp std_msgs) -ament_target_dependencies(talkerCV rclcpp std_msgs) -ament_target_dependencies(talkerAI rclcpp std_msgs) +# ament_target_dependencies(talkerCV rclcpp std_msgs) +# ament_target_dependencies(talkerAI rclcpp std_msgs) install(TARGETS talker DESTINATION lib/${PROJECT_NAME}) @@ -56,9 +55,9 @@ set(dependencies rclcpp # Messages TODO_EXTRA # ackermann_msgs - # sensor_msgs + sensor_msgs std_msgs - # nav_msgs + nav_msgs geometry_msgs tf2_geometry_msgs # OpenCv TODO_EXTRA diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 52f81af..4c883c7 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,12 +1,12 @@ #pragma once -// #include +#include -#include "opencv2/core/types.hpp" +#include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -// #include "std_msgs/msg/string.hpp" class PolynomialPlanner : public rclcpp::Node { private: @@ -18,8 +18,8 @@ class PolynomialPlanner : public rclcpp::Node { std::unique_ptr> Backend; // TF2 stuff - td::unique_ptr tf2_listener; - td::unique_ptr tf2_buffer; + std::unique_ptr tf2_listener; + std::unique_ptr tf2_buffer; public: PolynomialPlanner(const rclcpp::NodeOptions& options); diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index df5f4e4..2c53f3c 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -5,7 +5,7 @@ #include #include -#include "nav_msgs/msgs/Path.hpp" +#include "nav_msgs/msg/path.hpp" class Polynomial { public: diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 17e0a9e..2b8f5b1 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -12,7 +12,7 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( // delcare params auto random_int = this->declare_parameter("random_int", 0); - pub = this->create_publisher("topic", 10); + pub = this->create_publisher("topic", 10); // TF2 things // this->tf2_buffer = std::make_unique(this->get_clock()); diff --git a/src/backend.cpp b/src/backend.cpp index d1e2d76..b67b3db 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -24,10 +24,11 @@ std::optional backend::create_path(const std::vector& leftP } leftPoly = (is_left_valid) ? new Polynomial(/* vector from ros listener */) : null; if (is_left_valid) { - Polynomial leftPoly = new Polynomial(/* vector from ros listener */); + Polynomistd_msgs::Stringal leftPoly = new Polynomial(/* vector from ros listener */); } else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; + std_msgs::String + // TODO this is lazy and bad fix please + Polynomial leftPoly = null; } rightPoly = (is_right_valid) ? new Polynomial(/* vector from ros listener */) : null; if (is_right_valid) { From 4b756237c034630bee73aa684ef4c657d5329100 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Fri, 14 Feb 2025 17:10:24 -0500 Subject: [PATCH 05/46] Add depth --- CMakeLists.txt | 3 ++ .../PolynomialPlanner_node.hpp | 13 ++++++++ src/PolynomialPlanner_node.cpp | 32 ++++++++++++++++++- 3 files changed, 47 insertions(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 4eac80d..53474a1 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ 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(sensor_msgs REQUIRED) @@ -63,6 +64,8 @@ set(dependencies # OpenCv TODO_EXTRA cv_bridge OpenCV + # for camera space transformation + image_geometry ) # Link ros dependencies diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 4c883c7..143a9f3 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -2,11 +2,15 @@ #include +#include + #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "sensor_msgs/msg/camera_info.hpp" +#include "image_geometry/pinhole_camera_model.h" class PolynomialPlanner : public rclcpp::Node { private: @@ -14,6 +18,10 @@ class PolynomialPlanner : public rclcpp::Node { rclcpp::Subscription::SharedPtr sub; + // Camera info sub & model vars + rclcpp::Subscription::SharedPtr rgb_info_sub; + image_geometry::PinholeCameraModel rgb_model; + // std::optional std::unique_ptr> Backend; @@ -21,9 +29,14 @@ class PolynomialPlanner : public rclcpp::Node { std::unique_ptr tf2_listener; std::unique_ptr tf2_buffer; + + public: PolynomialPlanner(const rclcpp::NodeOptions& options); /// subscriber callback void sub_cb(std_msgs::msg::String::SharedPtr msg); + + // camera transform + std::vector cameraPixelToGroundPos(std::vector& pixels); }; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 2b8f5b1..8ffc27b 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -4,6 +4,9 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include + // For _1 using namespace std::placeholders; @@ -14,9 +17,36 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( pub = this->create_publisher("topic", 10); + this->rgb_info_sub = this->create_subscription("/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this -> rgb_model.fromCameraInfo(ci); }); + // TF2 things // this->tf2_buffer = std::make_unique(this->get_clock()); // this->tf2_listener = std::make_unique(*this->tf2_buffer); } -void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} \ No newline at end of file +void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} + +// 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 PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { + + std::vector rwpoints; + for (const cv::Point2d& pixel : pixels) { + + // gotta rectify the pixel before we raycast + cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); + cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); + + // ask zach for the trig + ray /= ray.z / 0.6; + // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight + + cv::Point2d dvector(ray.x,ray.y); + rwpoints.push_back(dvector); + + } + + return rwpoints; +} \ No newline at end of file From d6a486c5786a3796e7cb87e01821864c7049a800 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Fri, 14 Feb 2025 17:13:24 -0500 Subject: [PATCH 06/46] Reformat for derek --- .../PolynomialPlanner_node.hpp | 9 +++------ src/PolynomialPlanner_node.cpp | 18 ++++++++---------- 2 files changed, 11 insertions(+), 16 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 143a9f3..6fa3e71 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,16 +1,15 @@ #pragma once #include - #include +#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" -#include "sensor_msgs/msg/camera_info.hpp" -#include "image_geometry/pinhole_camera_model.h" class PolynomialPlanner : public rclcpp::Node { private: @@ -29,14 +28,12 @@ class PolynomialPlanner : public rclcpp::Node { std::unique_ptr tf2_listener; std::unique_ptr tf2_buffer; - - public: PolynomialPlanner(const rclcpp::NodeOptions& options); /// subscriber callback void sub_cb(std_msgs::msg::String::SharedPtr msg); - // camera transform + // camera transform std::vector cameraPixelToGroundPos(std::vector& pixels); }; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 8ffc27b..dc933ac 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -1,12 +1,12 @@ #include "polynomial_planner/PolynomialPlanner_node.hpp" // Required for doTransform +#include + #include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include - // For _1 using namespace std::placeholders; @@ -17,8 +17,9 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( pub = this->create_publisher("topic", 10); - this->rgb_info_sub = this->create_subscription("/camera/mid/rgb/camera_info", 1, - [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this -> rgb_model.fromCameraInfo(ci); }); + this->rgb_info_sub = this->create_subscription( + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); // TF2 things // this->tf2_buffer = std::make_unique(this->get_clock()); @@ -31,21 +32,18 @@ void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} // TODO: make it not die when z is too small // or make z not too small std::vector PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { - std::vector rwpoints; for (const cv::Point2d& pixel : pixels) { - // gotta rectify the pixel before we raycast cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); - + // ask zach for the trig ray /= ray.z / 0.6; // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight - cv::Point2d dvector(ray.x,ray.y); + cv::Point2d dvector(ray.x, ray.y); rwpoints.push_back(dvector); - } return rwpoints; From 130bd2132ac4960fd8888b1359edeaca298268a3 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Fri, 14 Feb 2025 17:49:17 -0500 Subject: [PATCH 07/46] switch from two polynomial to one --- src/backend.cpp | 75 +++++++++++-------------------------------------- 1 file changed, 17 insertions(+), 58 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index b67b3db..dae4d93 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,8 +4,7 @@ #include "geometry_msgs/msgs/PoseStamped.hpp" -std::optional backend::create_path(const std::vector& leftPoly, - const std::vector& rightPoly std::string_view frame) { +std::optional backend::create_path(const std::vector& leftPoly, std::string_view frame) { // std::string_view is a string lol std::vector path; // this is the array for cone points // this will make the @@ -19,72 +18,32 @@ std::optional backend::create_path(const std::vector& leftP for (int i = 0; i < /* listenerArry.length*/; i++) { is_left_valid = (leftPoly == NULL) ? is_left_valid : (leftPoly[i] != 0) ? true : is_left_valid; - - is_right_valid = (rightPoly == NULL) ? is_right_valid : (rightPoly[i] != 0) ? true : is_right_valid; } leftPoly = (is_left_valid) ? new Polynomial(/* vector from ros listener */) : null; if (is_left_valid) { - Polynomistd_msgs::Stringal leftPoly = new Polynomial(/* vector from ros listener */); + Polynomial leftPoly = new Polynomial(/* vector from ros listener */); } else { - std_msgs::String // TODO this is lazy and bad fix please Polynomial leftPoly = null; } - rightPoly = (is_right_valid) ? new Polynomial(/* vector from ros listener */) : null; - if (is_right_valid) { - Polynomial rightPoly = new Polynomial(/* vector from ros listener */); - } else { - // TODO this is lazy and bad fix please - Polynomial rightPoly = null; - } // interval for polynomial - float max = 10; - float interval = 0.25; - float start = 0; - std::vector nav_points; - for (float i = start; i < max; i += interval) { - // generate points - nav_points.pushback(); - if (leftPoly != null) { - // do left poly math; - // - float dx = leftPoly.polyDirvative(x); - // calulate dx were dy is 1 - // becuase I dont want to use a wrapper class - float dy = 1; - // set dy to 1 - float l = std::sqrt(dx * dx + dy * dy); - // find the magnitude - dx = dx / l; // normalize - dy = dy / l; // normalize - cv::Point2d temp; - float x = i; // define x - float y = leftPoly.poly(x); // define y - temp.x = (x + 7 * dy); // project x - temp.y = (y - 7 * dx); // project y - nav_points[i].pushback(temp); - // return vector as < y, -x > - } - if (rightPoly != null) { - // do right poly math - // <-y, x> - // same steps different numbers - float dx = rightPoly.polyDirvative(x); - float dy = 1; - float l = std::sqrt(dx * dx + dy * dy); - l = std::hypot(dx, dy); - dx = dx / l; - dy = dy / l; - cv::Point2d temp; - float x = i; - float y = leftPoly.poly(x); - temp.x = (x - 7 * dy); - temp.y = (y + 7 * dx); - nav_points[i].pushback(temp); - // return vector as < -y , x > + float max = 200; + float interval = 3; + float start = 5; + float threshold = 15.0; + + // TODO this is lazy and bad fix please + float dist = 0; + for (int i = start; i < max; i += interval) { + + dist += sqrt(interval * interval + pow(leftPoly.poly(i) - leftPoly.poly(i + interval), 2)); + + if (dist > threshold) { + path.pushback(cv::Point2d(i,leftPoly.poly(i))); + dist = 0; } - // rememebr to return + } if (path.empty()) { From 869eb4a98a79bec407b8bdcaefe7e8e53134045e Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Fri, 14 Feb 2025 18:08:31 -0500 Subject: [PATCH 08/46] make derek happy --- src/backend.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index dae4d93..156572a 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -23,8 +23,8 @@ std::optional backend::create_path(const std::vector& leftP if (is_left_valid) { Polynomial leftPoly = new Polynomial(/* vector from ros listener */); } else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; + // TODO this is lazy and bad fix please + Polynomial leftPoly = null; } // interval for polynomial @@ -32,18 +32,16 @@ std::optional backend::create_path(const std::vector& leftP float interval = 3; float start = 5; float threshold = 15.0; - + // TODO this is lazy and bad fix please float dist = 0; for (int i = start; i < max; i += interval) { - dist += sqrt(interval * interval + pow(leftPoly.poly(i) - leftPoly.poly(i + interval), 2)); if (dist > threshold) { - path.pushback(cv::Point2d(i,leftPoly.poly(i))); + path.pushback(cv::Point2d(i, leftPoly.poly(i))); dist = 0; } - } if (path.empty()) { From f524af2fc9728eea5aa4de46b60fd8afffed1502 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Mon, 17 Feb 2025 20:22:30 -0500 Subject: [PATCH 09/46] bug update + derek happy comaand --- src/backend.cpp | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 156572a..2b87c5b 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -16,16 +16,10 @@ std::optional backend::create_path(const std::vector& leftP float is_right_valid = false; float is_left_valid = false; - for (int i = 0; i < /* listenerArry.length*/; i++) { + for (int i = 0; i < leftPoly.size(); i++) { is_left_valid = (leftPoly == NULL) ? is_left_valid : (leftPoly[i] != 0) ? true : is_left_valid; } - leftPoly = (is_left_valid) ? new Polynomial(/* vector from ros listener */) : null; - if (is_left_valid) { - Polynomial leftPoly = new Polynomial(/* vector from ros listener */); - } else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; - } + leftPoly = (is_left_valid) ? new Polynomial(leftPoly) : null; // interval for polynomial float max = 200; @@ -40,7 +34,14 @@ std::optional backend::create_path(const std::vector& leftP if (dist > threshold) { path.pushback(cv::Point2d(i, leftPoly.poly(i))); - dist = 0; + // TODO was disif probably a typo thb + if (is_left_valid) { + Polynomial leftPoly = new Polynomial(leftPoly); + } else { + // TODO this is lazy and bad fix please + Polynomial leftPoly = null; + } + t = 0; } } From 5ed500f9e59f6af8d4feeb27c9d55cf7cbafa5ec Mon Sep 17 00:00:00 2001 From: Jared Date: Mon, 17 Feb 2025 20:34:35 -0500 Subject: [PATCH 10/46] pub/sub works from detector to planner --- CMakeLists.txt | 32 +-------- .../PolynomialPlannerAi_node.hpp | 15 ++++- .../PolynomialPlanner_node.hpp | 3 +- include/polynomial_planner/backend.hpp | 43 ------------ package.xml | 1 + src/PolynomialPlannerAi_node.cpp | 21 +++++- src/PolynomialPlanner_node.cpp | 18 +++-- src/backend.cpp | 67 ------------------- src/publisher_member_function.cpp | 52 -------------- src/subscriber_member_function.cpp | 42 ------------ 10 files changed, 48 insertions(+), 246 deletions(-) delete mode 100644 include/polynomial_planner/backend.hpp delete mode 100644 src/backend.cpp delete mode 100644 src/publisher_member_function.cpp delete mode 100644 src/subscriber_member_function.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 53474a1..0708565 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,29 +20,6 @@ find_package(tf2_geometry_msgs REQUIRED) 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) -# add_executable(talkerCV src/PolynomialPlanner_node.cpp) -# add_executable(talkerAI src/PolynomialPlanner_node.cpp) -ament_target_dependencies(talker rclcpp std_msgs) -# ament_target_dependencies(talkerCV rclcpp std_msgs) -# ament_target_dependencies(talkerAI 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 - $ - $) -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) @@ -69,10 +46,7 @@ set(dependencies ) # Link ros dependencies -ament_target_dependencies( - polynomial_planner - ${dependencies} -) + # Link ros dependencies ament_target_dependencies( @@ -80,8 +54,6 @@ ament_target_dependencies( ${dependencies} ) -install(TARGETS polynomial_planner - DESTINATION lib/${PROJECT_NAME}) install(TARGETS polynomial_planner_ai DESTINATION lib/${PROJECT_NAME}) @@ -103,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}) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index f4ae2de..aeeb3b4 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -1,17 +1,26 @@ #pragma once +#include #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "image_geometry/pinhole_camera_model.h" + class PolynomialPlannerAi : public rclcpp::Node { private: - rclcpp::Publisher::SharedPtr pub; + rclcpp::Publisher::SharedPtr path_pub; + + rclcpp::Subscription::SharedPtr poly_sub; + + rclcpp::Subscription::SharedPtr rgb_info_sub; - rclcpp::Subscription::SharedPtr sub; + image_geometry::PinholeCameraModel rgb_model; public: PolynomialPlannerAi(const rclcpp::NodeOptions& options); /// subscriber callback - void sub_cb(std_msgs::msg::String::SharedPtr msg); + void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg); }; diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 6fa3e71..2f4a9eb 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -8,6 +8,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" +#include "nav_msgs/msg/path.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" @@ -35,5 +36,5 @@ class PolynomialPlanner : public rclcpp::Node { void sub_cb(std_msgs::msg::String::SharedPtr msg); // camera transform - std::vector cameraPixelToGroundPos(std::vector& pixels); + nav_msgs::msg::Path cameraPixelToGroundPos(std::vector& pixels); }; diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp deleted file mode 100644 index 2c53f3c..0000000 --- a/include/polynomial_planner/backend.hpp +++ /dev/null @@ -1,43 +0,0 @@ -#include -#include -#include -#include -#include -#include - -#include "nav_msgs/msg/path.hpp" - -class Polynomial { -public: - // TODO what does std::pmr::vec mean compared to std::vec ??? - Polynomial(std::pmr::vector& vect) { this->vect = vect; } - Polynomial() = default; - // store the vector -private: - std::pmr::vector 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; - // TODO this pow function might be wrong - 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; - // todo finish? - result += vect[i] * i * pow(x, power); // a[n] * n * x ^ (n - 1) - } - } -}; - -namespace backend { -std::optional create_path(const std::vector& leftPoly, - const std::vector& rightPoly std::string_view frame); -} \ No newline at end of file diff --git a/package.xml b/package.xml index fbd9ed6..2b756ea 100644 --- a/package.xml +++ b/package.xml @@ -20,6 +20,7 @@ nav_msgs geometry_msgs tf2_geometry_msgs + image_geometry cv_bridge diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 3342486..5824d93 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,11 +1,28 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" + // Required for doTransform #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" // For _1 using namespace std::placeholders; -PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) {} +PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { + + this->poly_sub = this->create_subscription("/road/polynomial" , 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); + + this->path_pub = this->create_publisher("/path", 5); + + + + // Copy rgb camera info directly into model + this->rgb_info_sub = this->create_subscription( + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + +} + +void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg) { -void PolynomialPlannerAi::sub_cb(std_msgs::msg::String::SharedPtr msg) {} \ No newline at end of file + RCLCPP_INFO(this->get_logger(), "My log message %d", 4); +} \ No newline at end of file diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index dc933ac..b80970b 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -5,6 +5,7 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" + #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -31,19 +32,24 @@ void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} // 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 PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { +nav_msgs::msg::Path PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { std::vector rwpoints; + for (const cv::Point2d& pixel : pixels) { // gotta rectify the pixel before we raycast cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); - // ask zach for the trig - ray /= ray.z / 0.6; - // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight +// TODO: +// Vector3D intersectPoint(Vector3D rayVector, Vector3D rayPoint, Vector3D planeNormal, Vector3D planePoint) { +// Vector3D diff = rayPoint - planePoint; +// double prod1 = diff.dot(planeNormal); +// double prod2 = rayVector.dot(planeNormal); +// double prod3 = prod1 / prod2; +// return rayPoint - rayVector * prod3; +// } + - cv::Point2d dvector(ray.x, ray.y); - rwpoints.push_back(dvector); } return rwpoints; diff --git a/src/backend.cpp b/src/backend.cpp deleted file mode 100644 index 156572a..0000000 --- a/src/backend.cpp +++ /dev/null @@ -1,67 +0,0 @@ -#include "polynomial_planner/backend.hpp" - -#include - -#include "geometry_msgs/msgs/PoseStamped.hpp" - -std::optional backend::create_path(const std::vector& leftPoly, std::string_view frame) { - // std::string_view is a string lol - std::vector path; // this is the array for cone points - // this will make the - // Start path from kart - path.emplace_back(0, 0); - // take in Polynomial - // ros polynoial take in code... - // transfer to Polynomial class; - float is_right_valid = false; - float is_left_valid = false; - - for (int i = 0; i < /* listenerArry.length*/; i++) { - is_left_valid = (leftPoly == NULL) ? is_left_valid : (leftPoly[i] != 0) ? true : is_left_valid; - } - leftPoly = (is_left_valid) ? new Polynomial(/* vector from ros listener */) : null; - if (is_left_valid) { - Polynomial leftPoly = new Polynomial(/* vector from ros listener */); - } else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; - } - - // interval for polynomial - float max = 200; - float interval = 3; - float start = 5; - float threshold = 15.0; - - // TODO this is lazy and bad fix please - float dist = 0; - for (int i = start; i < max; i += interval) { - dist += sqrt(interval * interval + pow(leftPoly.poly(i) - leftPoly.poly(i + interval), 2)); - - if (dist > threshold) { - path.pushback(cv::Point2d(i, leftPoly.poly(i))); - dist = 0; - } - } - - if (path.empty()) { - return std::nullopt; - } else { - // Convert from cv types to ros - nav_msgs::msg::Path msg{}; - msg.header.frame_id = frame; - - // how do cv types differ from ros types. - // converting to message type in ROS - std::transform(path.begin(), path.end(), std::back_inserter(msg.poses), [&frame](const cv::Point2d& point) { - geometry_msgs::msg::PoseStamped pose{}; - pose.header.frame_id = frame; - pose.pose.position.x = point.x; - pose.pose.position.y = point.y; - - return pose; - }); - - return msg; - } -} diff --git a/src/publisher_member_function.cpp b/src/publisher_member_function.cpp deleted file mode 100644 index 632881b..0000000 --- a/src/publisher_member_function.cpp +++ /dev/null @@ -1,52 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a - * member function as a callback from the timer. */ - -class MinimalPublisher : public rclcpp::Node { -public: - MinimalPublisher() : Node("minimal_publisher"), count_(0) { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer(500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } - -private: - void timer_callback() { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/subscriber_member_function.cpp b/src/subscriber_member_function.cpp deleted file mode 100644 index cd557d5..0000000 --- a/src/subscriber_member_function.cpp +++ /dev/null @@ -1,42 +0,0 @@ -// Copyright 2016 Open Source Robotics Foundation, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using std::placeholders::_1; - -class MinimalSubscriber : public rclcpp::Node { -public: - MinimalSubscriber() : Node("minimal_subscriber") { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } - -private: - void topic_callback(const std_msgs::msg::String& msg) const { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg.data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; -}; - -int main(int argc, char* argv[]) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} From aaa351751dc311b8823cbd0b6a4d487c3463f5e6 Mon Sep 17 00:00:00 2001 From: Jared Date: Tue, 18 Feb 2025 16:47:40 -0500 Subject: [PATCH 11/46] logic changes in backend cpp --- src/backend.cpp | 35 ++++++++++++++--------------------- 1 file changed, 14 insertions(+), 21 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 2b87c5b..df766a7 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,10 +4,10 @@ #include "geometry_msgs/msgs/PoseStamped.hpp" -std::optional backend::create_path(const std::vector& leftPoly, std::string_view frame) { +std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { // std::string_view is a string lol - std::vector path; // this is the array for cone points - // this will make the + std::vector path; // this is the vector of path plannign points + // Start path from kart path.emplace_back(0, 0); // take in Polynomial @@ -16,32 +16,25 @@ std::optional backend::create_path(const std::vector& leftP float is_right_valid = false; float is_left_valid = false; - for (int i = 0; i < leftPoly.size(); i++) { - is_left_valid = (leftPoly == NULL) ? is_left_valid : (leftPoly[i] != 0) ? true : is_left_valid; + for (int i = 0; i < leftPolyVector.size(); i++) { + is_left_valid = (leftPolyVector == NULL) ? is_left_valid : (leftPolyVector[i] != 0) ? true : is_left_valid; } - leftPoly = (is_left_valid) ? new Polynomial(leftPoly) : null; + leftPoly = (is_left_valid) ? new Polynomial(leftPolyVector) : null; // interval for polynomial - float max = 200; - float interval = 3; - float start = 5; - float threshold = 15.0; + float max = 280; // artificial event horizon + float interval = 3; // stepping x value up by 3camera px on each iteration + float start = 475; // bottom of frame + float threshold = 15.0; // min dist between points // TODO this is lazy and bad fix please float dist = 0; - for (int i = start; i < max; i += interval) { - dist += sqrt(interval * interval + pow(leftPoly.poly(i) - leftPoly.poly(i + interval), 2)); + for (int x = start; x > max; x -= interval) { + dist += sqrt(interval * interval + pow(leftPoly.poly(x) - leftPoly.poly(x + interval), 2)); if (dist > threshold) { - path.pushback(cv::Point2d(i, leftPoly.poly(i))); - // TODO was disif probably a typo thb - if (is_left_valid) { - Polynomial leftPoly = new Polynomial(leftPoly); - } else { - // TODO this is lazy and bad fix please - Polynomial leftPoly = null; - } - t = 0; + path.pushback(cv::Point2d(x, leftPoly.poly(x))); + dist = 0; } } From f1930ad065f2f53a00f825b7c9c1128dcb2855b1 Mon Sep 17 00:00:00 2001 From: Jared Date: Tue, 18 Feb 2025 17:05:44 -0500 Subject: [PATCH 12/46] logic changes in backend cpp --- src/backend.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/backend.cpp b/src/backend.cpp index df766a7..8676c23 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -2,6 +2,7 @@ #include + #include "geometry_msgs/msgs/PoseStamped.hpp" std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { From 33603549fe850b0c400446c0746cc98c861d9790 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 19 Feb 2025 14:34:58 -0500 Subject: [PATCH 13/46] added depth, worked on pub/sub --- .../PolynomialPlannerAi_node.hpp | 8 +-- .../PolynomialPlanner_node.hpp | 3 +- src/PolynomialPlannerAi_node.cpp | 13 ++-- src/PolynomialPlanner_node.cpp | 68 +++++++++++++++---- 4 files changed, 63 insertions(+), 29 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index aeeb3b4..2de0e7b 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -1,12 +1,12 @@ #pragma once #include + +#include "image_geometry/pinhole_camera_model.h" #include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -#include "std_msgs/msg/float32_multi_array.hpp" #include "sensor_msgs/msg/camera_info.hpp" -#include "image_geometry/pinhole_camera_model.h" - +#include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/string.hpp" class PolynomialPlannerAi : public rclcpp::Node { private: diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 2f4a9eb..89e1aa4 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -8,13 +8,12 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" -#include "nav_msgs/msg/path.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" class PolynomialPlanner : public rclcpp::Node { private: - rclcpp::Publisher::SharedPtr pub; +rclcpp::Publisher::SharedPtr path_pub; rclcpp::Subscription::SharedPtr sub; diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 5824d93..781b61a 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,6 +1,5 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" - // Required for doTransform #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -8,21 +7,17 @@ using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { - - this->poly_sub = this->create_subscription("/road/polynomial" , 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); + this->poly_sub = this->create_subscription( + "/road/polynomial", 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); this->path_pub = this->create_publisher("/path", 5); - - // Copy rgb camera info directly into model this->rgb_info_sub = this->create_subscription( - "/camera/mid/rgb/camera_info", 1, - [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); - + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "My log message %d", 4); } \ No newline at end of file diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index b80970b..72fe804 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -5,7 +5,6 @@ #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" - #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" @@ -16,21 +15,21 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( // delcare params auto random_int = this->declare_parameter("random_int", 0); - pub = this->create_publisher("topic", 10); + this->path_pub = this->create_publisher("/path", 5); this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); // TF2 things - // this->tf2_buffer = std::make_unique(this->get_clock()); - // this->tf2_listener = std::make_unique(*this->tf2_buffer); + this->tf2_buffer = std::make_unique(this->get_clock()); + this->tf2_listener = std::make_unique(*this->tf2_buffer); } void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} // why are the pointer things the way they are -// TODO: make it not die when z is too small +// TODO: make it not die when z is too smallf // or make z not too small nav_msgs::msg::Path PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { std::vector rwpoints; @@ -40,17 +39,58 @@ nav_msgs::msg::Path PolynomialPlanner::cameraPixelToGroundPos(std::vectorrgb_model.rectifyPoint(pixel); cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); -// TODO: -// Vector3D intersectPoint(Vector3D rayVector, Vector3D rayPoint, Vector3D planeNormal, Vector3D planePoint) { -// Vector3D diff = rayPoint - planePoint; -// double prod1 = diff.dot(planeNormal); -// double prod2 = rayVector.dot(planeNormal); -// double prod3 = prod1 / prod2; -// return rayPoint - rayVector * prod3; -// } + // TODO: + // Vector3D intersectPoint(Vector3D rayVector, Vector3D rayPoint, Vector3D planeNormal, Vector3D planePoint) { + // Vector3D diff = rayPoint - planePoint; + // double prod1 = diff.dot(planeNormal); + // double prod2 = rayVector.dot(planeNormal); + // double prod3 = prod1 / prod2; + // return rayPoint - rayVector * prod3; + // } + } + + return rwpoints; +} + +geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vector& 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(center) / 1000 : depth.at(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); } - return rwpoints; + poses.header.stamp = this->get_clock()->now(); + return poses; } \ No newline at end of file From 844eb22634f2b7f6b9675950b99231a961a11592 Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 19 Feb 2025 14:55:38 -0500 Subject: [PATCH 14/46] format for derek --- .../PolynomialPlanner_node.hpp | 2 +- src/PolynomialPlanner_node.cpp | 18 +++++++++--------- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 89e1aa4..b4d4d93 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -13,7 +13,7 @@ class PolynomialPlanner : public rclcpp::Node { private: -rclcpp::Publisher::SharedPtr path_pub; + rclcpp::Publisher::SharedPtr path_pub; rclcpp::Subscription::SharedPtr sub; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 72fe804..306ffd3 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -39,21 +39,21 @@ nav_msgs::msg::Path PolynomialPlanner::cameraPixelToGroundPos(std::vectorrgb_model.rectifyPoint(pixel); cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); - // TODO: - // Vector3D intersectPoint(Vector3D rayVector, Vector3D rayPoint, Vector3D planeNormal, Vector3D planePoint) { - // Vector3D diff = rayPoint - planePoint; - // double prod1 = diff.dot(planeNormal); - // double prod2 = rayVector.dot(planeNormal); - // double prod3 = prod1 / prod2; - // return rayPoint - rayVector * prod3; - // } + // TODO: TEST + Vector3D intersectPoint(Vector3D rayVector, Vector3D rayPoint, Vector3D planeNormal, Vector3D planePoint) { + Vector3D diff = rayPoint - planePoint; + double prod1 = diff.dot(planeNormal); + double prod2 = rayVector.dot(planeNormal); + double prod3 = prod1 / prod2; + return rayPoint - rayVector * prod3; + } } return rwpoints; } geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vector& object_locations, - const cv::Mat& depth) { + const cv::Mat& depth) { geometry_msgs::msg::PoseArray poses{}; poses.header.frame_id = this->get_parameter(std::string{"camera_frame"}).as_string(); From 78ebb86963c045e1f223b632dc697c606fd3e03f Mon Sep 17 00:00:00 2001 From: Jared Date: Wed, 19 Feb 2025 15:30:05 -0500 Subject: [PATCH 15/46] plannerAI structure commit --- .../PolynomialPlannerAi_node.hpp | 1 + src/PolynomialPlannerAi_node.cpp | 40 +++++++++++++++---- 2 files changed, 34 insertions(+), 7 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index 2de0e7b..acaa00d 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -23,4 +23,5 @@ class PolynomialPlannerAi : public rclcpp::Node { /// subscriber callback void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg); + void evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values); }; diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 781b61a..3a7ae12 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,23 +1,49 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" + // Required for doTransform #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -// For _1 + +#include +#include + using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { this->poly_sub = this->create_subscription( - "/road/polynomial", 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); + "/road/polynomial", 1, + std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); this->path_pub = this->create_publisher("/path", 5); - // Copy rgb camera info directly into model this->rgb_info_sub = this->create_subscription( - "/camera/mid/rgb/camera_info", 1, - [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + + RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data..."); } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "My log message %d", 4); -} \ No newline at end of file + if (msg->data.empty()) { + RCLCPP_WARN(this->get_logger(), "Received an empty polynomial array!"); + return; + } + + // 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]); + } +} + + +void PolynomialPlannerAi::evaluate_polynomial(const std::vector& coeffs, const std::vector& 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); + } +} From 06bfedcf2ea191229942c7df2c5535e05c642a6f Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 19 Feb 2025 21:58:41 -0500 Subject: [PATCH 16/46] added rotation --- src/PolynomialPlanner_node.cpp | 5 +++++ src/backend.cpp | 9 ++++----- 2 files changed, 9 insertions(+), 5 deletions(-) diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index dc933ac..b025a7a 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -43,6 +43,11 @@ std::vector PolynomialPlanner::cameraPixelToGroundPos(std::vector - #include "geometry_msgs/msgs/PoseStamped.hpp" std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { @@ -23,10 +22,10 @@ std::optional backend::create_path(const std::vector& leftP leftPoly = (is_left_valid) ? new Polynomial(leftPolyVector) : null; // interval for polynomial - float max = 280; // artificial event horizon - float interval = 3; // stepping x value up by 3camera px on each iteration - float start = 475; // bottom of frame - float threshold = 15.0; // min dist between points + float max = 280; // artificial event horizon + float interval = 3; // stepping x value up by 3camera px on each iteration + float start = 475; // bottom of frame + float threshold = 15.0; // min dist between points // TODO this is lazy and bad fix please float dist = 0; From e2adb28da1af29edfef805f211b4824585f01dab Mon Sep 17 00:00:00 2001 From: redto0 Date: Wed, 19 Feb 2025 22:34:07 -0500 Subject: [PATCH 17/46] tf transform copy paste TODO FIX --- src/PolynomialPlanner_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index b025a7a..e1b9a4a 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -7,6 +7,7 @@ #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include // For _1 using namespace std::placeholders; @@ -48,6 +49,8 @@ std::vector PolynomialPlanner::cameraPixelToGroundPos(std::vector Date: Thu, 20 Feb 2025 14:50:48 -0500 Subject: [PATCH 18/46] make derek happy with code formats --- src/PolynomialPlannerAi_node.cpp | 16 ++++++---------- 1 file changed, 6 insertions(+), 10 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 3a7ae12..f577db4 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,25 +1,22 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" - // Required for doTransform -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - - -#include #include +#include + +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { this->poly_sub = this->create_subscription( - "/road/polynomial", 1, - std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); + "/road/polynomial", 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); this->path_pub = this->create_publisher("/path", 5); this->rgb_info_sub = this->create_subscription( - "/camera/mid/rgb/camera_info", 1, - [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + "/camera/mid/rgb/camera_info", 1, + [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data..."); } @@ -37,7 +34,6 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared } } - void PolynomialPlannerAi::evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values) { for (float x : x_values) { float y = 0.0; From ec0521c06045a5cf30916ede13d1c462cd0c1485 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 20 Feb 2025 17:05:07 -0500 Subject: [PATCH 19/46] solved inverstion projection, 100% real this time guys --- src/PolynomialPlanner_node.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index e1b9a4a..23c7398 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -33,22 +33,31 @@ void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} // TODO: make it not die when z is too small // or make z not too small std::vector PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { + + // 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(-M_PI / 2, 0.0, -M_PI / 2); + std::vector rwpoints; for (const cv::Point2d& pixel : pixels) { // gotta rectify the pixel before we raycast cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); + - // ask zach for the trig - ray /= ray.z / 0.6; + // ask zach for the trig, extend ray to the floor. + ray /= ray.z / -0.6; // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight + tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; + tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); - cv::Point2d dvector(ray.x, ray.y); + //return type world_vec, use this is + + cv::Point2d dvector(world_vec.x, world_vec.y); + - // 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); // push back vectors rwpoints.push_back(dvector); From a63709b1cade445d8f922665f7fc00176ac02745 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Thu, 20 Feb 2025 18:26:23 -0500 Subject: [PATCH 20/46] added pub, moved projection to backend --- src/PolynomialPlanner_node.cpp | 59 ++++++++++++++----------------- src/backend.cpp | 63 +++++++++++++++++++++++++++------- 2 files changed, 75 insertions(+), 47 deletions(-) diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index b0654bd..5e1ce16 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -1,13 +1,15 @@ #include "polynomial_planner/PolynomialPlanner_node.hpp" // Required for doTransform +#include + #include +#include "geometry_msgs/msgs/PoseStamped.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include // For _1 using namespace std::placeholders; @@ -27,46 +29,34 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( this->tf2_listener = std::make_unique(*this->tf2_buffer); } -void PolynomialPlanner::sub_cb(std_msgs::msg::String::SharedPtr msg) {} - -// why are the pointer things the way they are -// TODO: make it not die when z is too smallf -// or make z not too small -std::vector PolynomialPlanner::cameraPixelToGroundPos(std::vector& pixels) { - - // 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(-M_PI / 2, 0.0, -M_PI / 2); +void PolynomialPlanner::polynomial_cb(std_msgs::msg::String::SharedPtr msg) { + if (msg->empty()) { + RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); + return; - std::vector rwpoints; + } else { + std::vector coeff{}; + int no_coeff = msg->data.size(); - for (const cv::Point2d& pixel : pixels) { - // gotta rectify the pixel before we raycast - cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); - cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); - - - // ask zach for the trig, extend ray to the floor. - ray /= ray.z / -0.6; - // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight - tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; - tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); + for (int i = 0; i < no_coeff; i++) { + coeff.push_back(msg->data[i]); + } - //return type world_vec, use this is + nav_msgs::msg::Path path = backend::create_path(coeff, frame); - cv::Point2d dvector(world_vec.x, world_vec.y); - - + geometry_msgs::msg::PoseStamped path_poses[] = path.poses; - // push back vectors - rwpoints.push_back(dvector); + bool do_logger = true; + if (do_logger) { + for (int i = 0; i < path_poses.size(); i++) { + RCLCPP_INFO(this->get_logger(), "") + } + } + this->path_pub->publish(*path); } - - return rwpoints; } +/* not so obvious reference: geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vector& object_locations, const cv::Mat& depth) { geometry_msgs::msg::PoseArray poses{}; @@ -108,4 +98,5 @@ geometry_msgs::msg::PoseArray PolynomialPlanner::project_to_world(const std::vec poses.header.stamp = this->get_clock()->now(); return poses; -} \ No newline at end of file +} + */ \ No newline at end of file diff --git a/src/backend.cpp b/src/backend.cpp index 82da6f0..399e6b2 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -6,10 +6,8 @@ std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { // std::string_view is a string lol - std::vector path; // this is the vector of path plannign points + std::vector cam_path; // this is the vector of path plannign points - // Start path from kart - path.emplace_back(0, 0); // take in Polynomial // ros polynoial take in code... // transfer to Polynomial class; @@ -21,41 +19,80 @@ std::optional backend::create_path(const std::vector& leftP } leftPoly = (is_left_valid) ? new Polynomial(leftPolyVector) : null; + if (is_left_valid != true) return std::nullopt; + // interval for polynomial float max = 280; // artificial event horizon float interval = 3; // stepping x value up by 3camera px on each iteration float start = 475; // bottom of frame float threshold = 15.0; // min dist between points - // TODO this is lazy and bad fix please float dist = 0; for (int x = start; x > max; x -= interval) { dist += sqrt(interval * interval + pow(leftPoly.poly(x) - leftPoly.poly(x + interval), 2)); if (dist > threshold) { - path.pushback(cv::Point2d(x, leftPoly.poly(x))); + cam_path.pushback(cv::Point2d(x, leftPoly.poly(x))); dist = 0; } } - if (path.empty()) { + if (cam_path.empty()) { return std::nullopt; } else { // Convert from cv types to ros + + std::vector ground_path = cameraPixelToGroundPos(cam_path); + nav_msgs::msg::Path msg{}; msg.header.frame_id = frame; // how do cv types differ from ros types. // converting to message type in ROS - std::transform(path.begin(), path.end(), std::back_inserter(msg.poses), [&frame](const cv::Point2d& point) { - geometry_msgs::msg::PoseStamped pose{}; - pose.header.frame_id = frame; - pose.pose.position.x = point.x; - pose.pose.position.y = point.y; + std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), + [&frame](const cv::Point2d& point) { + geometry_msgs::msg::PoseStamped pose{}; + pose.header.frame_id = frame; + pose.pose.position.x = point.x; + pose.pose.position.y = point.y; - return pose; - }); + return pose; + }); return msg; } } + +// why are the pointer things the way they are +// TODO: make it not die when z is too smallf +// or make z not too small +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, std::string_view frame) { + // 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(-M_PI / 2, 0.0, -M_PI / 2); + + std::vector rwpoints; + + for (const cv::Point2d& pixel : pixels) { + // gotta rectify the pixel before we raycast + cv::Point2d rectPixel = this->rgb_model.rectifyPoint(pixel); + cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); + + // ask zach for the trig, extend ray to the floor. + ray /= ray.z / -0.6; + // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight + tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; + tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); + + //return type world_vec, use this is + + cv::Point2d dvector(world_vec.x, world_vec.y); + + // push back vectors + rwpoints.push_back(dvector); + } + + return rwpoints; +} \ No newline at end of file From e16b589255b8edff75a40a1fc4647a6e1b5217ce Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Thu, 20 Feb 2025 18:29:31 -0500 Subject: [PATCH 21/46] bug fix --- src/PolynomialPlanner_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 5e1ce16..5f288ac 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -49,7 +49,7 @@ void PolynomialPlanner::polynomial_cb(std_msgs::msg::String::SharedPtr msg) { bool do_logger = true; if (do_logger) { for (int i = 0; i < path_poses.size(); i++) { - RCLCPP_INFO(this->get_logger(), "") + RCLCPP_INFO(this->get_logger(), ""); } } this->path_pub->publish(*path); From 4168546c12d7cadca71864a2b1e7fa07e0542794 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Mon, 24 Feb 2025 18:50:05 -0500 Subject: [PATCH 22/46] bringing AI planner up to CV's speed --- .../PolynomialPlannerAi_node.hpp | 16 +++++++-- .../PolynomialPlanner_node.hpp | 2 +- src/PolynomialPlannerAi_node.cpp | 35 ++++++++++++++++--- 3 files changed, 45 insertions(+), 8 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index acaa00d..21077ae 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -1,12 +1,15 @@ #pragma once -#include +#include +#include #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_ros/buffer.h" +#include "tf2_ros/transform_listener.h" class PolynomialPlannerAi : public rclcpp::Node { private: @@ -14,10 +17,17 @@ class PolynomialPlannerAi : public rclcpp::Node { rclcpp::Subscription::SharedPtr poly_sub; + // Camera info sub & model vars rclcpp::Subscription::SharedPtr rgb_info_sub; - image_geometry::PinholeCameraModel rgb_model; + // std::optional + std::unique_ptr> Backend; + + // TF2 stuff + std::unique_ptr tf2_listener; + std::unique_ptr tf2_buffer; + public: PolynomialPlannerAi(const rclcpp::NodeOptions& options); diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index b4d4d93..1af9fd0 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -15,7 +15,7 @@ class PolynomialPlanner : public rclcpp::Node { private: rclcpp::Publisher::SharedPtr path_pub; - rclcpp::Subscription::SharedPtr sub; + rclcpp::Subscription::SharedPtr sub; // Camera info sub & model vars rclcpp::Subscription::SharedPtr rgb_info_sub; diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index f577db4..9d17e53 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,11 +1,15 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" // Required for doTransform -#include +#include + #include +#include "geometry_msgs/msgs/PoseStamped.hpp" +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "std_msgs/msg/string.hpp" #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" - using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { @@ -19,12 +23,35 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data..."); + // TF2 things + this->tf2_buffer = std::make_unique(this->get_clock()); + this->tf2_listener = std::make_unique(*this->tf2_buffer); } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg) { - if (msg->data.empty()) { - RCLCPP_WARN(this->get_logger(), "Received an empty polynomial array!"); + if (msg->empty()) { + RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); return; + + } else { + std::vector coeff{}; + int no_coeff = msg->data.size(); + + for (int i = 0; i < no_coeff; i++) { + coeff.push_back(msg->data[i]); + } + + nav_msgs::msg::Path path = backend::create_path(coeff, frame); + + geometry_msgs::msg::PoseStamped path_poses[] = path.poses; + + bool do_logger = true; + if (do_logger) { + for (int i = 0; i < path_poses.size(); i++) { + // RCLCPP_INFO(this->get_logger(), ""); + } + } + this->path_pub->publish(*path); } // Extract and print coefficients From 93dc1e8fc05f5d2b32f91687cee857616bd958ea Mon Sep 17 00:00:00 2001 From: Jared Date: Sat, 1 Mar 2025 11:32:41 -0500 Subject: [PATCH 23/46] Updated geometry include message to work --- src/PolynomialPlannerAi_node.cpp | 2 +- src/PolynomialPlanner_node.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 9d17e53..4a83d7b 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -5,7 +5,7 @@ #include -#include "geometry_msgs/msgs/PoseStamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 5f288ac..d303883 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -5,7 +5,7 @@ #include -#include "geometry_msgs/msgs/PoseStamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/camera_info.hpp" #include "std_msgs/msg/string.hpp" From 5d4aa084b3c33cf3588d0333810068267afe514d Mon Sep 17 00:00:00 2001 From: Jared Date: Sat, 1 Mar 2025 11:44:58 -0500 Subject: [PATCH 24/46] Updated #includes --- include/polynomial_planner/PolynomialPlannerAi_node.hpp | 3 +++ src/PolynomialPlannerAi_node.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index 21077ae..63f7a60 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -10,6 +10,9 @@ #include "std_msgs/msg/string.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "std_msgs/msg/float32_multi_array.hpp" + class PolynomialPlannerAi : public rclcpp::Node { private: diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 4a83d7b..d326243 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -6,10 +6,10 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/camera_info.hpp" -#include "std_msgs/msg/string.hpp" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" + + + + using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { From 1b34a719fedc5e3dd1b8bdd225f9b1001939f616 Mon Sep 17 00:00:00 2001 From: Jared Date: Sat, 1 Mar 2025 12:25:02 -0500 Subject: [PATCH 25/46] Debugging checkpoint, #includes work, figure out new errors --- .../PolynomialPlanner_node.hpp | 78 +++++++++---------- include/polynomial_planner/backend.hpp | 46 +++++++++++ src/PolynomialPlannerAi_node.cpp | 1 + src/backend.cpp | 2 +- 4 files changed, 87 insertions(+), 40 deletions(-) create mode 100644 include/polynomial_planner/backend.hpp diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 1af9fd0..7ece804 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,39 +1,39 @@ -#pragma once - -#include -#include - -#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::SharedPtr path_pub; - - rclcpp::Subscription::SharedPtr sub; - - // Camera info sub & model vars - rclcpp::Subscription::SharedPtr rgb_info_sub; - image_geometry::PinholeCameraModel rgb_model; - - // std::optional - std::unique_ptr> Backend; - - // TF2 stuff - std::unique_ptr tf2_listener; - std::unique_ptr 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& pixels); -}; +//#pragma once +// +//#include +//#include +// +//#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::SharedPtr path_pub; +// +// rclcpp::Subscription::SharedPtr sub; +// +// // Camera info sub & model vars +// rclcpp::Subscription::SharedPtr rgb_info_sub; +// image_geometry::PinholeCameraModel rgb_model; +// +// // std::optional +// std::unique_ptr> Backend; +// +// // TF2 stuff +// std::unique_ptr tf2_listener; +// std::unique_ptr 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& pixels); +//}; diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp new file mode 100644 index 0000000..245d0b2 --- /dev/null +++ b/include/polynomial_planner/backend.hpp @@ -0,0 +1,46 @@ +// +// Created by jaredwensley on 3/1/25. +// +#include +#include +#include +#include +#include +#include + +#include "nav_msgs/msg/path.hpp" + +class Polynomial { +public: + // TODO what does std::pmr::vec mean compared to std::vec ??? + Polynomial(std::pmr::vector& vect) { this->vect = vect; } + Polynomial() = default; + // store the vector +private: + std::pmr::vector 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; + // TODO this pow function might be wrong + 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; + // todo finish? + result += vect[i] * i * pow(x, power); // a[n] * n * x ^ (n - 1) + } + } +}; + +namespace backend { + std::optional create_path(const std::vector& leftPoly, + const std::vector& rightPoly std::string_view frame); +} \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index d326243..d3b26e7 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,4 +1,5 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" +#include "backend.cpp" // Required for doTransform #include diff --git a/src/backend.cpp b/src/backend.cpp index 399e6b2..6f0b493 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -2,7 +2,7 @@ #include -#include "geometry_msgs/msgs/PoseStamped.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { // std::string_view is a string lol From 17868559a5d1c1f5002393894a333964574a686e Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 1 Mar 2025 19:16:26 -0500 Subject: [PATCH 26/46] minor fixes, reverted the pmr::vect Signed-off-by: redto0 --- include/polynomial_planner/backend.hpp | 8 +++----- src/PolynomialPlannerAi_node.cpp | 3 ++- src/backend.cpp | 14 ++++++++++---- 3 files changed, 15 insertions(+), 10 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 245d0b2..6f46ba2 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -1,6 +1,3 @@ -// -// Created by jaredwensley on 3/1/25. -// #include #include #include @@ -13,11 +10,11 @@ class Polynomial { public: // TODO what does std::pmr::vec mean compared to std::vec ??? - Polynomial(std::pmr::vector& vect) { this->vect = vect; } + Polynomial(std::vector& vect) { this->vect = vect; } Polynomial() = default; // store the vector private: - std::pmr::vector vect; + std::vector vect; public: // returns the y value at a given X. @@ -26,6 +23,7 @@ class Polynomial { for (int i = 0; i < vect.size(); i++) { int power = vect.size() - i; // 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; diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index d3b26e7..6ae6ab3 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -48,11 +48,12 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared bool do_logger = true; if (do_logger) { + // TODO change path_poses.size() "size does not exist" for (int i = 0; i < path_poses.size(); i++) { // RCLCPP_INFO(this->get_logger(), ""); } } - this->path_pub->publish(*path); + this->path_pub->publish(*path); // error invalid operator *path } // Extract and print coefficients diff --git a/src/backend.cpp b/src/backend.cpp index 6f0b493..55c0589 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -40,13 +40,15 @@ std::optional backend::create_path(const std::vector& leftP if (cam_path.empty()) { return std::nullopt; } else { - // Convert from cv types to ros + // Convert from cv types to nav::msg std::vector ground_path = cameraPixelToGroundPos(cam_path); nav_msgs::msg::Path msg{}; - msg.header.frame_id = frame; - + // msg.header.frame_id = frame; + for (cv::Point2d ground_points : ground_path){ + std:: + } // how do cv types differ from ros types. // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), @@ -81,7 +83,11 @@ std::vector backend::cameraPixelToGroundPos(std::vectorrgb_model.projectPixelTo3dRay(rectPixel); // ask zach for the trig, extend ray to the floor. - ray /= ray.z / -0.6; + float divisor = ray.z / -0.6; + ray.x = ray.x / divisor; + ray.y = ray.y / divisor; + ray.z = ray.z / divisor; + // ray /= ray.z / -0.6; // ray.setZ(ray.getZ() * -1); we don't really care abt z, since it -will- *should* always just be cameraHeight tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); From 2c4856b1dc83d750ca6daf5b2575d12dad1af828 Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 3 Mar 2025 17:47:37 -0500 Subject: [PATCH 27/46] WIP jared sync --- .../PolynomialPlannerAi_node.hpp | 8 ++--- include/polynomial_planner/backend.hpp | 9 +++-- src/PolynomialPlannerAi_node.cpp | 22 ++++++------- src/backend.cpp | 33 +++++++++---------- 4 files changed, 36 insertions(+), 36 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index 63f7a60..1cd5612 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -7,12 +7,11 @@ #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" -#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" -#include "std_msgs/msg/float32_multi_array.hpp" - class PolynomialPlannerAi : public rclcpp::Node { private: @@ -35,6 +34,7 @@ class PolynomialPlannerAi : public rclcpp::Node { PolynomialPlannerAi(const rclcpp::NodeOptions& options); /// subscriber callback - void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg); + void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, + sensor_msgs::msg::CameraInfo camera_rgb); void evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values); }; diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 6f46ba2..f930e83 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,6 +6,7 @@ #include #include "nav_msgs/msg/path.hpp" +#include "sensor_msgs/msg/camera_info.hpp" class Polynomial { public: @@ -39,6 +40,8 @@ class Polynomial { }; namespace backend { - std::optional create_path(const std::vector& leftPoly, - const std::vector& rightPoly std::string_view frame); -} \ No newline at end of file +std::optional create_path(std::vector& leftPoly, sensor_msgs::msg::CameraInfo rgb_info_sub); + +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, + sensor_msgs::msg::CameraInfo& rgb_info_sub); +} // namespace backend \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 6ae6ab3..73ded99 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,4 +1,5 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" + #include "backend.cpp" // Required for doTransform @@ -8,28 +9,27 @@ #include "geometry_msgs/msg/pose_stamped.hpp" - - - using namespace std::placeholders; PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : Node("polynomial_planner_ai", options) { - this->poly_sub = this->create_subscription( - "/road/polynomial", 1, std::bind(&PolynomialPlannerAi::polynomial_cb, this, _1)); - - this->path_pub = this->create_publisher("/path", 5); - this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); + this->poly_sub = this->create_subscription( + "/road/polynomial", 1, + [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) { this->polynomial_cb(msg, rgb_info_sub); }); + + this->path_pub = this->create_publisher("/path", 5); + RCLCPP_INFO(this->get_logger(), "PolynomialPlannerAi Node Started! Waiting for polynomial data..."); // TF2 things this->tf2_buffer = std::make_unique(this->get_clock()); this->tf2_listener = std::make_unique(*this->tf2_buffer); } -void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg) { +void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, + sensor_msgs::msg::CameraInfo camera_rgb) { if (msg->empty()) { RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); return; @@ -42,7 +42,7 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared coeff.push_back(msg->data[i]); } - nav_msgs::msg::Path path = backend::create_path(coeff, frame); + nav_msgs::msg::Path path = backend::create_path(coeff, camera_rgb->data); geometry_msgs::msg::PoseStamped path_poses[] = path.poses; @@ -53,7 +53,7 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared // RCLCPP_INFO(this->get_logger(), ""); } } - this->path_pub->publish(*path); // error invalid operator *path + this->path_pub->publish(path); // error invalid operator *path } // Extract and print coefficients diff --git a/src/backend.cpp b/src/backend.cpp index 55c0589..89066a5 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,7 +4,8 @@ #include "geometry_msgs/msg/pose_stamped.hpp" -std::optional backend::create_path(const std::vector& leftPolyVector, std::string_view frame) { +std::optional backend::create_path(std::vector& leftPolyVector, + sensor_msgs::msg::CameraInfo& rgb_info_sub) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -14,12 +15,7 @@ std::optional backend::create_path(const std::vector& leftP float is_right_valid = false; float is_left_valid = false; - for (int i = 0; i < leftPolyVector.size(); i++) { - is_left_valid = (leftPolyVector == NULL) ? is_left_valid : (leftPolyVector[i] != 0) ? true : is_left_valid; - } - leftPoly = (is_left_valid) ? new Polynomial(leftPolyVector) : null; - - if (is_left_valid != true) return std::nullopt; + auto leftPoly = new Polynomial(leftPolyVector); // interval for polynomial float max = 280; // artificial event horizon @@ -29,10 +25,10 @@ std::optional backend::create_path(const std::vector& leftP float dist = 0; for (int x = start; x > max; x -= interval) { - dist += sqrt(interval * interval + pow(leftPoly.poly(x) - leftPoly.poly(x + interval), 2)); + dist += sqrt(interval * interval + pow(leftPoly->poly(x) - leftPoly->poly(x + interval), 2)); if (dist > threshold) { - cam_path.pushback(cv::Point2d(x, leftPoly.poly(x))); + cam_path.push_back(cv::Point2d(x, leftPoly->poly(x))); dist = 0; } } @@ -42,19 +38,19 @@ std::optional backend::create_path(const std::vector& leftP } else { // Convert from cv types to nav::msg - std::vector ground_path = cameraPixelToGroundPos(cam_path); + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; - for (cv::Point2d ground_points : ground_path){ - std:: + for (cv::Point2d ground_points : ground_path) { + // std:: } // how do cv types differ from ros types. // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), - [&frame](const cv::Point2d& point) { + [](const cv::Point2d& point) { geometry_msgs::msg::PoseStamped pose{}; - pose.header.frame_id = frame; + // pose.header.frame_id = frame; pose.pose.position.x = point.x; pose.pose.position.y = point.y; @@ -68,7 +64,8 @@ std::optional backend::create_path(const std::vector& leftP // why are the pointer things the way they are // TODO: make it not die when z is too smallf // or make z not too small -std::vector backend::cameraPixelToGroundPos(std::vector& pixels, std::string_view frame) { +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, + sensor_msgs::msg::CameraInfo rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates tf2::Quaternion optical_to_ros{}; @@ -79,8 +76,8 @@ std::vector backend::cameraPixelToGroundPos(std::vectorrgb_model.rectifyPoint(pixel); - cv::Point3d ray = this->rgb_model.projectPixelTo3dRay(rectPixel); + cv::Point2d rectPixel = rgb_info_sub->rgb_model.rectifyPoint(pixel); + cv::Point3d ray = rgb_info_sub->rgb_model.projectPixelTo3dRay(rectPixel); // ask zach for the trig, extend ray to the floor. float divisor = ray.z / -0.6; @@ -101,4 +98,4 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Wed, 5 Mar 2025 22:50:40 -0500 Subject: [PATCH 28/46] wip day 2 --- .../PolynomialPlannerAi_node.hpp | 5 ++- include/polynomial_planner/backend.hpp | 9 +++-- src/PolynomialPlannerAi_node.cpp | 38 +++++++++++-------- src/PolynomialPlanner_node.cpp | 7 ++-- src/backend.cpp | 19 +++++----- 5 files changed, 44 insertions(+), 34 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index 1cd5612..6de3afa 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -1,6 +1,7 @@ #pragma once #include +#include #include #include "image_geometry/pinhole_camera_model.h" @@ -34,7 +35,7 @@ class PolynomialPlannerAi : public rclcpp::Node { PolynomialPlannerAi(const rclcpp::NodeOptions& options); /// subscriber callback - void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, - sensor_msgs::msg::CameraInfo camera_rgb); + /// 'PolynomialPlannerAi::' is unnesscary + void polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, image_geometry::PinholeCameraModel camera_rgb); void evaluate_polynomial(const std::vector& coeffs, const std::vector& x_values); }; diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index f930e83..6e5438c 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,7 +6,7 @@ #include #include "nav_msgs/msg/path.hpp" -#include "sensor_msgs/msg/camera_info.hpp" +#include class Polynomial { public: @@ -40,8 +40,9 @@ class Polynomial { }; namespace backend { -std::optional create_path(std::vector& leftPoly, sensor_msgs::msg::CameraInfo rgb_info_sub); -std::vector backend::cameraPixelToGroundPos(std::vector& pixels, - sensor_msgs::msg::CameraInfo& rgb_info_sub); +std::vector cameraPixelToGroundPos(std::vector& pixels, + sensor_msgs::msg::CameraInfo& rgb_info_sub, std::string frame); +std::optional create_path(std::vector& leftPoly, image_geometry::PinholeCameraModel rgb_info_sub, + std::string frame); } // namespace backend \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 73ded99..4c1f471 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -1,5 +1,7 @@ #include "polynomial_planner/PolynomialPlannerAi_node.hpp" +#include + #include "backend.cpp" // Required for doTransform @@ -12,13 +14,17 @@ 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( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); this->poly_sub = this->create_subscription( - "/road/polynomial", 1, - [this](const std_msgs::msg::Float32MultiArray::SharedPtr msg) { this->polynomial_cb(msg, rgb_info_sub); }); + "/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("/path", 5); @@ -29,8 +35,9 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, - sensor_msgs::msg::CameraInfo camera_rgb) { - if (msg->empty()) { + image_geometry::PinholeCameraModel camera_rgb) { + // fix msg->empty + if (false){ RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); return; @@ -42,24 +49,23 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared coeff.push_back(msg->data[i]); } - nav_msgs::msg::Path path = backend::create_path(coeff, camera_rgb->data); + std::string frame_id = this->get_parameter("camera_frame").as_string(); - geometry_msgs::msg::PoseStamped path_poses[] = path.poses; + std::optional path_optional = backend::create_path(coeff, camera_rgb, frame_id); + nav_msgs::msg::Path path; + + if (path_optional.has_value()) { + path = path_optional.value(); + } else + return; - bool do_logger = true; - if (do_logger) { - // TODO change path_poses.size() "size does not exist" - for (int i = 0; i < path_poses.size(); i++) { - // RCLCPP_INFO(this->get_logger(), ""); - } - } this->path_pub->publish(path); // error invalid operator *path } // Extract and print coefficients - RCLCPP_INFO(this->get_logger(), "Received Polynomial 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]); + // RCLCPP_INFO(this->get_logger(), "Coefficient[%zu] = %.15e", i, msg->data[i]); } } @@ -69,6 +75,6 @@ void PolynomialPlannerAi::evaluate_polynomial(const std::vector& coeffs, 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); + // RCLCPP_INFO(this->get_logger(), "P(%f) = %f", x, y); } } diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index d303883..fb30d3e 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -17,6 +17,7 @@ using namespace std::placeholders; PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node("polynomial_planner", options) { // delcare params auto random_int = this->declare_parameter("random_int", 0); + this->declare_parameter("camera_frame", "mid_cam_link"); this->path_pub = this->create_publisher("/path", 5); @@ -29,7 +30,7 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( this->tf2_listener = std::make_unique(*this->tf2_buffer); } -void PolynomialPlanner::polynomial_cb(std_msgs::msg::String::SharedPtr msg) { +void polynomial_cb(std_msgs::msg::String::SharedPtr) { if (msg->empty()) { RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); return; @@ -41,8 +42,8 @@ void PolynomialPlanner::polynomial_cb(std_msgs::msg::String::SharedPtr msg) { for (int i = 0; i < no_coeff; i++) { coeff.push_back(msg->data[i]); } - - nav_msgs::msg::Path path = backend::create_path(coeff, frame); + // TODO fix params + nav_msgs::msg::Path path = backend::create_path(coeff, frame); geometry_msgs::msg::PoseStamped path_poses[] = path.poses; diff --git a/src/backend.cpp b/src/backend.cpp index 89066a5..74dd746 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -5,15 +5,15 @@ #include "geometry_msgs/msg/pose_stamped.hpp" std::optional backend::create_path(std::vector& leftPolyVector, - sensor_msgs::msg::CameraInfo& rgb_info_sub) { + image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points // take in Polynomial // ros polynoial take in code... // transfer to Polynomial class; - float is_right_valid = false; - float is_left_valid = false; + // float is_right_valid = false; + // float is_left_valid = false; auto leftPoly = new Polynomial(leftPolyVector); @@ -37,7 +37,7 @@ std::optional backend::create_path(std::vector& left return std::nullopt; } else { // Convert from cv types to nav::msg - + // too few args std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); nav_msgs::msg::Path msg{}; @@ -48,9 +48,10 @@ std::optional backend::create_path(std::vector& left // how do cv types differ from ros types. // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), - [](const cv::Point2d& point) { + [frame](const cv::Point2d& point) { geometry_msgs::msg::PoseStamped pose{}; - // pose.header.frame_id = frame; + // frame = "redto0 isn't sure if we use this"; + pose.header.frame_id = frame; pose.pose.position.x = point.x; pose.pose.position.y = point.y; @@ -64,8 +65,8 @@ std::optional backend::create_path(std::vector& left // why are the pointer things the way they are // TODO: make it not die when z is too smallf // or make z not too small -std::vector backend::cameraPixelToGroundPos(std::vector& pixels, - sensor_msgs::msg::CameraInfo rgb_info_sub) { +std::vector cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel& rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates tf2::Quaternion optical_to_ros{}; @@ -91,7 +92,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Thu, 6 Mar 2025 21:24:14 -0500 Subject: [PATCH 29/46] wip pre jared assistance --- include/polynomial_planner/backend.hpp | 3 +-- src/backend.cpp | 6 +++--- 2 files changed, 4 insertions(+), 5 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 6e5438c..b81e2db 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -43,6 +43,5 @@ namespace backend { std::vector cameraPixelToGroundPos(std::vector& pixels, sensor_msgs::msg::CameraInfo& rgb_info_sub, std::string frame); -std::optional create_path(std::vector& leftPoly, image_geometry::PinholeCameraModel rgb_info_sub, - std::string frame); +std::optional create_path(std::vector& leftPoly, const image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame); } // namespace backend \ No newline at end of file diff --git a/src/backend.cpp b/src/backend.cpp index 74dd746..4b7def6 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -5,7 +5,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" std::optional backend::create_path(std::vector& leftPolyVector, - image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame) { + const image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -38,7 +38,7 @@ std::optional backend::create_path(std::vector& left } else { // Convert from cv types to nav::msg // too few args - std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub.cameraInfo()); nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; @@ -66,7 +66,7 @@ std::optional backend::create_path(std::vector& left // TODO: make it not die when z is too smallf // or make z not too small std::vector cameraPixelToGroundPos(std::vector& pixels, - image_geometry::PinholeCameraModel& rgb_info_sub) { + const sensor_msgs::msg::CameraInfo& rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates tf2::Quaternion optical_to_ros{}; From 970100a7ca3bb5c202acc3ba76a929fcec846116 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 6 Mar 2025 22:07:07 -0500 Subject: [PATCH 30/46] wip thursday pt2 --- include/polynomial_planner/backend.hpp | 8 +++++--- src/PolynomialPlannerAi_node.cpp | 4 ++-- src/PolynomialPlanner_node.cpp | 2 +- src/backend.cpp | 5 ++--- 4 files changed, 10 insertions(+), 9 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index b81e2db..24a0091 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -2,11 +2,11 @@ #include #include #include +#include #include #include #include "nav_msgs/msg/path.hpp" -#include class Polynomial { public: @@ -42,6 +42,8 @@ class Polynomial { namespace backend { std::vector cameraPixelToGroundPos(std::vector& pixels, - sensor_msgs::msg::CameraInfo& rgb_info_sub, std::string frame); -std::optional create_path(std::vector& leftPoly, const image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame); + sensor_msgs::msg::CameraInfo& rgb_info_sub, std::string frame); +std::optional create_path(std::vector& leftPoly, + const image_geometry::PinholeCameraModel& rgb_info_sub, + std::string frame); } // namespace backend \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 4c1f471..b2338b2 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -35,9 +35,9 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, - image_geometry::PinholeCameraModel camera_rgb) { + image_geometry::PinholeCameraModel camera_rgb) { // fix msg->empty - if (false){ + if (false) { RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); return; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index fb30d3e..5f1085f 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -43,7 +43,7 @@ void polynomial_cb(std_msgs::msg::String::SharedPtr) { coeff.push_back(msg->data[i]); } // TODO fix params - nav_msgs::msg::Path path = backend::create_path(coeff, frame); + nav_msgs::msg::Path path = backend::create_path(coeff, frame); geometry_msgs::msg::PoseStamped path_poses[] = path.poses; diff --git a/src/backend.cpp b/src/backend.cpp index 4b7def6..d49f7b4 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,8 +4,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" -std::optional backend::create_path(std::vector& leftPolyVector, - const image_geometry::PinholeCameraModel& rgb_info_sub, std::string frame) { +std::optional backend::create_path(std::vector& leftPolyVector, const image_geometry::PinholeCameraModel& rgb_info_sub,std::string frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -66,7 +65,7 @@ std::optional backend::create_path(std::vector& left // TODO: make it not die when z is too smallf // or make z not too small std::vector cameraPixelToGroundPos(std::vector& pixels, - const sensor_msgs::msg::CameraInfo& rgb_info_sub) { + const sensor_msgs::msg::CameraInfo& rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates tf2::Quaternion optical_to_ros{}; From 184806348daf32ee1f400ec28a0977ec1a0285d2 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 6 Mar 2025 22:27:43 -0500 Subject: [PATCH 31/46] wip pt3 --- src/PolynomialPlannerAi_node.cpp | 4 ++-- src/PolynomialPlanner_node.cpp | 2 ++ src/backend.cpp | 3 +-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index b2338b2..4957fe6 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -17,7 +17,7 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N // RGB_INFO PARAMETER DO NOT DELETE this->declare_parameter("camera_frame", "mid_cam_link"); - this->rgb_info_sub = this->create_subscription( + this->rgb_model_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); @@ -35,7 +35,7 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, - image_geometry::PinholeCameraModel camera_rgb) { + sensor_msgs::msg::CameraInfo camera_rgb) { // fix msg->empty if (false) { RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 5f1085f..3865e61 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -21,6 +21,8 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( this->path_pub = this->create_publisher("/path", 5); + + // run once get pointer never used again this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); diff --git a/src/backend.cpp b/src/backend.cpp index d49f7b4..906812a 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -4,7 +4,7 @@ #include "geometry_msgs/msg/pose_stamped.hpp" -std::optional backend::create_path(std::vector& leftPolyVector, const image_geometry::PinholeCameraModel& rgb_info_sub,std::string frame) { +std::optional backend::create_path(std::vector& leftPolyVector, const sensor_msgs::msg::CameraInfo& rgb_info_sub,std::string frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -44,7 +44,6 @@ std::optional backend::create_path(std::vector& left for (cv::Point2d ground_points : ground_path) { // std:: } - // how do cv types differ from ros types. // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), [frame](const cv::Point2d& point) { From 477535da9e7fdf22117243d3da111531dd079185 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 6 Mar 2025 22:53:18 -0500 Subject: [PATCH 32/46] WORKING DEMO v1.0000.00000 --- include/polynomial_planner/backend.hpp | 6 +++--- src/PolynomialPlannerAi_node.cpp | 5 +++-- src/PolynomialPlanner_node.cpp | 1 - src/backend.cpp | 15 +++++++++------ 4 files changed, 15 insertions(+), 12 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 24a0091..7415b89 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -6,6 +6,7 @@ #include #include +#include "image_geometry/pinhole_camera_model.h" #include "nav_msgs/msg/path.hpp" class Polynomial { @@ -42,8 +43,7 @@ class Polynomial { namespace backend { std::vector cameraPixelToGroundPos(std::vector& pixels, - sensor_msgs::msg::CameraInfo& rgb_info_sub, std::string frame); + image_geometry::PinholeCameraModel rgb_info_sub); std::optional create_path(std::vector& leftPoly, - const image_geometry::PinholeCameraModel& rgb_info_sub, - std::string frame); + image_geometry::PinholeCameraModel rgb_info_sub, std::string frame); } // namespace backend \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 4957fe6..d9044ac 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -10,6 +10,7 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "image_geometry/pinhole_camera_model.h" using namespace std::placeholders; @@ -17,7 +18,7 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N // RGB_INFO PARAMETER DO NOT DELETE this->declare_parameter("camera_frame", "mid_cam_link"); - this->rgb_model_sub = this->create_subscription( + this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); @@ -35,7 +36,7 @@ PolynomialPlannerAi::PolynomialPlannerAi(const rclcpp::NodeOptions& options) : N } void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::SharedPtr msg, - sensor_msgs::msg::CameraInfo camera_rgb) { + image_geometry::PinholeCameraModel camera_rgb) { // fix msg->empty if (false) { RCLCPP_WARN(this->get_logger(), "Received empty polynomial (non-AI)"); diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 3865e61..7e77a44 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -21,7 +21,6 @@ PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node( this->path_pub = this->create_publisher("/path", 5); - // run once get pointer never used again this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, diff --git a/src/backend.cpp b/src/backend.cpp index 906812a..808a496 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -3,8 +3,11 @@ #include #include "geometry_msgs/msg/pose_stamped.hpp" +#include "image_geometry/pinhole_camera_model.h" -std::optional backend::create_path(std::vector& leftPolyVector, const sensor_msgs::msg::CameraInfo& rgb_info_sub,std::string frame) { +std::optional backend::create_path(std::vector& leftPolyVector, + image_geometry::PinholeCameraModel rgb_info_sub, + std::string frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -37,7 +40,7 @@ std::optional backend::create_path(std::vector& left } else { // Convert from cv types to nav::msg // too few args - std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub.cameraInfo()); + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; @@ -63,8 +66,8 @@ std::optional backend::create_path(std::vector& left // why are the pointer things the way they are // TODO: make it not die when z is too smallf // or make z not too small -std::vector cameraPixelToGroundPos(std::vector& pixels, - const sensor_msgs::msg::CameraInfo& rgb_info_sub) { +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, + image_geometry::PinholeCameraModel rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates tf2::Quaternion optical_to_ros{}; @@ -75,8 +78,8 @@ std::vector cameraPixelToGroundPos(std::vector& pixels for (const cv::Point2d& pixel : pixels) { // gotta rectify the pixel before we raycast - cv::Point2d rectPixel = rgb_info_sub->rgb_model.rectifyPoint(pixel); - cv::Point3d ray = rgb_info_sub->rgb_model.projectPixelTo3dRay(rectPixel); + cv::Point2d rectPixel = rgb_info_sub.rectifyPoint(pixel); + cv::Point3d ray = rgb_info_sub.projectPixelTo3dRay(rectPixel); // ask zach for the trig, extend ray to the floor. float divisor = ray.z / -0.6; From 02947e481f2f593d4056e78a1ea2e0c60c106b4f Mon Sep 17 00:00:00 2001 From: redto0 Date: Fri, 7 Mar 2025 01:04:19 -0500 Subject: [PATCH 33/46] WORKING DEMO v1.000.001 --- include/polynomial_planner/backend.hpp | 2 +- src/PolynomialPlannerAi_node.cpp | 7 +++++-- src/backend.cpp | 7 ++++--- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 7415b89..68ec286 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -45,5 +45,5 @@ namespace backend { std::vector cameraPixelToGroundPos(std::vector& pixels, image_geometry::PinholeCameraModel rgb_info_sub); std::optional create_path(std::vector& leftPoly, - image_geometry::PinholeCameraModel rgb_info_sub, std::string frame); + image_geometry::PinholeCameraModel rgb_info_sub, std::string_view frame); } // namespace backend \ No newline at end of file diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index d9044ac..ab0805f 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -50,13 +50,16 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared coeff.push_back(msg->data[i]); } - std::string frame_id = this->get_parameter("camera_frame").as_string(); - + //std::string frame_id = this->get_parameter("camera_frame").as_string(); + //std::string frame_id = "notemptystring"; + auto frame_id = this->get_parameter(std::string("camera_frame")).as_string(); std::optional path_optional = backend::create_path(coeff, camera_rgb, frame_id); nav_msgs::msg::Path path; if (path_optional.has_value()) { path = path_optional.value(); + path.header.frame_id = this->get_parameter(std::string("camera_frame")).as_string(); + } else return; diff --git a/src/backend.cpp b/src/backend.cpp index 808a496..27d04eb 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -7,7 +7,7 @@ std::optional backend::create_path(std::vector& leftPolyVector, image_geometry::PinholeCameraModel rgb_info_sub, - std::string frame) { + std::string_view frame) { // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points @@ -49,10 +49,11 @@ std::optional backend::create_path(std::vector& left } // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), - [frame](const cv::Point2d& point) { + [&frame](const cv::Point2d& point) { geometry_msgs::msg::PoseStamped pose{}; // frame = "redto0 isn't sure if we use this"; - pose.header.frame_id = frame; + // redto0 is SURE that we use this update and fix ASAP + pose.header.frame_id = frame; // literally is "notaemptystring" pose.pose.position.x = point.x; pose.pose.position.y = point.y; From 9fdc4182b526ad18c602e3dd1e4614fa4aa5470f Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Sun, 9 Mar 2025 21:40:47 -0400 Subject: [PATCH 34/46] Poly class fix backend.hpp Signed-off-by: Alexander Boccaccio --- include/polynomial_planner/backend.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 68ec286..82ffbbb 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -23,7 +23,7 @@ class Polynomial { 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 @@ -33,9 +33,9 @@ class Polynomial { 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) } } }; @@ -46,4 +46,4 @@ std::vector cameraPixelToGroundPos(std::vector& pixels image_geometry::PinholeCameraModel rgb_info_sub); std::optional create_path(std::vector& leftPoly, image_geometry::PinholeCameraModel rgb_info_sub, std::string_view frame); -} // namespace backend \ No newline at end of file +} // namespace backend From 0a44167f18ffb940b75b0ab0dbef928377049ac8 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Mon, 24 Feb 2025 20:01:25 -0500 Subject: [PATCH 35/46] Fazal said it doesn't work (he didnt lie) --- .../PolynomialPlannerAi_node.hpp | 1 + .../PolynomialPlanner_node.hpp | 79 ++++++++++--------- src/PolynomialPlanner_node.cpp | 7 +- 3 files changed, 44 insertions(+), 43 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index 6de3afa..dcede37 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -13,6 +13,7 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" +#include "std_msgs/msg/float32_multi_array.hpp" class PolynomialPlannerAi : public rclcpp::Node { private: diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index 7ece804..ecc8905 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,39 +1,40 @@ -//#pragma once -// -//#include -//#include -// -//#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::SharedPtr path_pub; -// -// rclcpp::Subscription::SharedPtr sub; -// -// // Camera info sub & model vars -// rclcpp::Subscription::SharedPtr rgb_info_sub; -// image_geometry::PinholeCameraModel rgb_model; -// -// // std::optional -// std::unique_ptr> Backend; -// -// // TF2 stuff -// std::unique_ptr tf2_listener; -// std::unique_ptr 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& pixels); -//}; +#pragma once + +#include +#include + +#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" +#include "std_msgs/msg/float32_multi_array.hpp" + +class PolynomialPlanner : public rclcpp::Node { +private: + rclcpp::Publisher::SharedPtr path_pub; + + rclcpp::Subscription::SharedPtr poly_sub; + + // Camera info sub & model vars + rclcpp::Subscription::SharedPtr rgb_info_sub; + image_geometry::PinholeCameraModel rgb_model; + + // std::optional + std::unique_ptr> Backend; + + // TF2 stuff + std::unique_ptr tf2_listener; + std::unique_ptr 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& pixels); +}; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 7e77a44..f25ab97 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -15,13 +15,12 @@ using namespace std::placeholders; PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node("polynomial_planner", options) { - // delcare params - auto random_int = this->declare_parameter("random_int", 0); - this->declare_parameter("camera_frame", "mid_cam_link"); this->path_pub = this->create_publisher("/path", 5); - // run once get pointer never used again + this->poly_sub = this->create_subscription( + "/road/polynomial", 1, std::bind(&PolynomialPlanner::polynomial_cb, this, _1)); + this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); From b5e5723faed382d8041b11e9fae88a3dea8ffb32 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Mon, 10 Mar 2025 15:54:02 -0400 Subject: [PATCH 36/46] Revert "Fazal said it doesn't work (he didnt lie)" This reverts commit 0a44167f18ffb940b75b0ab0dbef928377049ac8. --- .../PolynomialPlannerAi_node.hpp | 1 - .../PolynomialPlanner_node.hpp | 79 +++++++++---------- src/PolynomialPlanner_node.cpp | 7 +- 3 files changed, 43 insertions(+), 44 deletions(-) diff --git a/include/polynomial_planner/PolynomialPlannerAi_node.hpp b/include/polynomial_planner/PolynomialPlannerAi_node.hpp index dcede37..6de3afa 100644 --- a/include/polynomial_planner/PolynomialPlannerAi_node.hpp +++ b/include/polynomial_planner/PolynomialPlannerAi_node.hpp @@ -13,7 +13,6 @@ #include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" -#include "std_msgs/msg/float32_multi_array.hpp" class PolynomialPlannerAi : public rclcpp::Node { private: diff --git a/include/polynomial_planner/PolynomialPlanner_node.hpp b/include/polynomial_planner/PolynomialPlanner_node.hpp index ecc8905..7ece804 100644 --- a/include/polynomial_planner/PolynomialPlanner_node.hpp +++ b/include/polynomial_planner/PolynomialPlanner_node.hpp @@ -1,40 +1,39 @@ -#pragma once - -#include -#include - -#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" -#include "std_msgs/msg/float32_multi_array.hpp" - -class PolynomialPlanner : public rclcpp::Node { -private: - rclcpp::Publisher::SharedPtr path_pub; - - rclcpp::Subscription::SharedPtr poly_sub; - - // Camera info sub & model vars - rclcpp::Subscription::SharedPtr rgb_info_sub; - image_geometry::PinholeCameraModel rgb_model; - - // std::optional - std::unique_ptr> Backend; - - // TF2 stuff - std::unique_ptr tf2_listener; - std::unique_ptr 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& pixels); -}; +//#pragma once +// +//#include +//#include +// +//#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::SharedPtr path_pub; +// +// rclcpp::Subscription::SharedPtr sub; +// +// // Camera info sub & model vars +// rclcpp::Subscription::SharedPtr rgb_info_sub; +// image_geometry::PinholeCameraModel rgb_model; +// +// // std::optional +// std::unique_ptr> Backend; +// +// // TF2 stuff +// std::unique_ptr tf2_listener; +// std::unique_ptr 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& pixels); +//}; diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index f25ab97..7e77a44 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -15,12 +15,13 @@ using namespace std::placeholders; PolynomialPlanner::PolynomialPlanner(const rclcpp::NodeOptions& options) : Node("polynomial_planner", options) { + // delcare params + auto random_int = this->declare_parameter("random_int", 0); + this->declare_parameter("camera_frame", "mid_cam_link"); this->path_pub = this->create_publisher("/path", 5); - this->poly_sub = this->create_subscription( - "/road/polynomial", 1, std::bind(&PolynomialPlanner::polynomial_cb, this, _1)); - + // run once get pointer never used again this->rgb_info_sub = this->create_subscription( "/camera/mid/rgb/camera_info", 1, [this](sensor_msgs::msg::CameraInfo::ConstSharedPtr ci) { this->rgb_model.fromCameraInfo(ci); }); From ee683864099e5ca65c0e673d3af74948b797b736 Mon Sep 17 00:00:00 2001 From: = <=> Date: Mon, 10 Mar 2025 18:06:05 -0400 Subject: [PATCH 37/46] `` connor commit --- src/backend.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 27d04eb..b4d8bfd 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -40,7 +40,7 @@ std::optional backend::create_path(std::vector& left } else { // Convert from cv types to nav::msg // too few args - std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; @@ -56,6 +56,7 @@ std::optional backend::create_path(std::vector& left pose.header.frame_id = frame; // literally is "notaemptystring" pose.pose.position.x = point.x; pose.pose.position.y = point.y; + pose.pose.position.z = point.z; return pose; }); @@ -67,13 +68,13 @@ std::optional backend::create_path(std::vector& left // why are the pointer things the way they are // TODO: make it not die when z is too smallf // or make z not too small -std::vector backend::cameraPixelToGroundPos(std::vector& pixels, +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, image_geometry::PinholeCameraModel rgb_info_sub) { // 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(-M_PI / 2, 0.0, -M_PI / 2); + optical_to_ros.setRPY(0.0, 0.0, -M_PI / 2); std::vector rwpoints; @@ -83,7 +84,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector backend::cameraPixelToGroundPos(std::vector Date: Mon, 10 Mar 2025 21:55:32 -0400 Subject: [PATCH 38/46] merge --- src/backend.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/backend.cpp b/src/backend.cpp index b4d8bfd..8d83606 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -75,6 +75,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector rwpoints; From af6d9b82f86bea807ab770af0ca0a6e8cb060377 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 1 Mar 2025 19:39:33 -0500 Subject: [PATCH 39/46] chelsea commit --- src/backend.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 8d83606..03d1d85 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -40,13 +40,13 @@ std::optional backend::create_path(std::vector& left } else { // Convert from cv types to nav::msg // too few args - std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); + std::vector ground_path = cameraPixelToGroundPos(cam_path, rgb_info_sub); nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; - for (cv::Point2d ground_points : ground_path) { + // for (cv::Point2d ground_points : ground_path) { // std:: - } + // } // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), [&frame](const cv::Point2d& point) { @@ -56,7 +56,7 @@ std::optional backend::create_path(std::vector& left pose.header.frame_id = frame; // literally is "notaemptystring" pose.pose.position.x = point.x; pose.pose.position.y = point.y; - pose.pose.position.z = point.z; + // pose.pose.position.z = point.z; return pose; }); @@ -68,7 +68,7 @@ std::optional backend::create_path(std::vector& left // why are the pointer things the way they are // TODO: make it not die when z is too smallf // or make z not too small -std::vector backend::cameraPixelToGroundPos(std::vector& pixels, +std::vector backend::cameraPixelToGroundPos(std::vector& pixels, image_geometry::PinholeCameraModel rgb_info_sub) { // Rotation that rotates left 90 and backwards 90. // This converts from camera coordinates in OpenCV to ROS coordinates @@ -96,7 +96,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Wed, 12 Mar 2025 20:05:08 -0400 Subject: [PATCH 40/46] update to path projection --- include/polynomial_planner/backend.hpp | 1 + src/PolynomialPlannerAi_node.cpp | 20 +++++++++----------- src/PolynomialPlanner_node.cpp | 2 +- src/backend.cpp | 23 ++++++++++++----------- 4 files changed, 23 insertions(+), 23 deletions(-) diff --git a/include/polynomial_planner/backend.hpp b/include/polynomial_planner/backend.hpp index 82ffbbb..617bd2b 100644 --- a/include/polynomial_planner/backend.hpp +++ b/include/polynomial_planner/backend.hpp @@ -37,6 +37,7 @@ class Polynomial { // todo finish? result += vect[i] * (power + 1) * pow(x, power); // a[n] * n * x ^ (n - 1) } + return result; } }; diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index ab0805f..5be4140 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -52,6 +52,7 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared //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 path_optional = backend::create_path(coeff, camera_rgb, frame_id); nav_msgs::msg::Path path; @@ -59,17 +60,14 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared if (path_optional.has_value()) { path = path_optional.value(); path.header.frame_id = this->get_parameter(std::string("camera_frame")).as_string(); - - } else - return; - - 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]); + 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]); + } + } + return; } } diff --git a/src/PolynomialPlanner_node.cpp b/src/PolynomialPlanner_node.cpp index 7e77a44..a8f2111 100644 --- a/src/PolynomialPlanner_node.cpp +++ b/src/PolynomialPlanner_node.cpp @@ -51,7 +51,7 @@ void polynomial_cb(std_msgs::msg::String::SharedPtr) { bool do_logger = true; if (do_logger) { for (int i = 0; i < path_poses.size(); i++) { - RCLCPP_INFO(this->get_logger(), ""); + RCLCPP_INFO(this->get_logger(), " we have polynomial"); } } this->path_pub->publish(*path); diff --git a/src/backend.cpp b/src/backend.cpp index 03d1d85..ce0b1be 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -20,17 +20,18 @@ std::optional backend::create_path(std::vector& left auto leftPoly = new Polynomial(leftPolyVector); // interval for polynomial - float max = 280; // artificial event horizon + float max = 256; // artificial event horizon, + // the x value in which path points are no longer allowed to cross. float interval = 3; // stepping x value up by 3camera px on each iteration float start = 475; // bottom of frame - float threshold = 15.0; // min dist between points + float threshold = 10.0; // min dist between points - float dist = 0; + float dist = 0; // the value between the last published point and the current point for (int x = start; x > max; x -= interval) { dist += sqrt(interval * interval + pow(leftPoly->poly(x) - leftPoly->poly(x + interval), 2)); if (dist > threshold) { - cam_path.push_back(cv::Point2d(x, leftPoly->poly(x))); + cam_path.push_back(cv::Point2d(x + 256, leftPoly->poly(x + 256))); dist = 0; } } @@ -45,8 +46,8 @@ std::optional backend::create_path(std::vector& left nav_msgs::msg::Path msg{}; // msg.header.frame_id = frame; // for (cv::Point2d ground_points : ground_path) { - // std:: - // } + // std:: + // } // converting to message type in ROS std::transform(ground_path.begin(), ground_path.end(), std::back_inserter(msg.poses), [&frame](const cv::Point2d& point) { @@ -66,7 +67,7 @@ std::optional backend::create_path(std::vector& left } // why are the pointer things the way they are -// TODO: make it not die when z is too smallf +// TODO: make it not die when z is too small // or make z not too small std::vector backend::cameraPixelToGroundPos(std::vector& pixels, image_geometry::PinholeCameraModel rgb_info_sub) { @@ -75,7 +76,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector rwpoints; @@ -85,18 +86,18 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Fri, 14 Mar 2025 22:30:35 -0400 Subject: [PATCH 41/46] pointing right wayish --- src/backend.cpp | 28 +++++++++++++++++++++++----- 1 file changed, 23 insertions(+), 5 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index ce0b1be..4db82ec 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -20,10 +20,10 @@ std::optional backend::create_path(std::vector& left auto leftPoly = new Polynomial(leftPolyVector); // interval for polynomial - float max = 256; // artificial event horizon, + float max = 20; // artificial event horizon, // the x value in which path points are no longer allowed to cross. float interval = 3; // stepping x value up by 3camera px on each iteration - float start = 475; // bottom of frame + float start = 220; // bottom of frame float threshold = 10.0; // min dist between points float dist = 0; // the value between the last published point and the current point @@ -31,11 +31,27 @@ std::optional backend::create_path(std::vector& left dist += sqrt(interval * interval + pow(leftPoly->poly(x) - leftPoly->poly(x + interval), 2)); if (dist > threshold) { - cam_path.push_back(cv::Point2d(x + 256, leftPoly->poly(x + 256))); + int translate = 480 - 224; + float camX = leftPoly->poly(x + translate); + float camY = x + translate; + if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { + // CV point should be x,y + // the poly is P(y) + // translate should apply to the Y values? + // cam_path.push_back(cv::Point2d(camX, camY)); + } dist = 0; } } + for (int i = 55; i < 200; i++) { + float camX = 320; + float camY = (i) + 240; + if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { + cam_path.push_back(cv::Point2d(camY, camX)); + } + } + if (cam_path.empty()) { return std::nullopt; } else { @@ -77,6 +93,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector rwpoints; @@ -91,13 +108,14 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Mon, 17 Mar 2025 18:03:37 -0400 Subject: [PATCH 42/46] bug fix --- src/backend.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 4db82ec..837e9a9 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -38,7 +38,7 @@ std::optional backend::create_path(std::vector& left // CV point should be x,y // the poly is P(y) // translate should apply to the Y values? - // cam_path.push_back(cv::Point2d(camX, camY)); + cam_path.push_back(cv::Point2d(camX, camY)); } dist = 0; } @@ -48,7 +48,7 @@ std::optional backend::create_path(std::vector& left float camX = 320; float camY = (i) + 240; if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { - cam_path.push_back(cv::Point2d(camY, camX)); + // cam_path.push_back(cv::Point2d(camY, camX)); } } @@ -103,7 +103,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector backend::cameraPixelToGroundPos(std::vector Date: Tue, 18 Mar 2025 17:02:55 -0400 Subject: [PATCH 43/46] WORKING DEMO 2.000.000 --- src/PolynomialPlannerAi_node.cpp | 3 +++ src/backend.cpp | 26 ++++++++++++++++++-------- 2 files changed, 21 insertions(+), 8 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 5be4140..a509e32 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -49,6 +49,9 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared for (int i = 0; i < no_coeff; i++) { coeff.push_back(msg->data[i]); } + + std::string p = std::to_string(camera_rgb.cx()); + RCLCPP_INFO(this->get_logger(), p.c_str()); //std::string frame_id = this->get_parameter("camera_frame").as_string(); //std::string frame_id = "notemptystring"; diff --git a/src/backend.cpp b/src/backend.cpp index 837e9a9..647f717 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -48,10 +48,12 @@ std::optional backend::create_path(std::vector& left float camX = 320; float camY = (i) + 240; if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { - // cam_path.push_back(cv::Point2d(camY, camX)); + // cam_path.push_back(cv::Point2d(camX, camY)); } } + + if (cam_path.empty()) { return std::nullopt; } else { @@ -96,26 +98,34 @@ std::vector backend::cameraPixelToGroundPos(std::vector rwpoints; - - for (const cv::Point2d& pixel : pixels) { + + 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(rectPixel); + // -- 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. - float divisor = ray.z /1; + float divisor = ray.y / 0.6; 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}; - tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); + tf2::Vector3 tf_vec{ray.z, -ray.x, -ray.y}; + //tf2::Vector3 tf_vec{ray.x, ray.y, ray.z}; + //tf2::Vector3 world_vec = tf2::quatRotate(optical_to_ros, tf_vec); //return type world_vec, use this is - cv::Point2d dvector(world_vec.y(), world_vec.x()); + cv::Point2d dvector(tf_vec.x(), tf_vec.y()); // push back vectors rwpoints.push_back(dvector); From 9674e320ecc7f562b5335b2170d1de8cb6f65f13 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Tue, 18 Mar 2025 17:51:00 -0400 Subject: [PATCH 44/46] update --- src/backend.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/backend.cpp b/src/backend.cpp index 647f717..df7560b 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -32,6 +32,7 @@ std::optional backend::create_path(std::vector& left if (dist > threshold) { int translate = 480 - 224; + // translate =0; float camX = leftPoly->poly(x + translate); float camY = x + translate; if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { @@ -103,8 +104,8 @@ std::vector backend::cameraPixelToGroundPos(std::vector Date: Tue, 18 Mar 2025 19:29:18 -0400 Subject: [PATCH 45/46] code reformat --- src/PolynomialPlannerAi_node.cpp | 2 +- src/backend.cpp | 4 +--- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index a509e32..6ca0322 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -49,7 +49,7 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared for (int i = 0; i < no_coeff; i++) { coeff.push_back(msg->data[i]); } - + std::string p = std::to_string(camera_rgb.cx()); RCLCPP_INFO(this->get_logger(), p.c_str()); diff --git a/src/backend.cpp b/src/backend.cpp index df7560b..369a17e 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -53,8 +53,6 @@ std::optional backend::create_path(std::vector& left } } - - if (cam_path.empty()) { return std::nullopt; } else { @@ -99,7 +97,7 @@ std::vector backend::cameraPixelToGroundPos(std::vector rwpoints; - + for (cv::Point2d& pixel : pixels) { // gotta rectify the pixel before we raycast pixel.y += 120; From ddbe97de600e32260866d58c8f2e030b54bc7340 Mon Sep 17 00:00:00 2001 From: Alexander Boccaccio Date: Sat, 5 Apr 2025 16:25:24 -0400 Subject: [PATCH 46/46] using original frame with camera info constants Signed-off-by: Alexander Boccaccio --- src/PolynomialPlannerAi_node.cpp | 10 +++++++--- src/backend.cpp | 15 ++++++++------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/PolynomialPlannerAi_node.cpp b/src/PolynomialPlannerAi_node.cpp index 6ca0322..a4cf5f6 100644 --- a/src/PolynomialPlannerAi_node.cpp +++ b/src/PolynomialPlannerAi_node.cpp @@ -50,9 +50,7 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared coeff.push_back(msg->data[i]); } - std::string p = std::to_string(camera_rgb.cx()); - RCLCPP_INFO(this->get_logger(), p.c_str()); - + //std::string frame_id = this->get_parameter("camera_frame").as_string(); //std::string frame_id = "notemptystring"; // TODO camera frame_id is wrong @@ -60,8 +58,11 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared std::optional 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 @@ -69,6 +70,9 @@ void PolynomialPlannerAi::polynomial_cb(std_msgs::msg::Float32MultiArray::Shared 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; } diff --git a/src/backend.cpp b/src/backend.cpp index 369a17e..ac987e5 100644 --- a/src/backend.cpp +++ b/src/backend.cpp @@ -11,6 +11,9 @@ std::optional backend::create_path(std::vector& left // std::string_view is a string lol std::vector cam_path; // this is the vector of path plannign points + int width = rgb_info_sub.fullResolution().width; // camera space sizes! + int height = rgb_info_sub.fullResolution().height; // Camera space sizes! + // take in Polynomial // ros polynoial take in code... // transfer to Polynomial class; @@ -20,10 +23,10 @@ std::optional backend::create_path(std::vector& left auto leftPoly = new Polynomial(leftPolyVector); // interval for polynomial - float max = 20; // artificial event horizon, + float max = height - height * .40; // artificial event horizon, // the x value in which path points are no longer allowed to cross. float interval = 3; // stepping x value up by 3camera px on each iteration - float start = 220; // bottom of frame + float start = height - height * 0.30; // bottom of frame float threshold = 10.0; // min dist between points float dist = 0; // the value between the last published point and the current point @@ -32,15 +35,15 @@ std::optional backend::create_path(std::vector& left if (dist > threshold) { int translate = 480 - 224; - // translate =0; + translate =0; float camX = leftPoly->poly(x + translate); float camY = x + translate; - if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { + // if (camY >= 240 && camY <= 480 && camX >= 0 && camX <= 640) { // CV point should be x,y // the poly is P(y) // translate should apply to the Y values? cam_path.push_back(cv::Point2d(camX, camY)); - } + // } dist = 0; } } @@ -100,8 +103,6 @@ std::vector backend::cameraPixelToGroundPos(std::vector