diff --git a/include/hybrid_pp/PurePursuitNode_node.hpp b/include/hybrid_pp/PurePursuitNode_node.hpp index 811b182..8af87ab 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 { @@ -71,6 +71,8 @@ class PurePursuitNode : public rclcpp::Node { float k_dd; /// Publishes visualisations if true bool debug; + // to reverse the path because I like -AFB + bool reverse_path_search; // Multithreading /// Set to true to kill thread @@ -117,9 +119,9 @@ class PurePursuitNode : public rclcpp::Node { /// 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; + //TODO hotfix to get left at comp + stop.steering_angle = 0.27; + stop.speed = 1.0; this->nav_ack_vel_pub->publish(stop); } diff --git a/src/PurePursuitNode_node.cpp b/src/PurePursuitNode_node.cpp index 6c64c9f..db3d8e1 100644 --- a/src/PurePursuitNode_node.cpp +++ b/src/PurePursuitNode_node.cpp @@ -23,6 +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); + reverse_path_search = this->declare_parameter("reverse_path_search", true); this->declare_parameter("stop_on_no_path", true); // Var init @@ -308,33 +309,46 @@ std::optional PurePursuitNode::get_path_point( geometry_msgs::msg::PoseStamped intercepted_pose{}; - // look ahead sanity check! - // max ahead point + // look ahead sanity check! + // max ahead point float max_distance = 0.0; - for (const auto& i : local_path){ - if (max_distance < std::abs(distance(i.pose.position, zero) )){ + for (const auto& i : local_path) { + if (max_distance < std::abs(distance(i.pose.position, zero))) { max_distance = std::abs(distance(i.pose.position, zero)); } } - if ( look_ahead_distance > max_distance ) { + if (look_ahead_distance > max_distance) { // do the thing I suppose look_ahead_distance = max_distance; } - if (look_ahead_distance < min_look_ahead_distance ){ + if (look_ahead_distance < min_look_ahead_distance) { // basically prevent lookahead from exceeding the minium distance ircc return std::nullopt; } // Find point on the spline that intersects our lookahead, must be in front of us bool point_found = false; - for (const auto& i : local_path) { - // The spline is somewhat sparse, so have some leeway in selecting points on it - if (i.pose.position.x >= 0 && std::abs(distance(i.pose.position, zero) - look_ahead_distance) <= 1.0) { - intercepted_pose = i; - point_found = true; - break; + if (reverse_path_search) { + for (auto it = local_path.rbegin(); it != local_path.rend(); ++it) { + const auto& i = *it; + // The spline is somewhat sparse, so have some leeway in selecting points on it + if (i.pose.position.x >= 0 && std::abs(distance(i.pose.position, zero) - look_ahead_distance) <= 1.0) { + intercepted_pose = i; + point_found = true; + break; + } + } + + } else { + for (const auto& i : local_path) { + // The spline is somewhat sparse, so have some leeway in selecting points on it + if (i.pose.position.x >= 0 && std::abs(distance(i.pose.position, zero) - look_ahead_distance) <= 1.0) { + intercepted_pose = i; + point_found = true; + break; + } } } @@ -385,7 +399,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 } } @@ -396,9 +410,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);