diff --git a/src/launcher/launch/pacsim-testing.launch.py b/src/launcher/launch/pacsim-testing.launch.py index 7eb008d64..ffd103af2 100644 --- a/src/launcher/launch/pacsim-testing.launch.py +++ b/src/launcher/launch/pacsim-testing.launch.py @@ -63,6 +63,6 @@ def generate_launch_description(): # evaluator_launch_description, planning_launch_description, control_launch_description, - mocker_node_launch_description, + #mocker_node_launch_description, ], ) diff --git a/src/planning/CMakeLists.txt b/src/planning/CMakeLists.txt index 6cc47cd66..72a626b8a 100644 --- a/src/planning/CMakeLists.txt +++ b/src/planning/CMakeLists.txt @@ -41,6 +41,7 @@ set(TESTFILES test/smoothing_tests.cpp test/splines_tests.cpp test/test_utils/utils.cpp + test/velocity_planning_tests.cpp ) set(IMPLEMENTATION_FILES @@ -53,6 +54,7 @@ set(IMPLEMENTATION_FILES src/planning/smoothing.cpp src/planning/outliers.cpp src/planning/planning.cpp + src/planning/velocity_planning.cpp src/utils/cone.cpp src/utils/files.cpp ) diff --git a/src/planning/include/adapter_planning/parameters_factory.hpp b/src/planning/include/adapter_planning/parameters_factory.hpp index 74e780fb3..7353e6853 100644 --- a/src/planning/include/adapter_planning/parameters_factory.hpp +++ b/src/planning/include/adapter_planning/parameters_factory.hpp @@ -45,6 +45,11 @@ std::string load_adapter_parameters(PlanningParameters& params) { static_cast(adapter_node->declare_parameter("pre_defined_velocity_planning", 0.5)); params.use_outlier_removal_ = adapter_node->declare_parameter("use_outlier_removal", false); params.use_path_smoothing_ = adapter_node->declare_parameter("use_path_smoothing", true); + params.minimum_velocity_ = adapter_node->declare_parameter("minimum_velocity", 3.0); + params.braking_acceleration_ = adapter_node->declare_parameter("braking_acceleration", -2.0); + params.normal_acceleration_ = adapter_node->declare_parameter("normal_acceleration", 6.0); + params.use_velocity_planning_ = adapter_node->declare_parameter("use_velocity_planning", true); + std::string adapter_type = adapter_node->declare_parameter("adapter", "vehicle"); params.map_frame_id_ = adapter_type == "eufs" ? "base_footprint" : "map"; diff --git a/src/planning/include/config/planning_config.hpp b/src/planning/include/config/planning_config.hpp index 44f0f0857..9d5586b7c 100644 --- a/src/planning/include/config/planning_config.hpp +++ b/src/planning/include/config/planning_config.hpp @@ -6,6 +6,7 @@ #include "path_calculation_config.hpp" #include "simulation_config.hpp" #include "smoothing_config.hpp" +#include "velocity_config.hpp" struct PlanningParameters { double angle_gain_; @@ -28,6 +29,10 @@ struct PlanningParameters { bool use_outlier_removal_; bool use_path_smoothing_; double desired_velocity_; + double minimum_velocity_; + double braking_acceleration_; + double normal_acceleration_; + bool use_velocity_planning_; std::string map_frame_id_; }; @@ -41,6 +46,7 @@ struct PlanningConfig { PathCalculationConfig path_calculation_; PathSmoothingConfig smoothing_; SimulationConfig simulation_; + VelocityPlanningConfig velocity_planning_; PlanningConfig() = default; explicit PlanningConfig(const PlanningParameters ¶ms) { @@ -68,6 +74,11 @@ struct PlanningConfig { simulation_.publishing_visualization_msgs_ = params.publishing_visualization_msgs_; simulation_.using_simulated_se_ = params.using_simulated_se_; + + velocity_planning_.minimum_velocity_ = params.minimum_velocity_; + velocity_planning_.braking_acceleration_ = params.braking_acceleration_; + velocity_planning_.normal_acceleration_ = params.normal_acceleration_; + velocity_planning_.use_velocity_planning_ = params.use_velocity_planning_; } }; diff --git a/src/planning/include/config/velocity_config.hpp b/src/planning/include/config/velocity_config.hpp new file mode 100644 index 000000000..5cd11b38f --- /dev/null +++ b/src/planning/include/config/velocity_config.hpp @@ -0,0 +1,37 @@ +#ifndef SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_ +#define SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_ + +/** + * @brief struct for the configuration of the velocity planning algorithm + * + */ +struct VelocityPlanningConfig { + /** + * @brief minimum speed the car will consider on the velocity planning lookahead + * + */ + double minimum_velocity_ = 3; + /** + * @brief maximum braking acceleration + * + */ + double braking_acceleration_ = -4; + /** + * @brief the maximum normal acceleration + * + */ + double normal_acceleration_ = 7; + /** + * @brief flag to enable/disable velocity planning + * + */ + bool use_velocity_planning_ = true; + VelocityPlanningConfig() = default; + VelocityPlanningConfig(double minimum_velocity, double braking_acc, double normal_acc, bool use_velocity_planning) + : minimum_velocity_(minimum_velocity), + braking_acceleration_(braking_acc), + normal_acceleration_(normal_acc), + use_velocity_planning_(use_velocity_planning) {} +}; + +#endif // SRC_PLANNING_INCLUDE_CONFIG_VELOCITY_CONFIG_HPP_ \ No newline at end of file diff --git a/src/planning/include/planning/path_calculation.hpp b/src/planning/include/planning/path_calculation.hpp index d12863cbd..5dcaff54a 100644 --- a/src/planning/include/planning/path_calculation.hpp +++ b/src/planning/include/planning/path_calculation.hpp @@ -13,6 +13,7 @@ #include "common_lib/structures/path_point.hpp" #include "config/path_calculation_config.hpp" #include "rclcpp/rclcpp.hpp" +#include "common_lib/structures/pose.hpp" // Add this line to include Pose definition using K = CGAL::Exact_predicates_inexact_constructions_kernel; using DT = CGAL::Delaunay_triangulation_2; @@ -34,6 +35,10 @@ class PathCalculation { */ PathCalculationConfig config_; +private: + bool path_orientation_corrected_ = false; // TODO: Put in Skidpad class + std::vector predefined_path_; // TODO: Put in Skidpad class + public: /** * @brief Construct a new default PathCalculation object @@ -62,6 +67,8 @@ class PathCalculation { */ std::vector process_delaunay_triangulations( std::pair, std::vector> refined_cones) const; + + std::vector skidpad_path(std::vector& cone_array, common_lib::structures::Pose pose); }; #endif // SRC_PLANNING_PLANNING_INCLUDE_PLANNING_PATH_CALCULATION_HPP_ diff --git a/src/planning/include/planning/planning.hpp b/src/planning/include/planning/planning.hpp index 8b0f755f8..74f1fdb64 100644 --- a/src/planning/include/planning/planning.hpp +++ b/src/planning/include/planning/planning.hpp @@ -22,6 +22,7 @@ #include "planning/outliers.hpp" #include "planning/path_calculation.hpp" #include "planning/smoothing.hpp" +#include "planning/velocity_planning.hpp" #include "sensor_msgs/msg/point_cloud2.hpp" #include "std_msgs/msg/float64.hpp" #include "utils/files.hpp" @@ -49,6 +50,7 @@ class Planning : public rclcpp::Node { Outliers outliers_; PathCalculation path_calculation_; PathSmoothing path_smoothing_; + VelocityPlanning velocity_planning_; double desired_velocity_; double initial_car_orientation_; diff --git a/src/planning/include/planning/velocity_planning.hpp b/src/planning/include/planning/velocity_planning.hpp new file mode 100644 index 000000000..dd1999845 --- /dev/null +++ b/src/planning/include/planning/velocity_planning.hpp @@ -0,0 +1,65 @@ +#ifndef SRC_PLANNING_INCLUDE_PLANNING_VELOCITY_HPP_ +#define SRC_PLANNING_INCLUDE_PLANNING_VELOCITY_HPP_ + +#include + +#include "common_lib/structures/path_point.hpp" +#include "config/velocity_config.hpp" + +using PathPoint = common_lib::structures::PathPoint; + +/** + * @brief class that defines the path smoothing algorithm + * + */ +class VelocityPlanning { +private: + /** + * @brief configuration of the smoothing algorithm + * + */ + VelocityPlanningConfig config_; + + /** + * @brief function to calculate the radius of the circle that passes through 3 points + * + * @param point1 first point + * @param point2 second point + * @param point3 third point + * @return radius of the circle + */ + double find_circle_center(PathPoint &point1, PathPoint &point2, PathPoint &point3); + + /** + * @brief function to limit the speed of the car according to the curvature of the lookahead path + * + * @param points path points + * @param velocities velocities of the path points + * @param brake_acelleration maximum braking acelleration + */ + void speed_limiter(std::vector &points, std::vector &velocities); + + /** + * @brief function to calculate the speed of the car according to the curvature of the path + * + * @param radiuses radiuses vector of the path points + * @param velocities velocities vector of the path points + */ + void point_speed(std::vector &radiuses, std::vector &velocities); + +public: + /** + * @brief Construct a new default Path Smoothing object + * + */ + VelocityPlanning() = default; + /** + * @brief Construct a new Path Smoothing object with a given configuration + * + */ + explicit VelocityPlanning(VelocityPlanningConfig config) : config_(config) {} + + void set_velocity(std::vector &final_path); +}; + +#endif // SRC_PLANNING_INCLUDE_PLANNING_SMOOTHING2_HPP_ \ No newline at end of file diff --git a/src/planning/launch/eufs.launch.py b/src/planning/launch/eufs.launch.py index 6f235dff1..f1403a7f4 100644 --- a/src/planning/launch/eufs.launch.py +++ b/src/planning/launch/eufs.launch.py @@ -115,6 +115,26 @@ def generate_launch_description(): description="Whether to use path smoothing or to skip it", default_value="true", ), + DeclareLaunchArgument( + "minimum_velocity", + description="Minimum speed for velocity planning", + default_value="3.0", + ), + DeclareLaunchArgument( + "braking_acceleration", + description="Braking acceleration to consider in velocity planning", + default_value="-4.0", + ), + DeclareLaunchArgument( + "normal_acceleration", + description="Normal acceleration to consider in velocity planning", + default_value="7.0", + ), + DeclareLaunchArgument( + "use_velocity_planning", + description="Whether to use velocity planning or to skip it", + default_value="true", + ), Node( package="planning", executable="planning", @@ -185,6 +205,10 @@ def generate_launch_description(): }, {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")}, {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")}, + {"minimum_velocity": LaunchConfiguration("minimum_velocity")}, + {"braking_acceleration": LaunchConfiguration("braking_acceleration")}, + {"normal_acceleration": LaunchConfiguration("normal_acceleration")}, + {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")}, ], arguments=["--ros-args", "--log-level", "planning:=info"], ), diff --git a/src/planning/launch/fsds.launch.py b/src/planning/launch/fsds.launch.py index 46ff08b76..12ed163bf 100644 --- a/src/planning/launch/fsds.launch.py +++ b/src/planning/launch/fsds.launch.py @@ -115,6 +115,26 @@ def generate_launch_description(): description="Whether to use path smoothing or to skip it", default_value="true", ), + DeclareLaunchArgument( + "minimum_velocity", + description="Minimum speed for velocity planning", + default_value="3.0", + ), + DeclareLaunchArgument( + "braking_acceleration", + description="Braking acceleration to consider in velocity planning", + default_value="-4.0", + ), + DeclareLaunchArgument( + "normal_acceleration", + description="Normal acceleration to consider in velocity planning", + default_value="7.0", + ), + DeclareLaunchArgument( + "use_velocity_planning", + description="Whether to use velocity planning or to skip it", + default_value="true", + ), Node( package="planning", executable="planning", @@ -185,6 +205,10 @@ def generate_launch_description(): } {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")}, {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")}, + {"minimum_velocity": LaunchConfiguration("minimum_velocity")}, + {"braking_acceleration": LaunchConfiguration("braking_acceleration")}, + {"normal_acceleration": LaunchConfiguration("normal_acceleration")}, + {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")}, ], arguments=["--ros-args", "--log-level", "planning:=info"], ), diff --git a/src/planning/launch/pacsim.launch.py b/src/planning/launch/pacsim.launch.py index c50bb6782..d37fc9aa7 100644 --- a/src/planning/launch/pacsim.launch.py +++ b/src/planning/launch/pacsim.launch.py @@ -115,6 +115,26 @@ def generate_launch_description(): description="Whether to use path smoothing or to skip it", default_value="true", ), + DeclareLaunchArgument( + "minimum_velocity", + description="Minimum speed for velocity planning", + default_value="2.0", + ), + DeclareLaunchArgument( + "braking_acceleration", + description="Braking acceleration to consider in velocity planning", + default_value="-1.0", + ), + DeclareLaunchArgument( + "normal_acceleration", + description="Normal acceleration to consider in velocity planning", + default_value="4.0", + ), + DeclareLaunchArgument( + "use_velocity_planning", + description="Whether to use velocity planning or to skip it", + default_value="true", + ), Node( package="planning", executable="planning", @@ -185,6 +205,10 @@ def generate_launch_description(): }, {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")}, {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")}, + {"minimum_velocity": LaunchConfiguration("minimum_velocity")}, + {"braking_acceleration": LaunchConfiguration("braking_acceleration")}, + {"normal_acceleration": LaunchConfiguration("normal_acceleration")}, + {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")}, ], arguments=["--ros-args", "--log-level", "planning:=info"], ), diff --git a/src/planning/launch/vehicle.launch.py b/src/planning/launch/vehicle.launch.py index 64f8fc664..a8fa0c7c7 100644 --- a/src/planning/launch/vehicle.launch.py +++ b/src/planning/launch/vehicle.launch.py @@ -115,6 +115,26 @@ def generate_launch_description(): description="Whether to use path smoothing or to skip it", default_value="true", ), + DeclareLaunchArgument( + "minimum_velocity", + description="Minimum speed for velocity planning", + default_value="3.0", + ), + DeclareLaunchArgument( + "braking_acceleration", + description="Braking acceleration to consider in velocity planning", + default_value="-4.0", + ), + DeclareLaunchArgument( + "normal_acceleration", + description="Normal acceleration to consider in velocity planning", + default_value="7.0", + ), + DeclareLaunchArgument( + "use_velocity_planning", + description="Whether to use velocity planning or to skip it", + default_value="true", + ), Node( package="planning", executable="planning", @@ -185,6 +205,10 @@ def generate_launch_description(): }, {"use_outlier_removal": LaunchConfiguration("use_outlier_removal")}, {"use_path_smoothing": LaunchConfiguration("use_path_smoothing")}, + {"minimum_velocity": LaunchConfiguration("minimum_velocity")}, + {"braking_acceleration": LaunchConfiguration("braking_acceleration")}, + {"normal_acceleration": LaunchConfiguration("normal_acceleration")}, + {"use_velocity_planning": LaunchConfiguration("use_velocity_planning")}, ], arguments=["--ros-args", "--log-level", "planning:=info"], ), diff --git a/src/planning/src/planning/path_calculation.cpp b/src/planning/src/planning/path_calculation.cpp index 1a2bc10f5..4652e3708 100644 --- a/src/planning/src/planning/path_calculation.cpp +++ b/src/planning/src/planning/path_calculation.cpp @@ -26,7 +26,7 @@ std::vector PathCalculation::process_delaunay_triangulations( // Create a Delaunay triangulation DT dt; // Insert cones coordinates into the Delaunay triangulation - for (const Cone& cone : cones) { + for (const Cone &cone : cones) { dt.insert(Point(cone.position.x, cone.position.y)); } @@ -68,3 +68,71 @@ std::vector PathCalculation::process_delaunay_triangulations( return unordered_path; } + +std::vector PathCalculation::skidpad_path(std::vector &cone_array, + common_lib::structures::Pose pose) { + if (!path_orientation_corrected_) { + std::string file = "./src/planning/src/utils/skidpad.txt"; // -------------------- + // open and read file line by line + std::ifstream infile(file); + std::string line; + std::vector hardcoded_path_; + while (std::getline(infile, line)) { + std::istringstream iss(line); + double x, y, v; + if (!(iss >> x >> y >> v)) { + break; + } // error + hardcoded_path_.push_back(PathPoint(x, y, v)); + } + + + if (cone_array.size() < 4) { + throw std::runtime_error("Insufficient cones to calculate path"); // -------------------- + } + // sort the cones by distance + std::sort(cone_array.begin(), cone_array.end(), [&pose](Cone &a, Cone &b) { + return sqrt(pow((a.position.x - pose.position.x), 2) + + pow((a.position.y - pose.position.y), 2)) < + sqrt(pow((b.position.x - pose.position.x), 2) + + pow((b.position.y - pose.position.y), 2)); + }); + + // get the middle point between the two closest points + PathPoint middle_closest = + PathPoint((cone_array[0].position.x + cone_array[1].position.x) / 2, + (cone_array[0].position.y + cone_array[1].position.y) / 2, 0); + + // get the middle point between the two farthest points + PathPoint middle_farthest = + PathPoint((cone_array[2].position.x + cone_array[3].position.x) / 2, + (cone_array[2].position.y + cone_array[3].position.y) / 2, 0); + + // Find the slope of the line between the two middle points + double dx = middle_farthest.position.x - middle_closest.position.x; // -------------------- + double dy = middle_farthest.position.y - middle_closest.position.y; // -------------------- + double slope = (dx != 0) ? (dy / dx) : 10000000000; // -------------------- + + // calculate the angle + double angle = atan(slope); + + // rotate all the points by the angle and add the origin point + for (auto &point : hardcoded_path_) { + double x = point.position.x; + double y = point.position.y; + point.position.x = x * cos(angle) - y * sin(angle) + middle_closest.position.x; + point.position.y = x * sin(angle) + y * cos(angle) + middle_closest.position.y; + } + + predefined_path_ = hardcoded_path_; + path_orientation_corrected_ = true; + } + + while (!predefined_path_.empty() && + pose.position.euclidean_distance(predefined_path_[0].position) < 1) { + predefined_path_.erase(predefined_path_.begin()); + } + + // set it as the final path + return std::vector(predefined_path_.begin(), predefined_path_.begin() + 70); +} diff --git a/src/planning/src/planning/planning.cpp b/src/planning/src/planning/planning.cpp index 1801be045..0822d8e3c 100644 --- a/src/planning/src/planning/planning.cpp +++ b/src/planning/src/planning/planning.cpp @@ -1,5 +1,7 @@ #include "planning/planning.hpp" +#include + #include "adapter_planning/pacsim.hpp" #include "adapter_planning/vehicle.hpp" @@ -14,6 +16,7 @@ Planning::Planning(const PlanningParameters ¶ms) outliers_ = Outliers(planning_config_.outliers_); path_calculation_ = PathCalculation(planning_config_.path_calculation_); path_smoothing_ = PathSmoothing(planning_config_.smoothing_); + velocity_planning_ = VelocityPlanning(planning_config_.velocity_planning_); // Control Publisher this->local_pub_ = @@ -134,10 +137,25 @@ void Planning::run_planning_algorithms() { RCLCPP_INFO(rclcpp::get_logger("planning"), "Final path size: %d", static_cast(final_path.size())); } - // Velocity Planning - // TODO: Remove this when velocity planning is a reality - for (auto &path_point : final_path) { - path_point.ideal_velocity = desired_velocity_; + + if ((this->mission == common_lib::competition_logic::Mission::SKIDPAD)) { // place a ! before the condition, to test skidpad until the simulator publishes the mission correctly + final_path = path_calculation_.skidpad_path(this->cone_array_, this->pose); + } + + if ((this->mission == common_lib::competition_logic::Mission::ACCELERATION)) { // place a ! before the condition, to test acceleration until the simulator publishes the mission correctly + double dist_from_origin = sqrt(this->pose.position.x * this->pose.position.x + + this->pose.position.y * this->pose.position.y); + if (dist_from_origin > 80.0) { + for (auto &point : final_path) { + point.ideal_velocity = 0.0; + } + } else { + for (auto &point : final_path) { + point.ideal_velocity = 1000.0; + } + } + } else if (!(this->mission == common_lib::competition_logic::Mission::SKIDPAD)) { // remove the ! before the condition, to test skidpad until the simulator publishes the mission correctly + velocity_planning_.set_velocity(final_path); } // Execution Time calculation diff --git a/src/planning/src/planning/velocity_planning.cpp b/src/planning/src/planning/velocity_planning.cpp new file mode 100644 index 000000000..0379266c0 --- /dev/null +++ b/src/planning/src/planning/velocity_planning.cpp @@ -0,0 +1,86 @@ +#include "planning/velocity_planning.hpp" + +double VelocityPlanning::find_circle_center(PathPoint &point1, PathPoint &point2, + PathPoint &point3) { + double x1 = point1.position.x; + double y1 = point1.position.y; + double x2 = point2.position.x; + double y2 = point2.position.y; + double x3 = point3.position.x; + double y3 = point3.position.y; + + PathPoint mid1 = PathPoint((x1 + x2) / 2, (y1 + y2) / 2, 0); + PathPoint mid2 = PathPoint((x2 + x3) / 2, (y2 + y3) / 2, 0); + double slope1 = (x2 != x1) ? ((y2 - y1) / (x2 - x1)) : MAXFLOAT; + double slope2 = (x3 != x2) ? ((y3 - y2) / (x3 - x2)) : MAXFLOAT; + double slope1_perpendicular = (slope1 != 0) ? -1 / slope1 : 10000; + double slope2_perpendicular = (slope2 != 0) ? -1 / slope2 : 10000; + if (slope1_perpendicular == slope2_perpendicular) return 10000; + + double center_x = (slope1_perpendicular * mid1.position.x - + slope2_perpendicular * mid2.position.x + mid2.position.y - mid1.position.y) / + (slope1_perpendicular - slope2_perpendicular); + double center_y = slope1_perpendicular * (center_x - mid1.position.x) + mid1.position.y; + double radius = sqrt(pow(center_x - x2, 2) + pow(center_y - y2, 2)); + return radius; +} + +void VelocityPlanning::point_speed(std::vector &radiuses, std::vector &velocities) { + for (int i = 0; i < static_cast(radiuses.size()); i++) { + double velocity = sqrt(abs(config_.normal_acceleration_ * radiuses[i])); + velocities.push_back(std::max(velocity, config_.minimum_velocity_)); + } + velocities.back() = config_.minimum_velocity_; + + return; +} + +void VelocityPlanning::speed_limiter(std::vector &points, + std::vector &velocities) { + for (int i = static_cast(points.size()) - 2; i >= 0; i--) { + double distance = 0; + double max_speed = velocities[i]; + + //for (int j = i + 1; j < static_cast(points.size()) - 1; j++) { + // Calculate segment distance + int j = i+1; + distance = sqrt(pow(points[j].position.x - points[j - 1].position.x, 2) + + pow(points[j].position.y - points[j - 1].position.y, 2)); + + // Correct kinematic speed calculation + // v_f² = v_i² + 2ad + double max_terminal_speed = + sqrt(std::max(0.0, pow(velocities[j], 2) - 2 * config_.braking_acceleration_ * distance)); + + max_speed = std::min(max_speed, max_terminal_speed); + //} + velocities[i] = max_speed; + } +} + +// Main function to set the velocity of the car +void VelocityPlanning::set_velocity(std::vector &final_path) { + if ((config_.use_velocity_planning_) && (final_path.size() > 2)) { + std::vector radiuses; + radiuses.push_back(0); + for (int i = 1; i < static_cast(final_path.size()) - 1; i++) { + radiuses.push_back(find_circle_center(final_path[i - 1], final_path[i], final_path[i + 1])); + } + radiuses[0] = radiuses[1]; + radiuses.push_back(radiuses.back()); + + std::vector velocities; + point_speed(radiuses, velocities); + speed_limiter(final_path, velocities); + + for (int i = 0; i < static_cast(final_path.size()); i++) { + final_path[i].ideal_velocity = velocities[i]; + } + } + + else { + for (auto &path_point : final_path) { + path_point.ideal_velocity = config_.minimum_velocity_; + } + } +} \ No newline at end of file diff --git a/src/planning/src/utils/skidpad.txt b/src/planning/src/utils/skidpad.txt new file mode 100644 index 000000000..020700199 --- /dev/null +++ b/src/planning/src/utils/skidpad.txt @@ -0,0 +1,490 @@ +0.000 0 6 +0.500 0 6 +1.000 0 6 +1.500 0 6 +2.000 0 6 +2.500 0 6 +3.000 0 6 +3.500 0 6 +4.000 0 6 +4.500 0 6 +5.000 0 6 +5.500 0 6 +6.000 0 6 +6.500 0 6 +7.000 0 6 +7.500 0 6 +8.000 0 6 +8.500 0 6 +9.000 0 6 +9.500 0 6 +10.000 0 6 +10.500 0 6 +11.000 0 6 +11.500 0 6 +12.000 0 6 +12.500 0 6 +13.000 0 6 +13.500 0 6 +14.000 0 6 +14.500 0 6 +15.000 -0.000 5 +15.573 -0.018 5 +16.144 -0.072 5 +16.710 -0.162 5 +17.269 -0.287 5 +17.820 -0.447 5 +18.359 -0.641 5 +18.885 -0.868 5 +19.396 -1.129 5 +19.889 -1.421 5 +20.364 -1.743 5 +20.816 -2.094 5 +21.246 -2.473 5 +21.652 -2.879 5 +22.031 -3.309 5 +22.382 -3.761 5 +22.704 -4.236 5 +22.996 -4.729 5 +23.257 -5.240 5 +23.484 -5.766 5 +23.678 -6.305 5 +23.838 -6.856 5 +23.963 -7.415 5 +24.053 -7.981 5 +24.107 -8.552 5 +24.125 -9.125 5 +24.107 -9.698 5 +24.053 -10.269 5 +23.963 -10.835 5 +23.838 -11.394 5 +23.678 -11.945 5 +23.484 -12.484 5 +23.257 -13.010 5 +22.996 -13.521 5 +22.704 -14.014 5 +22.382 -14.489 5 +22.031 -14.941 5 +21.652 -15.371 5 +21.246 -15.777 5 +20.816 -16.156 5 +20.364 -16.507 5 +19.889 -16.829 5 +19.396 -17.121 5 +18.885 -17.382 5 +18.359 -17.609 5 +17.820 -17.803 5 +17.269 -17.963 5 +16.710 -18.088 5 +16.144 -18.178 5 +15.573 -18.232 5 +15.000 -18.250 5 +14.427 -18.232 5 +13.856 -18.178 5 +13.290 -18.088 5 +12.731 -17.963 5 +12.180 -17.803 5 +11.641 -17.609 5 +11.115 -17.382 5 +10.604 -17.121 5 +10.111 -16.829 5 +9.636 -16.507 5 +9.184 -16.156 5 +8.754 -15.777 5 +8.348 -15.371 5 +7.969 -14.941 5 +7.618 -14.489 5 +7.296 -14.014 5 +7.004 -13.521 5 +6.743 -13.010 5 +6.516 -12.484 5 +6.322 -11.945 5 +6.162 -11.394 5 +6.037 -10.835 5 +5.947 -10.269 5 +5.893 -9.698 5 +5.875 -9.125 5 +5.893 -8.552 5 +5.947 -7.981 5 +6.037 -7.415 5 +6.162 -6.856 5 +6.322 -6.305 5 +6.516 -5.766 5 +6.743 -5.240 5 +7.004 -4.729 5 +7.296 -4.236 5 +7.618 -3.761 5 +7.969 -3.309 5 +8.348 -2.879 5 +8.754 -2.473 5 +9.184 -2.094 5 +9.636 -1.743 5 +10.111 -1.421 5 +10.604 -1.129 5 +11.115 -0.868 5 +11.641 -0.641 5 +12.180 -0.447 5 +12.731 -0.287 5 +13.290 -0.162 5 +13.856 -0.072 5 +14.427 -0.018 5 +15.000 -0.000 5 +15.573 -0.018 5 +16.144 -0.072 5 +16.710 -0.162 5 +17.269 -0.287 5 +17.820 -0.447 5 +18.359 -0.641 5 +18.885 -0.868 5 +19.396 -1.129 5 +19.889 -1.421 5 +20.364 -1.743 5 +20.816 -2.094 5 +21.246 -2.473 5 +21.652 -2.879 5 +22.031 -3.309 5 +22.382 -3.761 5 +22.704 -4.236 5 +22.996 -4.729 5 +23.257 -5.240 5 +23.484 -5.766 5 +23.678 -6.305 5 +23.838 -6.856 5 +23.963 -7.415 5 +24.053 -7.981 5 +24.107 -8.552 5 +24.125 -9.125 5 +24.107 -9.698 5 +24.053 -10.269 5 +23.963 -10.835 5 +23.838 -11.394 5 +23.678 -11.945 5 +23.484 -12.484 5 +23.257 -13.010 5 +22.996 -13.521 5 +22.704 -14.014 5 +22.382 -14.489 5 +22.031 -14.941 5 +21.652 -15.371 5 +21.246 -15.777 5 +20.816 -16.156 5 +20.364 -16.507 5 +19.889 -16.829 5 +19.396 -17.121 5 +18.885 -17.382 5 +18.359 -17.609 5 +17.820 -17.803 5 +17.269 -17.963 5 +16.710 -18.088 5 +16.144 -18.178 5 +15.573 -18.232 5 +15.000 -18.250 5 +14.427 -18.232 5 +13.856 -18.178 5 +13.290 -18.088 5 +12.731 -17.963 5 +12.180 -17.803 5 +11.641 -17.609 5 +11.115 -17.382 5 +10.604 -17.121 5 +10.111 -16.829 5 +9.636 -16.507 5 +9.184 -16.156 5 +8.754 -15.777 5 +8.348 -15.371 5 +7.969 -14.941 5 +7.618 -14.489 5 +7.296 -14.014 5 +7.004 -13.521 5 +6.743 -13.010 5 +6.516 -12.484 5 +6.322 -11.945 5 +6.162 -11.394 5 +6.037 -10.835 5 +5.947 -10.269 5 +5.893 -9.698 5 +5.875 -9.125 5 +5.893 -8.552 5 +5.947 -7.981 5 +6.037 -7.415 5 +6.162 -6.856 5 +6.322 -6.305 5 +6.516 -5.766 5 +6.743 -5.240 5 +7.004 -4.729 5 +7.296 -4.236 5 +7.618 -3.761 5 +7.969 -3.309 5 +8.348 -2.879 5 +8.754 -2.473 5 +9.184 -2.094 5 +9.636 -1.743 5 +10.111 -1.421 5 +10.604 -1.129 5 +11.115 -0.868 5 +11.641 -0.641 5 +12.180 -0.447 5 +12.731 -0.287 5 +13.290 -0.162 5 +13.856 -0.072 5 +14.427 -0.018 5 +15.000 0.000 5 +15.573 0.018 5 +16.144 0.072 5 +16.710 0.162 5 +17.269 0.287 5 +17.820 0.447 5 +18.359 0.641 5 +18.885 0.868 5 +19.396 1.129 5 +19.889 1.421 5 +20.364 1.743 5 +20.816 2.094 5 +21.246 2.473 5 +21.652 2.879 5 +22.031 3.309 5 +22.382 3.761 5 +22.704 4.236 5 +22.996 4.729 5 +23.257 5.240 5 +23.484 5.766 5 +23.678 6.305 5 +23.838 6.856 5 +23.963 7.415 5 +24.053 7.981 5 +24.107 8.552 5 +24.125 9.125 5 +24.107 9.698 5 +24.053 10.269 5 +23.963 10.835 5 +23.838 11.394 5 +23.678 11.945 5 +23.484 12.484 5 +23.257 13.010 5 +22.996 13.521 5 +22.704 14.014 5 +22.382 14.489 5 +22.031 14.941 5 +21.652 15.371 5 +21.246 15.777 5 +20.816 16.156 5 +20.364 16.507 5 +19.889 16.829 5 +19.396 17.121 5 +18.885 17.382 5 +18.359 17.609 5 +17.820 17.803 5 +17.269 17.963 5 +16.710 18.088 5 +16.144 18.178 5 +15.573 18.232 5 +15.000 18.250 5 +14.427 18.232 5 +13.856 18.178 5 +13.290 18.088 5 +12.731 17.963 5 +12.180 17.803 5 +11.641 17.609 5 +11.115 17.382 5 +10.604 17.121 5 +10.111 16.829 5 +9.636 16.507 5 +9.184 16.156 5 +8.754 15.777 5 +8.348 15.371 5 +7.969 14.941 5 +7.618 14.489 5 +7.296 14.014 5 +7.004 13.521 5 +6.743 13.010 5 +6.516 12.484 5 +6.322 11.945 5 +6.162 11.394 5 +6.037 10.835 5 +5.947 10.269 5 +5.893 9.698 5 +5.875 9.125 5 +5.893 8.552 5 +5.947 7.981 5 +6.037 7.415 5 +6.162 6.856 5 +6.322 6.305 5 +6.516 5.766 5 +6.743 5.240 5 +7.004 4.729 5 +7.296 4.236 5 +7.618 3.761 5 +7.969 3.309 5 +8.348 2.879 5 +8.754 2.473 5 +9.184 2.094 5 +9.636 1.743 5 +10.111 1.421 5 +10.604 1.129 5 +11.115 0.868 5 +11.641 0.641 5 +12.180 0.447 5 +12.731 0.287 5 +13.290 0.162 5 +13.856 0.072 5 +14.427 0.018 5 +15.000 0.000 5 +15.573 0.018 5 +16.144 0.072 5 +16.710 0.162 5 +17.269 0.287 5 +17.820 0.447 5 +18.359 0.641 5 +18.885 0.868 5 +19.396 1.129 5 +19.889 1.421 5 +20.364 1.743 5 +20.816 2.094 5 +21.246 2.473 5 +21.652 2.879 5 +22.031 3.309 5 +22.382 3.761 5 +22.704 4.236 5 +22.996 4.729 5 +23.257 5.240 5 +23.484 5.766 5 +23.678 6.305 5 +23.838 6.856 5 +23.963 7.415 5 +24.053 7.981 5 +24.107 8.552 5 +24.125 9.125 5 +24.107 9.698 5 +24.053 10.269 5 +23.963 10.835 5 +23.838 11.394 5 +23.678 11.945 5 +23.484 12.484 5 +23.257 13.010 5 +22.996 13.521 5 +22.704 14.014 5 +22.382 14.489 5 +22.031 14.941 5 +21.652 15.371 5 +21.246 15.777 5 +20.816 16.156 5 +20.364 16.507 5 +19.889 16.829 5 +19.396 17.121 5 +18.885 17.382 5 +18.359 17.609 5 +17.820 17.803 5 +17.269 17.963 5 +16.710 18.088 5 +16.144 18.178 5 +15.573 18.232 5 +15.000 18.250 5 +14.427 18.232 5 +13.856 18.178 5 +13.290 18.088 5 +12.731 17.963 5 +12.180 17.803 5 +11.641 17.609 5 +11.115 17.382 5 +10.604 17.121 5 +10.111 16.829 5 +9.636 16.507 5 +9.184 16.156 5 +8.754 15.777 5 +8.348 15.371 5 +7.969 14.941 5 +7.618 14.489 5 +7.296 14.014 5 +7.004 13.521 5 +6.743 13.010 5 +6.516 12.484 5 +6.322 11.945 5 +6.162 11.394 5 +6.037 10.835 5 +5.947 10.269 5 +5.893 9.698 5 +5.875 9.125 5 +5.893 8.552 5 +5.947 7.981 5 +6.037 7.415 5 +6.162 6.856 5 +6.322 6.305 5 +6.516 5.766 5 +6.743 5.240 5 +7.004 4.729 5 +7.296 4.236 5 +7.618 3.761 5 +7.969 3.309 5 +8.348 2.879 5 +8.754 2.473 5 +9.184 2.094 5 +9.636 1.743 5 +10.111 1.421 5 +10.604 1.129 5 +11.115 0.868 5 +11.641 0.641 5 +12.180 0.447 5 +12.731 0.287 5 +13.290 0.162 5 +13.856 0.072 5 +14.427 0.018 5 +15.000 0 7 +15.500 0 7 +16.000 0 7 +16.500 0 7 +17.000 0 7 +17.500 0 7 +18.000 0 7 +18.500 0 7 +19.000 0 7 +19.500 0 7 +20.000 0 7 +20.500 0 7 +21.000 0 7 +21.500 0 7 +22.000 0 7 +22.500 0 7 +23.000 0 7 +23.500 0 7 +24.000 0 7 +24.500 0 7 +25.000 0 0 +25.500 0 0 +26.000 0 0 +26.500 0 0 +27.000 0 0 +27.500 0 0 +28.000 0 0 +28.500 0 0 +29.000 0 0 +29.500 0 0 +30.000 0 0 +30.500 0 0 +31.000 0 0 +31.500 0 0 +32.000 0 0 +32.500 0 0 +33.000 0 0 +33.500 0 0 +34.000 0 0 +34.500 0 0 +35.000 0 0 +35.500 0 0 +36.000 0 0 +36.500 0 0 +37.000 0 0 +37.500 0 0 +38.000 0 0 +38.500 0 0 +39.000 0 0 +39.500 0 0 +40.000 0 0 +40.500 0 0 +41.000 0 0 +41.500 0 0 +42.000 0 0 +42.500 0 0 +43.000 0 0 +43.500 0 0 +44.000 0 0 +44.500 0 0 diff --git a/src/planning/src/utils/skidpadgenerator.py b/src/planning/src/utils/skidpadgenerator.py new file mode 100644 index 000000000..9f07fdb6c --- /dev/null +++ b/src/planning/src/utils/skidpadgenerator.py @@ -0,0 +1,57 @@ +import math +import os +# Circle parameters +centerX = 15.0 +centerY = 9.125 +radius = 9.125 +numPoints = 100 # Number of points to describe the circle + +entryspeed=6 +exitspeed = 7 +circular_speed = 5 + +file_name = "./src/planning/src/utils/skidpad.txt" + +# Get the absolute path of the file + + + +with open(file_name, "w") as file: + + for i in range(0,30): + file.write(f"{i/2:.3f} 0 {entryspeed}\n") + + # Generate and print circular path points + for i in range(numPoints): + theta = (2 * math.pi / numPoints) * i - math.pi / 2 # Angle in radians, start at x=15, y=0 + x = centerX + radius * math.cos(theta) + y = centerY + radius * math.sin(theta) + file.write(f"{x:.3f} -{y:.3f} {circular_speed}\n") + + for i in range(numPoints): + theta = (2 * math.pi / numPoints) * i - math.pi / 2 # Angle in radians, start at x=15, y=0 + x = centerX + radius * math.cos(theta) + y = centerY + radius * math.sin(theta) + file.write(f"{x:.3f} -{y:.3f} {circular_speed}\n") + + for i in range(numPoints): + theta = (2 * math.pi / numPoints) * i - math.pi / 2 # Angle in radians, start at x=15, y=0 + x = centerX + radius * math.cos(theta) + y = centerY + radius * math.sin(theta) + file.write(f"{x:.3f} {y:.3f} {circular_speed}\n") + + for i in range(numPoints): + theta = (2 * math.pi / numPoints) * i - math.pi / 2 # Angle in radians, start at x=15, y=0 + x = centerX + radius * math.cos(theta) + y = centerY + radius * math.sin(theta) + file.write(f"{x:.3f} {y:.3f} {circular_speed}\n") + + + for i in range(0,20): + file.write(f"{15+i/2:.3f} 0 {exitspeed}\n") + + for i in range(0,40): + file.write(f"{25+i/2:.3f} 0 {0}\n") + + file_path = os.path.abspath(file_name) + print(f"File written to: {file_path}") \ No newline at end of file diff --git a/src/planning/test/test_utils/utils.hpp b/src/planning/test/test_utils/utils.hpp index 828d9bac0..83c015e79 100644 --- a/src/planning/test/test_utils/utils.hpp +++ b/src/planning/test/test_utils/utils.hpp @@ -19,6 +19,7 @@ #include "planning/path_calculation.hpp" #include "planning/planning.hpp" #include "planning/smoothing.hpp" +#include "planning/velocity_planning.hpp" #include "rclcpp/rclcpp.hpp" #include "utils/files.hpp" #include "utils/splines.hpp" diff --git a/src/planning/test/velocity_planning_tests.cpp b/src/planning/test/velocity_planning_tests.cpp new file mode 100644 index 000000000..845f1a568 --- /dev/null +++ b/src/planning/test/velocity_planning_tests.cpp @@ -0,0 +1,58 @@ +#include "test_utils/utils.hpp" + +/** + * @brief simple scenario with few points in the path + * + */ +TEST(VelocityPlanning, velocity0) { + std::string file_path = "src/planning/tracks/velocity1.txt"; + VelocityPlanning velocity_planning; + std::vector final_path = path_from_file(file_path); + velocity_planning.set_velocity(final_path); + + EXPECT_EQ((int)final_path.size(), 14); + EXPECT_EQ(final_path.back().ideal_velocity, 3); +} + +/** + * @brief Check if the velocity is always greater than the minimum velocity + * + */ +TEST(VelocityPlanning, velocity1) { + std::string file_path = "src/planning/tracks/velocity1.txt"; + VelocityPlanning velocity_planning; + std::vector final_path = path_from_file(file_path); + velocity_planning.set_velocity(final_path); + + for (std::size_t i = 0; i < final_path.size(); i++) { + EXPECT_TRUE(final_path[i].ideal_velocity >= 3); + } +} + +/** + * @brief test for a path with only one point + * + */ +TEST(VelocityPlanning, velocity2) { + std::string file_path = "src/planning/tracks/velocity2.txt"; + VelocityPlanning velocity_planning; + std::vector final_path = path_from_file(file_path); + velocity_planning.set_velocity(final_path); + + EXPECT_EQ((int)final_path.size(), 1); + EXPECT_EQ(final_path[0].ideal_velocity, 3); +} + + +/** + * @brief test for a path with no points + * + */ +TEST(VelocityPlanning, velocity3) { + std::string file_path = "src/planning/tracks/velocity3.txt"; + VelocityPlanning velocity_planning; + std::vector final_path = path_from_file(file_path); + velocity_planning.set_velocity(final_path); + + EXPECT_EQ((int)final_path.size(), 0); +} \ No newline at end of file diff --git a/src/planning/tracks/velocity1.txt b/src/planning/tracks/velocity1.txt new file mode 100644 index 000000000..beb9635aa --- /dev/null +++ b/src/planning/tracks/velocity1.txt @@ -0,0 +1,14 @@ +1.91769 0.81265 0 +4.04502 1.41728 0 +6.26203 3.27594 0 +8.79250 4.03732 0 +11.12143 3.20873 0 +12.01717 0.79026 0 +12.69396 -1.24291 0 +13.61348 -1.67343 0 +15.01695 -1.76911 0 +17.17054 0.43136 0 +18.00021 2.00334 0 +18.64660 3.37331 0 +19.61451 5.50203 0 +20.48563 7.39157 0 \ No newline at end of file diff --git a/src/planning/tracks/velocity2.txt b/src/planning/tracks/velocity2.txt new file mode 100644 index 000000000..858eff1eb --- /dev/null +++ b/src/planning/tracks/velocity2.txt @@ -0,0 +1 @@ +1.91769 0.81265 0 \ No newline at end of file diff --git a/src/planning/tracks/velocity3.txt b/src/planning/tracks/velocity3.txt new file mode 100644 index 000000000..e69de29bb