2025 comp dev

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 {
@@ -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<PathCalcResult> get_path_point(const std::vector<geometry_msgs::msg::PoseStamped>& path_spline);
     /// Creates a spline from the path recived from path planner and also checks if it needs to avoid objects
-    std::vector<geometry_msgs::msg::PoseStamped> get_path_spline(const nav_msgs::msg::Path& path);
+    std::optional<std::vector<geometry_msgs::msg::PoseStamped>> get_path_spline(const nav_msgs::msg::Path& path);
 
     ~PurePursuitNode() override { this->stop_token.store(true); }
 };

src/PurePursuitNode_node.cpp

@@ -17,13 +17,13 @@ PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions& options)
     max_look_ahead_distance = this->declare_parameter<float>("max_look_ahead_distance", 10.0);
     k_dd = this->declare_parameter<float>("k_dd", 1.0);
     max_speed = this->declare_parameter<float>("max_speed", 6.7056);
-    max_speed = this->declare_parameter<float>("min_speed", 0.5);
+    min_speed = this->declare_parameter<float>("min_speed", 0.5);
     avoidance_radius = this->declare_parameter<float>("avoidance_radius", 2);
     rear_axle_frame = this->declare_parameter<std::string>("rear_axle_frame", "rear_axle");
     wheel_base = this->declare_parameter<float>("wheel_base", 1.08);
     gravity_constant = this->declare_parameter<float>("gravity_constant", 9.81);
     debug = this->declare_parameter<bool>("debug", true);
-    this->declare_parameter<bool>("stop_on_no_path", true);
+    this->declare_parameter<bool>("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<PathCalcResult> path_result;
+            std::optional<PathCalcResult> last_path_result;
             // Visualization components for publishing visualization
             VisualizationComponents vis_components{};
 
@@ -74,11 +75,28 @@ 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 +148,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!
     }
 }
 
@@ -366,7 +387,7 @@ std::vector<geometry_msgs::msg::PoseStamped> 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
         }
     }
@@ -377,9 +398,11 @@ std::vector<geometry_msgs::msg::PoseStamped> 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);
@@ -390,9 +413,12 @@ std::vector<geometry_msgs::msg::PoseStamped> PurePursuitNode::get_objects_from_s
     return detected_points;
 }
 
-std::vector<geometry_msgs::msg::PoseStamped> PurePursuitNode::get_path_spline(const nav_msgs::msg::Path& path) {
+std::optional<std::vector<geometry_msgs::msg::PoseStamped>> 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<geometry_msgs::msg::PoseStamped>
     auto local_objects = this->objects;
 
     // Transform odom->rear-axle, for easy calculations
@@ -403,24 +429,42 @@ std::vector<geometry_msgs::msg::PoseStamped> PurePursuitNode::get_path_spline(co
     // Spline connecting our path points
     std::vector<geometry_msgs::msg::PoseStamped> spline;
 
+    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++) {
         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) {
+            fails_to_exceed_look_ahead = 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);
+        /*
+        // 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);
-        }
+        }*/
     }
+    // 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.