From 9ecc3a26a142febc6d59de3891758a3f38cc4d20 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 18 Dec 2022 13:23:32 +0900 Subject: [PATCH] feat(obstacle_cruies_planner): improve pid_based cruise planner (#2507) * feat(obstacle_cruies_planner): improve pid_based cruise planner Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update param in tier4_planning_launch Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 49 +- planning/obstacle_cruise_planner/README.md | 10 +- ...plot_juggler_obstacle_velocity_planner.xml | 36 +- .../common_structs.hpp | 6 +- .../include/obstacle_cruise_planner/node.hpp | 17 +- .../cruise_planning_debug_info.hpp | 97 +++ .../pid_based_planner/pid_based_planner.hpp | 96 ++- .../planner_interface.hpp | 15 + .../stop_planning_debug_info.hpp | 88 +++ planning/obstacle_cruise_planner/src/node.cpp | 62 +- .../pid_based_planner/pid_based_planner.cpp | 669 ++++++++++++++---- .../src/planner_interface.cpp | 16 + 12 files changed, 939 insertions(+), 222 deletions(-) create mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp create mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 520637abed775..dfc2b2e6ddb44 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -3,15 +3,18 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: true + is_showing_debug_info: false + disable_stop_planning: false # true # longitudinal info - idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] @@ -25,7 +28,7 @@ bus: true trailer: true motorcycle: true - bicycle: false + bicycle: true pedestrian: false stop_obstacle_type: @@ -62,7 +65,7 @@ stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle ignored_outside_obstacle_type: - unknown: false + unknown: true car: false truck: false bus: false @@ -72,21 +75,41 @@ pedestrian: true pid_based_planner: - # use_predicted_obstacle_pose: false + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic - # PID gains to keep safe distance with the front vehicle - kp: 2.5 - ki: 0.0 - kd: 2.3 + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + disable_target_acceleration: true + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 - lpf_cruise_gain: 0.2 + lpf_normalized_error_cruise_dist_gain: 0.2 optimization_based_planner: dense_resampling_time_interval: 0.2 diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 4d114c47bfaf0..5f372a5baed34 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -56,15 +56,15 @@ struct ObstacleCruisePlannerData struct TargetObstacle { rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; - Shape shape; + std::string uuid; std::vector predicted_paths; - geometry_msgs::msg::Point collision_point; + std::vector collision_points; + bool has_stopped; }; ``` diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml index 634268b93f2de..2e7eeee211963 100644 --- a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml @@ -10,17 +10,17 @@ - - - + + + - - + + @@ -29,17 +29,17 @@ - - - + + + - - + + @@ -56,17 +56,17 @@ - - - + + + - - + + @@ -75,9 +75,9 @@ - - - + + + diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 724e7dbf1372b..43949c55b21a5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -58,7 +58,6 @@ struct TargetObstacle pose = object.kinematics.initial_pose_with_covariance.pose; velocity_reliable = true; velocity = aligned_velocity; - is_classified = true; classification = object.classification.at(0); uuid = toHexString(object.object_id); @@ -72,11 +71,10 @@ struct TargetObstacle } rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; std::string uuid; std::vector predicted_paths; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 7570b1ad12274..8ce9ee3d7e925 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,7 +100,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const PredictedObject & predicted_object, const Trajectory & traj, const bool is_driving_forward); void publishVelocityLimit(const boost::optional & vel_limit); - void publishDebugData(const DebugData & debug_data) const; + void publishDebugMarker(const DebugData & debug_data) const; + void publishDebugInfo() const; void publishCalculationTime(const double calculation_time) const; bool isCruiseObstacle(const uint8_t label); @@ -128,6 +129,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_marker_pub_; rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber @@ -141,10 +144,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node tier4_autoware_utils::SelfPoseListener self_pose_listener_; // data for callback functions - PredictedObjects::ConstSharedPtr in_objects_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; - - geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_; + PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr}; + geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr}; + geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -160,7 +162,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node stop_watch_; // planner - std::unique_ptr planner_ptr_; + std::unique_ptr planner_ptr_{nullptr}; // previous closest obstacle std::shared_ptr prev_closest_obstacle_ptr_{nullptr}; @@ -195,8 +197,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool need_to_clear_vel_limit_{false}; bool is_driving_forward_{true}; + bool disable_stop_planning_{false}; std::vector prev_target_obstacles_; + + std::shared_ptr lpf_acc_ptr_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..3ab5dd0a011d9 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,97 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index a6ed9d3e66bdf..c2308adc1ca17 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ #define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ -#include "obstacle_cruise_planner/pid_based_planner/debug_values.hpp" +#include "obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -29,26 +28,25 @@ #include #include -using tier4_debug_msgs::msg::Float32MultiArrayStamped; - class PIDBasedPlanner : public PlannerInterface { public: struct CruiseObstacleInfo { CruiseObstacleInfo( - const TargetObstacle & obstacle_arg, const double dist_to_cruise_arg, - const double normalized_dist_to_cruise_arg, double dist_to_obstacle_arg) + const TargetObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) : obstacle(obstacle_arg), - dist_to_cruise(dist_to_cruise_arg), - normalized_dist_to_cruise(normalized_dist_to_cruise_arg), - dist_to_obstacle(dist_to_obstacle_arg) + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) { } + TargetObstacle obstacle; - double dist_to_cruise; - double normalized_dist_to_cruise; + double error_cruise_dist; double dist_to_obstacle; + double target_dist_to_obstacle; }; PIDBasedPlanner( @@ -66,39 +64,83 @@ class PIDBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & cruise_obstacle_info); - void planCruise( + Float32MultiArrayStamped getCruisePlanningDebugMessage( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convertToMessage(current_time); + } + +private: + boost::optional calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data); + Trajectory planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data); - VelocityLimit doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_walls_marker); - void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const; + // velocity limit based planner + VelocityLimit doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + + // velocityinsertion based planner + Trajectory doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + Trajectory getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const; // ROS parameters double min_accel_during_cruise_; - double vel_to_acc_weight_; double min_cruise_target_vel_; - // bool use_predicted_obstacle_pose_; - // pid controller - std::unique_ptr pid_controller_; - double output_ratio_during_accel_; + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; // stop watch tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; - // publisher - rclcpp::Publisher::SharedPtr debug_values_pub_; + boost::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + Trajectory prev_traj_; - boost::optional prev_target_vel_; + bool use_velocity_limit_based_planner_{true}; - DebugValues debug_values_; + double time_to_evaluate_rss_; - std::shared_ptr lpf_cruise_ptr_; + std::function error_func_; }; #endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7fe64181ff420..a01d3589245b5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -17,6 +17,7 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" +#include "obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -38,6 +39,7 @@ using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; @@ -105,6 +107,16 @@ class PlannerInterface smoothed_trajectory_ptr_ = traj; } + Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const + { + return stop_planning_debug_info_.convertToMessage(current_time); + } + virtual Float32MultiArrayStamped getCruisePlanningDebugMessage( + [[maybe_unused]] const rclcpp::Time & current_time) const + { + return Float32MultiArrayStamped{}; + } + protected: // Parameters bool is_showing_debug_info_{false}; @@ -123,6 +135,9 @@ class PlannerInterface EgoNearestParam ego_nearest_param_; + // debug info + StopPlanningDebugInfo stop_planning_debug_info_; + // TODO(shimizu) remove these parameters Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..7c682a1f63c01 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -0,0 +1,88 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 39a4dd5de0815..71ec3e5ef200c 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -134,13 +134,13 @@ double calcAlignedAdaptiveCruise( return object_vel * std::cos(object_yaw - traj_yaw); } -double calcObjectMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObjectMaxLength(const Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -192,6 +192,9 @@ Trajectory extendTrajectory( namespace motion_planning { +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), self_pose_listener_(this), @@ -223,11 +226,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); - debug_cruise_wall_marker_pub_ = - create_publisher("~/debug/cruise/virtual_wall", 1); - debug_stop_wall_marker_pub_ = - create_publisher("~/virtual_wall", 1); - debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); + debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); + debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_stop_planning_info_pub_ = + create_publisher("~/debug/stop_planning_info", 1); + debug_cruise_planning_info_pub_ = + create_publisher("~/debug/cruise_planning_info", 1); // longitudinal_info const auto longitudinal_info = [&]() { @@ -248,6 +253,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & const double terminal_safe_distance_margin = declare_parameter("common.terminal_safe_distance_margin"); + lpf_acc_ptr_ = std::make_shared(0.2); + return LongitudinalInfo{ max_accel, min_accel, @@ -273,6 +280,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & }(); is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); + disable_stop_planning_ = declare_parameter("common.disable_stop_planning"); { // Obstacle filtering parameters obstacle_filtering_param_.rough_detection_area_expand_width = @@ -424,7 +432,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); } } - // wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -456,6 +463,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + tier4_autoware_utils::updateParam( + parameters, "common.disable_stop_planning", disable_stop_planning_); + // obstacle_filtering tier4_autoware_utils::updateParam( parameters, "obstacle_filtering.rough_detection_area_expand_width", @@ -572,7 +582,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // publish debug data - publishDebugData(debug_data); + publishDebugMarker(debug_data); + publishDebugInfo(); // publish and print calculation time const double calculation_time = stop_watch_.toc(__func__); @@ -610,9 +621,10 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( planner_data.current_vel = current_vel; planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; + for (const auto & obstacle : obstacles) { // consider all target obstacles when driving backward - if (!planner_data.is_driving_forward || obstacle.has_stopped) { + if (!disable_stop_planning_ && (!planner_data.is_driving_forward || obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -651,7 +663,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; for (const auto & obstacle : obstacles) { - if (planner_data.is_driving_forward && !obstacle.has_stopped) { + if (disable_stop_planning_ || (planner_data.is_driving_forward && !obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -1024,11 +1036,11 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( } } -void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) const +void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const { stop_watch_.tic(__func__); - visualization_msgs::msg::MarkerArray debug_marker; + MarkerArray debug_marker; const auto current_time = now(); // obstacles to cruise @@ -1055,7 +1067,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // footprint polygons auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); @@ -1077,7 +1089,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // collision points for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "collision_points", i, visualization_msgs::msg::Marker::SPHERE, + "map", current_time, "collision_points", i, Marker::SPHERE, tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = debug_data.collision_points.at(i); @@ -1087,7 +1099,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c debug_marker_pub_->publish(debug_marker); - // wall for cruise and stop + // virtual wall for cruise and stop debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker); debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker); @@ -1098,6 +1110,19 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c __func__, calculation_time); } +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + const auto current_time = now(); + + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); +} + void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const { Float32Stamped calculation_time_msg; @@ -1106,5 +1131,6 @@ void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_ debug_calculation_time_pub_->publish(calculation_time_msg); } } // namespace motion_planning + #include RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleCruisePlannerNode) diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 267ff1f4502f2..cfd9dd24c543b 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,6 +14,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "interpolation/spline_interpolation.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -40,15 +41,15 @@ VelocityLimit createVelocityLimitMsg( return msg; } -Float32MultiArrayStamped convertDebugValuesToMsg( - const rclcpp::Time & current_time, const DebugValues & debug_values) +template +T getSign(const T val) { - Float32MultiArrayStamped debug_msg{}; - debug_msg.stamp = current_time; - for (const auto & v : debug_values.getValues()) { - debug_msg.data.push_back(v); + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); } - return debug_msg; + return static_cast(0); } } // namespace @@ -60,30 +61,86 @@ PIDBasedPlanner::PIDBasedPlanner( min_accel_during_cruise_ = node.declare_parameter("pid_based_planner.min_accel_during_cruise"); - // pid controller - const double kp = node.declare_parameter("pid_based_planner.kp"); - const double ki = node.declare_parameter("pid_based_planner.ki"); - const double kd = node.declare_parameter("pid_based_planner.kd"); - pid_controller_ = std::make_unique(kp, ki, kd); - output_ratio_during_accel_ = - node.declare_parameter("pid_based_planner.output_ratio_during_accel"); - - // some parameters - // use_predicted_obstacle_pose_ = - // node.declare_parameter("pid_based_planner.use_predicted_obstacle_pose"); + use_velocity_limit_based_planner_ = + node.declare_parameter("pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + } - vel_to_acc_weight_ = node.declare_parameter("pid_based_planner.vel_to_acc_weight"); + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc"); + } min_cruise_target_vel_ = node.declare_parameter("pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = node.declare_parameter("pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = + node.declare_parameter("pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); // low pass filter - const double lpf_cruise_gain = - node.declare_parameter("pid_based_planner.lpf_cruise_gain"); - lpf_cruise_ptr_ = std::make_shared(lpf_cruise_gain); - - // publisher - debug_values_pub_ = node.create_publisher("~/debug/values", 1); + const double lpf_normalized_error_cruise_dist_gain = + node.declare_parameter("pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); } Trajectory PIDBasedPlanner::generateCruiseTrajectory( @@ -91,36 +148,35 @@ Trajectory PIDBasedPlanner::generateCruiseTrajectory( DebugData & debug_data) { stop_watch_.tic(__func__); - debug_values_.resetValues(); + cruise_planning_debug_info_.reset(); // calc obstacles to cruise - boost::optional cruise_obstacle_info; - calcObstaclesToCruise(planner_data, cruise_obstacle_info); + const auto cruise_obstacle_info = calcObstacleToCruise(planner_data); // plan cruise - planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); - - // publish debug values - publishDebugValues(planner_data); + const auto cruise_traj = planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); - return planner_data.traj; + prev_traj_ = cruise_traj; + return cruise_traj; } -void PIDBasedPlanner::calcObstaclesToCruise( - const ObstacleCruisePlannerData & planner_data, - boost::optional & cruise_obstacle_info) +boost::optional PIDBasedPlanner::calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data) { - debug_values_.setValues(DebugValues::TYPE::CURRENT_VELOCITY, planner_data.current_vel); - debug_values_.setValues(DebugValues::TYPE::CURRENT_ACCELERATION, planner_data.current_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.current_acc); auto modified_target_obstacles = planner_data.target_obstacles; // search highest probability obstacle for cruise + boost::optional cruise_obstacle_info; for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { const auto & obstacle = planner_data.target_obstacles.at(o_idx); @@ -129,35 +185,102 @@ void PIDBasedPlanner::calcObstaclesToCruise( } // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle const double dist_to_obstacle = - calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point); - - if (!obstacle.has_stopped) { // cruise - // calculate distance between ego and obstacle based on RSS - const double rss_dist = calcRSSDistance( - planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); - - // calculate error distance and normalized one - const double error_dist = dist_to_obstacle - rss_dist; - if (cruise_obstacle_info) { - if (error_dist > cruise_obstacle_info->dist_to_cruise) { - continue; - } + calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data.current_vel) * time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calcRSSDistance( + planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; } - const double normalized_dist_to_cruise = error_dist / dist_to_obstacle; - cruise_obstacle_info = - CruiseObstacleInfo(obstacle, error_dist, normalized_dist_to_cruise, dist_to_obstacle); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_VELOCITY, obstacle.velocity); - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_OBJECT_DISTANCE, rss_dist); - debug_values_.setValues(DebugValues::TYPE::CRUISE_ERROR_OBJECT_DISTANCE, error_dist); } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); + } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); } + + return cruise_obstacle_info; } -void PIDBasedPlanner::planCruise( +Trajectory PIDBasedPlanner::planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data) { @@ -167,48 +290,68 @@ void PIDBasedPlanner::planCruise( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "cruise planning"); - vel_limit = doCruise( - planner_data, cruise_obstacle_info.get(), debug_data.obstacles_to_cruise, - debug_data.cruise_wall_marker); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_VELOCITY, vel_limit->max_velocity); - debug_values_.setValues( - DebugValues::TYPE::CRUISE_TARGET_ACCELERATION, vel_limit->constraints.min_acceleration); - } else { - // reset previous target velocity if adaptive cruise is not enabled - prev_target_vel_ = {}; - lpf_cruise_ptr_->reset(); - - // delete marker - const auto markers = - motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, + dist_to_rss_wall); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + // cruise obstalce + debug_data.obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = doCruiseWithTrajectory(planner_data, cruise_obstacle_info.get()); + return cruise_traj; + } + + vel_limit = doCruiseWithVelocityLimit(planner_data, cruise_obstacle_info.get()); + return planner_data.traj; } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + return planner_data.traj; } -VelocityLimit PIDBasedPlanner::doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_wall_marker) +VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) { - const double dist_to_cruise = cruise_obstacle_info.dist_to_cruise; - const double filtered_normalized_dist_to_cruise = [&]() { - const double normalized_dist_to_cruise = cruise_obstacle_info.normalized_dist_to_cruise; - return lpf_cruise_ptr_->filter(normalized_dist_to_cruise); + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); }(); - const double dist_to_obstacle = cruise_obstacle_info.dist_to_obstacle; - const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); // calculate target velocity with acceleration limit by PID controller - const double pid_output_vel = pid_controller_->calc(filtered_normalized_dist_to_cruise); - [[maybe_unused]] const double prev_vel = - prev_target_vel_ ? prev_target_vel_.get() : planner_data.current_vel; - + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); const double additional_vel = [&]() { - if (filtered_normalized_dist_to_cruise > 0) { - return pid_output_vel * output_ratio_during_accel_; + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; } return pid_output_vel; }(); @@ -217,42 +360,251 @@ VelocityLimit PIDBasedPlanner::doCruise( std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); // calculate target acceleration - const double target_acc = vel_to_acc_weight_ * additional_vel; - const double target_acc_with_acc_limit = - std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + const double target_acc = [&]() { + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "target_velocity %f", positive_target_vel); - prev_target_vel_ = positive_target_vel; - // set target longitudinal motion const auto vel_limit = createVelocityLimitMsg( - planner_data.current_time, positive_target_vel, target_acc_with_acc_limit, - longitudinal_info_.max_jerk, longitudinal_info_.min_jerk); + planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk, + longitudinal_info_.min_jerk); - // virtual wall marker for cruise - const double abs_ego_offset = planner_data.is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_to_rss_wall = - std::min(dist_to_cruise + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + return vel_limit; +} - const auto markers = motion_utils::createSlowDownVirtualWallMarker( - planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, - dist_to_rss_wall); - tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); +Trajectory PIDBasedPlanner::doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; - debug_obstacles_to_cruise.push_back(cruise_obstacle_info.obstacle); + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); - return vel_limit; + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + /* + const double target_acc = vel_to_acc_weight_ * additional_vel; + const double target_acc_with_acc_limit = + std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + */ + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + if (smoothed_trajectory_ptr_) { + return motion_utils::calcInterpolatedPoint( + *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + } + return motion_utils::calcInterpolatedPoint( + prev_traj_, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj = getAccelerationLimitedTrajectory( + planner_data.traj, planner_data.current_pose, v0, a0, target_acc, target_jerk_ratio); + + const auto zero_vel_idx_opt = motion_utils::searchZeroVelocityIndex(cruise_traj.points); + if (!zero_vel_idx_opt) { + return cruise_traj; + } + + for (size_t i = zero_vel_idx_opt.get(); i < cruise_traj.points.size(); ++i) { + cruise_traj.points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj; } -void PIDBasedPlanner::publishDebugValues(const ObstacleCruisePlannerData & planner_data) const +Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const { - const auto debug_values_msg = convertDebugValuesToMsg(planner_data.current_time, debug_values_); - debug_values_pub_->publish(debug_values_msg); + // calculate vt function + const auto & i = longitudinal_info_; + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = motion_utils::calcArcLength(traj.points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of obstacle_cruise_planner may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = + target_acc > a1 ? i.max_jerk * target_jerk_ratio : i.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj; + } + + auto acc_limited_traj = traj; + const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + acc_limited_traj.points, start_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += tier4_autoware_utils::calcDistance2d( + acc_limited_traj.points.at(i - 1), acc_limited_traj.points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj.points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj.points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj; } void PIDBasedPlanner::updateParam(const std::vector & parameters) @@ -260,24 +612,79 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); - // pid controller - double kp = pid_controller_->getKp(); - double ki = pid_controller_->getKi(); - double kd = pid_controller_->getKd(); - - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kp", kp); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.ki", ki); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kd", kd); - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.output_ratio_during_accel", output_ratio_during_accel_); + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kp", kp); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.ki", ki); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", + p.output_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } - // vel_to_acc_weight - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.vel_to_acc_weight", vel_to_acc_weight_); + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } // min_cruise_target_vel tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - - pid_controller_->updateParam(kp, ki, kd); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 45ed1d3f92d0e..adee76f67c065 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -106,6 +106,12 @@ double calcMinimumDistanceToStop( Trajectory PlannerInterface::generateStopTrajectory( const ObstacleCruisePlannerData & planner_data, DebugData & debug_data) { + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_acc); + const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -236,6 +242,16 @@ Trajectory PlannerInterface::generateStopTrajectory( stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); } + // stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, // + // TODO(murooka) + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); + + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, feasible_margin_from_obstacle); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); + return output_traj; }