diff --git a/include/ap1/planning/planner_node.hpp b/include/ap1/planning/planner_node.hpp index f938728..c9b343e 100644 --- a/include/ap1/planning/planner_node.hpp +++ b/include/ap1/planning/planner_node.hpp @@ -16,6 +16,7 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64.hpp" #include #include #include @@ -78,6 +79,8 @@ class PlannerNode : public rclcpp::Node // Publishers rclcpp::Publisher::SharedPtr speed_profile_pub_; rclcpp::Publisher::SharedPtr target_path_pub_; + rclcpp::Publisher::SharedPtr speed_limit_pub_; + // # Callbacks void on_hd_map(const Float32MultiArray::SharedPtr); // WRONG TYPE SHOULD BE XML diff --git a/src/planner_node.cpp b/src/planner_node.cpp index ecc74cd..7d20aea 100644 --- a/src/planner_node.cpp +++ b/src/planner_node.cpp @@ -21,6 +21,8 @@ #include "geometry_msgs/msg/point.hpp" #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float32_multi_array.hpp" +#include "std_msgs/msg/float64.hpp" + #include "ap1_msgs/msg/speed_profile_stamped.hpp" #include "ap1_msgs/msg/target_path_stamped.hpp" @@ -36,6 +38,13 @@ using ap1_msgs::msg::VehicleSpeedStamped; using geometry_msgs::msg::Point; using std_msgs::msg::Float32MultiArray; +// ===================== SPEED PROFILE CONSTANTS ===================== // + +constexpr double MAX_SPEED_MPS = 6.0; // Global max speed +constexpr double MAX_LAT_ACC_MPS2 = 1.5; // Lateral accel limit (turning) +constexpr double MAX_FWD_ACC_MPS2 = 2.0; // Forward accel limit +constexpr double MIN_TURN_RADIUS = 0.01; // Avoid division by zero + namespace ap1::planning { PlannerNode::PlannerNode(double rate_hz) : Node("planner_node"), rate_hz_(rate_hz) @@ -55,11 +64,13 @@ PlannerNode::PlannerNode(double rate_hz) : Node("planner_node"), rate_hz_(rate_h target_speed_sub_ = this->create_subscription( "/ap1/control/target_speed", 10, std::bind(&PlannerNode::on_target_speed, this, std::placeholders::_1)); + // # Publishers speed_profile_pub_ = this->create_publisher("/ap1/planning/speed_profile", 10); target_path_pub_ = this->create_publisher("/ap1/planning/target_path", 10); + speed_limit_pub_ = this->create_publisher("/ap1/planning/_current_speed_limit", 10); // # Create Planning Loop @ rate_hz timer_ = this->create_wall_timer(std::chrono::duration(1.0f / rate_hz), @@ -256,6 +267,27 @@ TargetPathStamped PlannerNode::create_route() return path_msg; } +double computeCurvature(const geometry_msgs::msg::Point &p0, const geometry_msgs::msg::Point &p1, const geometry_msgs::msg::Point &p2){ + double x1 = p0.x, y1 = p0.y; + double x2 = p1.x, y2 = p1.y; + double x3 = p2.x, y3 = p2.y; + + double dx1 = x2 - x1; + double dy1 = y2 - y1; + double dx2 = x3 - x2; + double dy2 = y3 - y2; + + double cross = dx1 * dy2 - dy1 * dx2; + [[maybe_unused]] double dot = dx1 * dx2 + dy1 * dy2; + double denom = std::sqrt((dx1*dx1 + dy1*dy1) * (dx2*dx2 + dy2*dy2)); + + if (denom < 1e-6) + return 0.0; + + return std::abs(cross) / denom; +} + + SpeedProfileStamped PlannerNode::create_speed_profile() { SpeedProfileStamped speed_msg; @@ -263,20 +295,116 @@ SpeedProfileStamped PlannerNode::create_speed_profile() speed_msg.header.stamp = this->now(); speed_msg.header.frame_id = "map"; - // set the speed to the system's last recieved target speed - speed_msg.speeds = {this->speed_}; + // ======================================================================= + // 1. Get the path (same one sent in target_path_pub_) + // ======================================================================= + TargetPathStamped route = create_route(); + const auto &pts = route.path; + + std::vector speeds; + speeds.resize(pts.size(), MAX_SPEED_MPS); + + // ======================================================================= + // 2. Dynamic turn-based speed profiling + // ======================================================================= + double last_speed = this->speed_; // previously commanded speed + double dt = 0.1; // assume 10 Hz planning loop + + for (size_t i = 1; i + 1 < pts.size(); i++) + { + const auto &p0 = pts[i - 1]; + const auto &p1 = pts[i]; + const auto &p2 = pts[i + 1]; + + // ---- curvature → radius ------------------------------------------ + double curvature = computeCurvature(p0, p1, p2); + double R = (curvature == 0.0) ? 1e9 : 1.0 / curvature; + + // ---- lateral-acceleration speed limit ----------------------------- + double turn_speed_limit = std::sqrt(MAX_LAT_ACC_MPS2 * std::max(R, MIN_TURN_RADIUS)); + + // ---- combine with global limit ------------------------------------ + double target_speed = std::min(MAX_SPEED_MPS, turn_speed_limit); + + // ---- forward acceleration limit ----------------------------------- + double dv = target_speed - last_speed; + double max_dv = MAX_FWD_ACC_MPS2 * dt; + + if (dv > max_dv) target_speed = last_speed + max_dv; + if (dv < -max_dv) target_speed = last_speed - max_dv; + + // ---- final safety clamp ------------------------------------------- + if (target_speed > MAX_SPEED_MPS) + target_speed = MAX_SPEED_MPS; + + double lat_acc = (target_speed * target_speed) / std::max(R, MIN_TURN_RADIUS); + if (lat_acc > MAX_LAT_ACC_MPS2) + target_speed = std::sqrt(MAX_LAT_ACC_MPS2 * R); + + speeds[i] = target_speed; + last_speed = target_speed; + } + + // Set first/last speeds + speeds.front() = std::min(MAX_SPEED_MPS, static_cast(this->speed_)); + speeds.back() = speeds[speeds.size() - 2]; + + // ------------------------------- +// FINAL SAFETY CHECK (required) +// ------------------------------- +for (size_t i = 1; i + 1 < speeds.size(); i++) +{ + double v = speeds[i]; + + if (v > MAX_SPEED_MPS) + v = MAX_SPEED_MPS; + + const auto &p0 = pts[i - 1]; + const auto &p1 = pts[i]; + const auto &p2 = pts[i + 1]; + + double curvature = computeCurvature(p0, p1, p2); + double R = (curvature == 0.0) ? 1e9 : 1.0 / curvature; + double max_v_turn = std::sqrt(MAX_LAT_ACC_MPS2 * R); + + if (v > max_v_turn) + v = max_v_turn; + + speeds[i] = v; +} + + speed_msg.speeds.clear(); + speed_msg.speeds.reserve(speeds.size()); + + for (double v : speeds) { + speed_msg.speeds.push_back(static_cast(v)); + } return speed_msg; } + void PlannerNode::planning_loop_callback() { // no log message for each loop - // send route msg to ctrl - target_path_pub_->publish(create_route()); - - // send speed msg to ctrl - speed_profile_pub_->publish(create_speed_profile()); + // 1. Create route + auto route = create_route(); + target_path_pub_->publish(route); + + // 2. Create dynamic speed profile + auto speed_profile = create_speed_profile(); + speed_profile_pub_->publish(speed_profile); + + // 3. Publish current speed limit (for UI) + // Use the first waypoint's computed limit. + std_msgs::msg::Float64 limit_msg; + if (!speed_profile.speeds.empty()) { + limit_msg.data = speed_profile.speeds.front(); + } else { + limit_msg.data = MAX_SPEED_MPS; // fallback + } + speed_limit_pub_->publish(limit_msg); } + } // namespace ap1::planning