From 2e5e7985702effba2e6eb1129d4c21bbf9899166 Mon Sep 17 00:00:00 2001 From: redto0 Date: Sat, 3 May 2025 17:53:29 -0400 Subject: [PATCH 1/4] WIP Updated Spline to not upate if brokern and added better no point support Signed-off-by: redto0 --- include/hybrid_pp/PurePursuitNode_node.hpp | 18 +++---- src/PurePursuitNode_node.cpp | 60 +++++++++++++++++----- 2 files changed, 56 insertions(+), 22 deletions(-) diff --git a/include/hybrid_pp/PurePursuitNode_node.hpp b/include/hybrid_pp/PurePursuitNode_node.hpp index 811b182..b95d96e 100644 --- a/include/hybrid_pp/PurePursuitNode_node.hpp +++ b/include/hybrid_pp/PurePursuitNode_node.hpp @@ -114,14 +114,14 @@ class PurePursuitNode : public rclcpp::Node { return std::hypot((float)p1.x - (float)p2.x, (float)p1.y - (float)p2.y); } - /// Publishes a zero move command - void publish_stop_command() { - ackermann_msgs::msg::AckermannDrive stop{}; - //TODO hotfix to get left at comp - stop.steering_angle = 0.27; - stop.speed = 1.0; - this->nav_ack_vel_pub->publish(stop); - } + // /// Publishes a zero move command + // void publish_stop_command() { + // ackermann_msgs::msg::AckermannDrive stop{}; + // //TODO hotfix to get left at comp + // stop.steering_angle = 0.27; + // stop.speed = 1.0; + // this->nav_ack_vel_pub->publish(stop); + // } /// Transforms a path into a given target frame. void transform_path(nav_msgs::msg::Path& path, std::string target_frame) { @@ -168,7 +168,7 @@ class PurePursuitNode : public rclcpp::Node { /// Get the point where the look ahead distance intersects the path std::optional get_path_point(const std::vector& path_spline); /// Creates a spline from the path recived from path planner and also checks if it needs to avoid objects - std::vector get_path_spline(const nav_msgs::msg::Path& path); + std::optional> get_path_spline(const nav_msgs::msg::Path& path); ~PurePursuitNode() override { this->stop_token.store(true); } }; diff --git a/src/PurePursuitNode_node.cpp b/src/PurePursuitNode_node.cpp index 0437151..db8fc0a 100644 --- a/src/PurePursuitNode_node.cpp +++ b/src/PurePursuitNode_node.cpp @@ -23,7 +23,7 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options) wheel_base = this->declare_parameter("wheel_base", 1.08); gravity_constant = this->declare_parameter("gravity_constant", 9.81); debug = this->declare_parameter("debug", true); - this->declare_parameter("stop_on_no_path", true); + this->declare_parameter("stop_on_no_path", false); // Var init current_speed = 1; @@ -58,6 +58,7 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options) // The calculated intersection and look ahead distance std::optional path_result; + std::optional last_path_result; // Visualization components for publishing visualization VisualizationComponents vis_components{}; @@ -74,11 +75,29 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options) // If no way to follow path, just stop if (!path_result.has_value()) { - if (this->get_parameter("stop_on_no_path").as_bool()) { - this->publish_stop_command(); + if (false && this->get_parameter("stop_on_no_path").as_bool()) { + // no more + // this->publish_stop_command(); + + } else { + + ackermann_msgs::msg::AckermannDrive keepgoing{}; + //TODO hotfix to get left at comp + keepgoing.speed = 1.0; + + if ((*last_path_result).intersection_point.pose.position.x < 0) { /////////////////////// this hopefully should switch the steering directions but breaks on right turns + keepgoing.steering_angle = -0.27; + } else { + keepgoing.steering_angle = 0.27; + } + + this->nav_ack_vel_pub->publish(keepgoing); + } continue; + } else { + last_path_result = path_result.value(); } // Calculate command to point on path @@ -130,7 +149,10 @@ void PurePursuitNode::path_cb(const nav_msgs::msg::Path::SharedPtr msg) { std::unique_lock lk2{this->obj_mtx}; // Create a spline from the path message - this->path_spline = get_path_spline(*msg); + auto holder = get_path_spline(*msg); + if ( holder.has_value()){ + this->path_spline = holder.value(); + } // otherwise we do nothing! } } @@ -390,7 +412,7 @@ std::vector PurePursuitNode::get_objects_from_s return detected_points; } -std::vector PurePursuitNode::get_path_spline(const nav_msgs::msg::Path& path) { +std::optional> PurePursuitNode::get_path_spline(const nav_msgs::msg::Path& path) { // Create a copy of the path to avoid mutating the main auto local_path = path; auto local_objects = this->objects; @@ -403,24 +425,36 @@ std::vector PurePursuitNode::get_path_spline(co // Spline connecting our path points std::vector spline; + bool gate = true; + float look_ahead_distance = std::clamp(k_dd * current_speed, min_look_ahead_distance, max_look_ahead_distance); // Build a spline of linear lines for our path for (size_t i = 0; i < local_path.poses.size() - 1; i++) { auto point1 = local_path.poses.at(i); - auto point2 = local_path.poses.at(i + 1); - // Do the DDA algorithm https://en.wikipedia.org/wiki/Digital_differential_analyzer_(graphics_algorithm) - float dx = (float)point2.pose.position.x - (float)point1.pose.position.x; - float dy = (float)point2.pose.position.y - (float)point1.pose.position.y; - float steps = std::max(std::abs(dx), std::abs(dy)); - float x_inc = dx / steps; - float y_inc = dy / steps; + double& x = point1.pose.position.x; + double& y = point1.pose.position.y; + + if ( std::pow(std::pow(x,2) + std::pow(y,2),0.5) < look_ahead_distance){ + gate = false; + } + // auto point2 = local_path.poses.at(i + 1); + // Do the DDA algorithm https://en.wikipedia.org/wiki/Digital_differential_analyzer_(graphics_algorithm) + // we publish to many path points for this is be useful anymore + // float dx = (float)point2.pose.position.x - (float)point1.pose.position.x; + // float dy = (float)point2.pose.position.y - (float)point1.pose.position.y; + // float steps = std::max(std::abs(dx), std::abs(dy)); + // float x_inc = dx / steps; + // float y_inc = dy / steps; + spline.push_back(point1); + /* for (int j = 0; (float)j < steps; j++) { point1.pose.position.x += x_inc; point1.pose.position.y += y_inc; spline.push_back(point1); - } + }*/ } + if (gate) return std::nullopt; // For each spline point, shift it if it is too close to an obstacle. This has the effect of wrapping the spline // around obstacles. From 4ddde60d99ffda76bfac421906493060f730476c Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 29 May 2025 19:13:29 -0400 Subject: [PATCH 2/4] Bug fix! Now with more speed! Signed-off-by: redto0 --- src/PurePursuitNode_node.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/PurePursuitNode_node.cpp b/src/PurePursuitNode_node.cpp index db8fc0a..1fde0e4 100644 --- a/src/PurePursuitNode_node.cpp +++ b/src/PurePursuitNode_node.cpp @@ -17,7 +17,7 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options) max_look_ahead_distance = this->declare_parameter("max_look_ahead_distance", 10.0); k_dd = this->declare_parameter("k_dd", 1.0); max_speed = this->declare_parameter("max_speed", 6.7056); - max_speed = this->declare_parameter("min_speed", 0.5); + min_speed = this->declare_parameter("min_speed", 0.5); avoidance_radius = this->declare_parameter("avoidance_radius", 2); rear_axle_frame = this->declare_parameter("rear_axle_frame", "rear_axle"); wheel_base = this->declare_parameter("wheel_base", 1.08); @@ -415,6 +415,8 @@ std::vector PurePursuitNode::get_objects_from_s std::optional> PurePursuitNode::get_path_spline(const nav_msgs::msg::Path& path) { // Create a copy of the path to avoid mutating the main auto local_path = path; + + // std::vector auto local_objects = this->objects; // Transform odom->rear-axle, for easy calculations From bf72b31270bcbd2c631bdad2a8ede726f50b0261 Mon Sep 17 00:00:00 2001 From: redto0 Date: Thu, 30 Oct 2025 00:22:42 -0400 Subject: [PATCH 3/4] minor comments --- src/PurePursuitNode_node.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/PurePursuitNode_node.cpp b/src/PurePursuitNode_node.cpp index 1fde0e4..49afecd 100644 --- a/src/PurePursuitNode_node.cpp +++ b/src/PurePursuitNode_node.cpp @@ -427,7 +427,7 @@ std::optional> PurePursuitNode::get // Spline connecting our path points std::vector spline; - bool gate = true; + bool fails_to_exceed_look_ahead = true; float look_ahead_distance = std::clamp(k_dd * current_speed, min_look_ahead_distance, max_look_ahead_distance); // Build a spline of linear lines for our path for (size_t i = 0; i < local_path.poses.size() - 1; i++) { @@ -437,7 +437,7 @@ std::optional> PurePursuitNode::get double& y = point1.pose.position.y; if ( std::pow(std::pow(x,2) + std::pow(y,2),0.5) < look_ahead_distance){ - gate = false; + fails_to_exceed_look_ahead = false; } // auto point2 = local_path.poses.at(i + 1); @@ -450,13 +450,19 @@ std::optional> PurePursuitNode::get // float y_inc = dy / steps; spline.push_back(point1); /* + // this is the DDA for when we dont have enough points for (int j = 0; (float)j < steps; j++) { point1.pose.position.x += x_inc; point1.pose.position.y += y_inc; spline.push_back(point1); }*/ } - if (gate) return std::nullopt; + // why do i do this? + // checks if look ahead distance does not ever exceed beyond look ahead distance + if (fails_to_exceed_look_ahead) return std::nullopt; + // stay on old path. + // Otherwise we will be provided a path that is impossible to follow + // because look ahead is what will choice the clsest point that also exceeds it. // For each spline point, shift it if it is too close to an obstacle. This has the effect of wrapping the spline // around obstacles. From e138ad3ac1f198ecd9b07e000269b0c5c8bb8cee Mon Sep 17 00:00:00 2001 From: redto0 Date: Mon, 22 Dec 2025 19:29:36 -0500 Subject: [PATCH 4/4] Reformatted for style guidelines --- include/hybrid_pp/PurePursuitNode_node.hpp | 2 +- src/PurePursuitNode_node.cpp | 34 ++++++++++++---------- 2 files changed, 19 insertions(+), 17 deletions(-) diff --git a/include/hybrid_pp/PurePursuitNode_node.hpp b/include/hybrid_pp/PurePursuitNode_node.hpp index b95d96e..a84421e 100644 --- a/include/hybrid_pp/PurePursuitNode_node.hpp +++ b/include/hybrid_pp/PurePursuitNode_node.hpp @@ -11,13 +11,13 @@ #include "ackermann_msgs/msg/ackermann_drive.hpp" #include "geometry_msgs/msg/pose_stamped.hpp" +#include "hybrid_pp/filters.hpp" #include "laser_geometry/laser_geometry/laser_geometry.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav_msgs/msg/path.hpp" #include "rclcpp/rclcpp.hpp" #include "sensor_msgs/msg/laser_scan.hpp" #include "visualization_msgs/msg/marker.hpp" -#include "hybrid_pp/filters.hpp" /// Pure pursuit command result, with components struct CommandCalcResult { diff --git a/src/PurePursuitNode_node.cpp b/src/PurePursuitNode_node.cpp index 49afecd..dca897b 100644 --- a/src/PurePursuitNode_node.cpp +++ b/src/PurePursuitNode_node.cpp @@ -80,19 +80,18 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options) // this->publish_stop_command(); } else { - ackermann_msgs::msg::AckermannDrive keepgoing{}; //TODO hotfix to get left at comp keepgoing.speed = 1.0; - if ((*last_path_result).intersection_point.pose.position.x < 0) { /////////////////////// this hopefully should switch the steering directions but breaks on right turns + if ((*last_path_result).intersection_point.pose.position.x < + 0) { /////////////////////// this hopefully should switch the steering directions but breaks on right turns keepgoing.steering_angle = -0.27; } else { keepgoing.steering_angle = 0.27; } this->nav_ack_vel_pub->publish(keepgoing); - } continue; @@ -150,9 +149,9 @@ void PurePursuitNode::path_cb(const nav_msgs::msg::Path::SharedPtr msg) { // Create a spline from the path message auto holder = get_path_spline(*msg); - if ( holder.has_value()){ + if (holder.has_value()) { this->path_spline = holder.value(); - } // otherwise we do nothing! + } // otherwise we do nothing! } } @@ -388,7 +387,7 @@ std::vector PurePursuitNode::get_objects_from_s } // If our count is greater than 3 we have an object with at least 3 consecutive laser scan points - if (count >= 450) { //TODO make param + if (count >= 450) { //TODO make param obj_index_pairs[starting_index] = ending_index; // Add the starting and ending index to our map } } @@ -399,9 +398,11 @@ std::vector PurePursuitNode::get_objects_from_s // Create the point from the laser scan geometry_msgs::msg::PoseStamped point; point.header.frame_id = laser_scan->header.frame_id; - // TODO hardcoded for ISC sick angles - point.pose.position.x = laser_scan->ranges.at(i) * -cos(laser_scan->angle_increment * i + (45 * M_PI / 180)); - point.pose.position.y = laser_scan->ranges.at(i) * -sin(laser_scan->angle_increment * i + (45 * M_PI / 180)); + // TODO hardcoded for ISC sick angles + point.pose.position.x = + laser_scan->ranges.at(i) * -cos(laser_scan->angle_increment * i + (45 * M_PI / 180)); + point.pose.position.y = + laser_scan->ranges.at(i) * -sin(laser_scan->angle_increment * i + (45 * M_PI / 180)); // Add point to detected objects detected_points.push_back(point); @@ -412,10 +413,11 @@ std::vector PurePursuitNode::get_objects_from_s return detected_points; } -std::optional> PurePursuitNode::get_path_spline(const nav_msgs::msg::Path& path) { +std::optional> PurePursuitNode::get_path_spline( + const nav_msgs::msg::Path& path) { // Create a copy of the path to avoid mutating the main auto local_path = path; - + // std::vector auto local_objects = this->objects; @@ -436,7 +438,7 @@ std::optional> PurePursuitNode::get double& x = point1.pose.position.x; double& y = point1.pose.position.y; - if ( std::pow(std::pow(x,2) + std::pow(y,2),0.5) < look_ahead_distance){ + if (std::pow(std::pow(x, 2) + std::pow(y, 2), 0.5) < look_ahead_distance) { fails_to_exceed_look_ahead = false; } @@ -459,10 +461,10 @@ std::optional> PurePursuitNode::get } // why do i do this? // checks if look ahead distance does not ever exceed beyond look ahead distance - if (fails_to_exceed_look_ahead) return std::nullopt; - // stay on old path. - // Otherwise we will be provided a path that is impossible to follow - // because look ahead is what will choice the clsest point that also exceeds it. + if (fails_to_exceed_look_ahead) return std::nullopt; + // stay on old path. + // Otherwise we will be provided a path that is impossible to follow + // because look ahead is what will choice the clsest point that also exceeds it. // For each spline point, shift it if it is too close to an obstacle. This has the effect of wrapping the spline // around obstacles.