diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 14cfaa7a72e3b..602b81ae5b946 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -5,6 +5,7 @@ 1.0.0 The lidar_centerpoint package Yusuke Muramatsu + Kenzo Lobos-Tsunekawa Apache License 2.0 ament_cmake_auto diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 5f372a5baed34..e3ddbfb1b157a 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -4,30 +4,29 @@ The `obstacle_cruise_planner` package has following modules. -- obstacle stop planning - - inserting a stop point in the trajectory when there is a static obstacle on the trajectory. -- adaptive cruise planning - - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory +- Stop planning + - stop when there is a static obstacle near the trajectory. +- Cruise planning + - slow down when there is a dynamic obstacle to cruise near the trajectory ## Interfaces ### Input topics -| Name | Type | Description | -| ----------------------------- | ----------------------------------------------- | --------------------------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ----------------------------------------------- | ---------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics -| Name | Type | Description | -| ------------------------------ | ---------------------------------------------- | ------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | -| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | -| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | ------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | +| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | ## Design @@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. $$ -d = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, +d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, $$ -assuming that $d$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. +assuming that $d_rss$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`. | Parameter | Type | Description | | --------------------------------- | ------ | ----------------------------------------------------------------------------- | | `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | -| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] | +| `common.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] | +| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] | + +The detailed formulation is as follows. + +$$ +d_{error} = d - d_{rss} \\ +d_{normalized} = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} = pid(d_{quad, normalized}) \\ +v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} = max(v_{ego} + v_{add}, v_{min, cruise}) +$$ + +| Variable | Description | +| ----------------- | --------------------------------------- | +| `d` | actual distane to obstacle | +| `d_{rss}` | ideal distance to obstacle based on RSS | +| `v_{min, cruise}` | `min_cruise_target_vel` | +| `w_{acc}` | `output_ratio_during_accel` | +| `lpf(val)` | apply low-pass filter to `val` | +| `pid(val)` | apply pid to `val` | ## Implementation diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 520637abed775..dfc2b2e6ddb44 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/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/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 43949c55b21a5..3c35abe7fa14c 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 @@ -15,37 +15,16 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; - -namespace -{ -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} -} // namespace - struct TargetObstacle { TargetObstacle( @@ -59,7 +38,7 @@ struct TargetObstacle velocity_reliable = true; velocity = aligned_velocity; classification = object.classification.at(0); - uuid = toHexString(object.object_id); + uuid = tier4_autoware_utils::toHexString(object.object_id); predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -85,7 +64,7 @@ struct TargetObstacle struct ObstacleCruisePlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + Trajectory traj; geometry_msgs::msg::Pose current_pose; double current_vel; double current_acc; @@ -95,6 +74,50 @@ struct ObstacleCruisePlannerData struct LongitudinalInfo { + explicit LongitudinalInfo(rclcpp::Node & node) + { + max_accel = node.declare_parameter("normal.max_acc"); + min_accel = node.declare_parameter("normal.min_acc"); + max_jerk = node.declare_parameter("normal.max_jerk"); + min_jerk = node.declare_parameter("normal.min_jerk"); + limit_max_accel = node.declare_parameter("limit.max_acc"); + limit_min_accel = node.declare_parameter("limit.min_acc"); + limit_max_jerk = node.declare_parameter("limit.max_jerk"); + limit_min_jerk = node.declare_parameter("limit.min_jerk"); + + idling_time = node.declare_parameter("common.idling_time"); + min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); + min_object_accel_for_rss = node.declare_parameter("common.min_object_accel_for_rss"); + + safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); + terminal_safe_distance_margin = + node.declare_parameter("common.terminal_safe_distance_margin"); + } + + void onParam(const std::vector & parameters) + { + tier4_autoware_utils::updateParam(parameters, "normal.max_accel", max_accel); + tier4_autoware_utils::updateParam(parameters, "normal.min_accel", min_accel); + tier4_autoware_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + tier4_autoware_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + + tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); + tier4_autoware_utils::updateParam( + parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); + tier4_autoware_utils::updateParam( + parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); + + tier4_autoware_utils::updateParam( + parameters, "common.safe_distance_margin", safe_distance_margin); + tier4_autoware_utils::updateParam( + parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + } + + // common parameter double max_accel; double min_accel; double max_jerk; @@ -103,9 +126,13 @@ struct LongitudinalInfo double limit_min_accel; double limit_max_jerk; double limit_min_jerk; + + // rss parameter double idling_time; double min_ego_accel_for_rss; double min_object_accel_for_rss; + + // distance margin double safe_distance_margin; double terminal_safe_distance_margin; }; @@ -115,18 +142,21 @@ struct DebugData std::vector intentionally_ignored_obstacles; std::vector obstacles_to_stop; std::vector obstacles_to_cruise; - visualization_msgs::msg::MarkerArray stop_wall_marker; - visualization_msgs::msg::MarkerArray cruise_wall_marker; + MarkerArray stop_wall_marker; + MarkerArray cruise_wall_marker; std::vector detection_polygons; std::vector collision_points; }; struct EgoNearestParam { - EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold) - : dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold) + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node & node) { + dist_threshold = node.declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node.declare_parameter("ego_nearest_yaw_threshold"); } + double dist_threshold; double yaw_threshold; }; 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 8ce9ee3d7e925..e52c2671fea5e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -18,25 +18,12 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -44,21 +31,6 @@ #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::AccelStamped; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_planning_msgs::msg::StopReasonArray; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; - namespace motion_planning { class ObstacleCruisePlannerNode : public rclcpp::Node @@ -70,9 +42,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // callback functions rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); - void onObjects(const PredictedObjects::ConstSharedPtr msg); - void onOdometry(const Odometry::ConstSharedPtr); - void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr); void onTrajectory(const Trajectory::ConstSharedPtr msg); void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); @@ -111,8 +80,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_showing_debug_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; double obstacle_velocity_threshold_from_cruise_to_stop_; double obstacle_velocity_threshold_from_stop_to_cruise_; @@ -126,27 +93,23 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr vel_limit_pub_; rclcpp::Publisher::SharedPtr clear_vel_limit_pub_; - 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_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 - rclcpp::Subscription::SharedPtr trajectory_sub_; - rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; + rclcpp::Subscription::SharedPtr traj_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr acc_sub_; - // self pose listener - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - // data for callback functions PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr}; - geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr}; - geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr}; + Odometry::ConstSharedPtr current_odom_ptr_{nullptr}; + AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -195,13 +158,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleFilteringParam obstacle_filtering_param_; bool need_to_clear_vel_limit_{false}; + EgoNearestParam ego_nearest_param_; 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/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 609a09e00d219..bdd1bb5ee276e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -18,6 +18,7 @@ #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -25,20 +26,12 @@ #include #include -#include -#include - #include #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::Float32Stamped; - class OptimizationBasedPlanner : public PlannerInterface { public: @@ -77,8 +70,7 @@ class OptimizationBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::PointStamped & point); boost::optional calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose); + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); geometry_msgs::msg::Pose transformBaseLink2Center( const geometry_msgs::msg::Pose & pose_base_link); @@ -102,6 +94,11 @@ class OptimizationBasedPlanner : public PlannerInterface rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + // Resampling Parameter double dense_resampling_time_interval_; double sparse_resampling_time_interval_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp deleted file mode 100644 index ae8a909d3bb51..0000000000000 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// 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__DEBUG_VALUES_HPP_ -#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ - -#include - -class DebugValues -{ -public: - enum class TYPE { - // current - CURRENT_VELOCITY = 0, - CURRENT_ACCELERATION, - CURRENT_JERK, // ignored - // stop - STOP_CURRENT_OBJECT_DISTANCE = 3, - STOP_CURRENT_OBJECT_VELOCITY, - STOP_TARGET_OBJECT_DISTANCE, - STOP_TARGET_VELOCITY, // ignored - STOP_TARGET_ACCELERATION, - STOP_TARGET_JERK, // ignored - STOP_ERROR_OBJECT_DISTANCE, - // cruise - CRUISE_CURRENT_OBJECT_DISTANCE = 10, - CRUISE_CURRENT_OBJECT_VELOCITY, - CRUISE_TARGET_OBJECT_DISTANCE, - CRUISE_TARGET_VELOCITY, - CRUISE_TARGET_ACCELERATION, - CRUISE_TARGET_JERK, - CRUISE_ERROR_OBJECT_DISTANCE, - - 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 getValuesIdx(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)> getValues() const { return values_; } - /** - * @brief set the given type to the given value - * @param [in] type TYPE of the value - * @param [in] value value to set - */ - void setValues(const TYPE type, const double val) { values_.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 setValues(const int type, const double val) { values_.at(type) = val; } - - void resetValues() { values_.fill(0.0); } - -private: - static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); - std::array(TYPE::SIZE)> values_; -}; - -#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_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 c2308adc1ca17..f94516407e3c5 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 @@ -102,6 +102,7 @@ class PIDBasedPlanner : public PlannerInterface double output_ratio_during_accel; double vel_to_acc_weight; bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; }; VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; @@ -131,6 +132,7 @@ class PIDBasedPlanner : public PlannerInterface std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; // lpf for output + std::shared_ptr lpf_output_vel_ptr_; std::shared_ptr lpf_output_acc_ptr_; std::shared_ptr lpf_output_jerk_ptr_; 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 a01d3589245b5..06d7710290081 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 @@ -18,31 +18,15 @@ #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/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "visualization_msgs/msg/marker_array.hpp" #include #include #include -using autoware_adapi_v1_msgs::msg::VelocityFactor; -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; - class PlannerInterface { public: @@ -53,8 +37,7 @@ class PlannerInterface vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param) { - stop_reasons_pub_ = - node.create_publisher("~/output/stop_reasons", 1); + stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = @@ -63,14 +46,10 @@ class PlannerInterface PlannerInterface() = default; - void setParams( - const bool is_showing_debug_info, const double min_behavior_stop_margin, - const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold) + void setParam(const bool is_showing_debug_info, const double min_behavior_stop_margin) { is_showing_debug_info_ = is_showing_debug_info; min_behavior_stop_margin_ = min_behavior_stop_margin; - nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold; - nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold; } Trajectory generateStopTrajectory( @@ -80,31 +59,10 @@ class PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, DebugData & debug_data) = 0; - void updateCommonParam(const std::vector & parameters) - { - auto & i = longitudinal_info_; - - tier4_autoware_utils::updateParam(parameters, "common.max_accel", i.max_accel); - tier4_autoware_utils::updateParam(parameters, "common.min_accel", i.min_accel); - tier4_autoware_utils::updateParam(parameters, "common.max_jerk", i.max_jerk); - tier4_autoware_utils::updateParam(parameters, "common.min_jerk", i.min_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.max_accel", i.limit_max_accel); - tier4_autoware_utils::updateParam(parameters, "limit.min_accel", i.limit_min_accel); - tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", i.limit_max_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", i.limit_min_jerk); - tier4_autoware_utils::updateParam( - parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss); - tier4_autoware_utils::updateParam( - parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss); - tier4_autoware_utils::updateParam(parameters, "common.idling_time", i.idling_time); - } - - virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} - - // TODO(shimizu) remove this function - void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj) + void onParam(const std::vector & parameters) { - smoothed_trajectory_ptr_ = traj; + updateCommonParam(parameters); + updateParam(parameters); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -122,11 +80,9 @@ class PlannerInterface bool is_showing_debug_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; // Publishers - rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; @@ -138,9 +94,6 @@ class PlannerInterface // debug info StopPlanningDebugInfo stop_planning_debug_info_; - // TODO(shimizu) remove these parameters - Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; - double calcDistanceToCollisionPoint( const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::Point & collision_point); @@ -155,6 +108,13 @@ class PlannerInterface return rss_dist_with_margin; } + void updateCommonParam(const std::vector & parameters) + { + longitudinal_info_.onParam(parameters); + } + + virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} + size_t findEgoIndex(const Trajectory & traj, const geometry_msgs::msg::Pose & ego_pose) const { const auto traj_points = motion_utils::convertToTrajectoryPointArray(traj); diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 651c63d4593a5..6e468bd82defa 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -15,14 +15,10 @@ #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" - #include #include @@ -36,39 +32,36 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_points, const double max_dist = std::numeric_limits::max()); std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward); std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width); geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward); + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp new file mode 100644 index 0000000000000..7f42512646f3c --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -0,0 +1,61 @@ +// 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__TYPE_ALIAS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ + +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_planning_msgs/msg/stop_factor.hpp" +#include "tier4_planning_msgs/msg/stop_reason_array.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using tier4_planning_msgs::msg::StopSpeedExceeded; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +#endif // OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 553f4ecfcfe9b..b509192d34371 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -17,18 +17,11 @@ #include "common_structs.hpp" #include "motion_utils/motion_utils.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -37,36 +30,27 @@ namespace obstacle_cruise_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; - -bool isVehicle(const uint8_t label); - -visualization_msgs::msg::Marker getObjectMarker( +Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b); boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length); + const Trajectory & traj, const size_t start_idx, const double target_length); boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction); + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction); boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles); - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id); + const Trajectory & traj, const std::vector & target_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 71ec3e5ef200c..8b344affd9c7f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -29,7 +29,7 @@ namespace { -VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time & current_time) +VelocityLimitClearCommand createVelocityLimitClearCommandMessage(const rclcpp::Time & current_time) { VelocityLimitClearCommand msg; msg.stamp = current_time; @@ -38,23 +38,11 @@ VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time return msg; } -// TODO(murooka) make this function common -size_t findExtendedNearestIndex( - const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist, - const double max_yaw) -{ - const auto nearest_idx = motion_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); - if (nearest_idx) { - return nearest_idx.get(); - } - return motion_utils::findNearestIndex(traj.points, pose.position); -} - -Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx) +Trajectory trimTrajectoryFrom(const Trajectory & input, const double start_idx) { Trajectory output{}; - for (size_t i = nearest_idx; i < input.points.size(); ++i) { + for (size_t i = start_idx; i < input.points.size(); ++i) { output.points.push_back(input.points.at(i)); } @@ -192,32 +180,25 @@ 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), - in_objects_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; // subscriber - trajectory_sub_ = create_subscription( + traj_sub_ = create_subscription( "~/input/trajectory", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); - smoothed_trajectory_sub_ = create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onSmoothedTrajectory, this, _1)); objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onObjects, this, _1)); + "~/input/objects", rclcpp::QoS{1}, + [this](const PredictedObjects::ConstSharedPtr msg) { in_objects_ptr_ = msg; }); odom_sub_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + [this](const Odometry::ConstSharedPtr msg) { current_odom_ptr_ = msg; }); acc_sub_ = create_subscription( "~/input/acceleration", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1)); + [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_accel_ptr_ = msg; }); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -225,6 +206,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); + + // debug publisher 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); @@ -234,50 +217,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_cruise_planning_info_pub_ = create_publisher("~/debug/cruise_planning_info", 1); - // longitudinal_info - const auto longitudinal_info = [&]() { - const double max_accel = declare_parameter("normal.max_acc"); - const double min_accel = declare_parameter("normal.min_acc"); - const double max_jerk = declare_parameter("normal.max_jerk"); - const double min_jerk = declare_parameter("normal.min_jerk"); - const double limit_max_accel = declare_parameter("limit.max_acc"); - const double limit_min_accel = declare_parameter("limit.min_acc"); - const double limit_max_jerk = declare_parameter("limit.max_jerk"); - const double limit_min_jerk = declare_parameter("limit.min_jerk"); - - const double min_ego_accel_for_rss = declare_parameter("common.min_ego_accel_for_rss"); - const double min_object_accel_for_rss = - declare_parameter("common.min_object_accel_for_rss"); - const double idling_time = declare_parameter("common.idling_time"); - const double safe_distance_margin = declare_parameter("common.safe_distance_margin"); - 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, - max_jerk, - min_jerk, - limit_max_accel, - limit_min_accel, - limit_max_jerk, - limit_min_jerk, - idling_time, - min_ego_accel_for_rss, - min_object_accel_for_rss, - safe_distance_margin, - terminal_safe_distance_margin}; - }(); - - const auto ego_nearest_param = [&]() { - const double ego_nearest_dist_threshold = - declare_parameter("ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + const auto longitudinal_info = LongitudinalInfo(*this); - return EgoNearestParam(ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - }(); + ego_nearest_param_ = EgoNearestParam(*this); is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); disable_stop_planning_ = declare_parameter("common.disable_stop_planning"); @@ -357,26 +299,20 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else { std::logic_error("Designated algorithm is not supported."); } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); - nearest_dist_deviation_threshold_ = - declare_parameter("common.nearest_dist_deviation_threshold"); - nearest_yaw_deviation_threshold_ = - declare_parameter("common.nearest_yaw_deviation_threshold"); obstacle_velocity_threshold_from_cruise_to_stop_ = declare_parameter("common.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise_ = declare_parameter("common.obstacle_velocity_threshold_from_stop_to_cruise"); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); } { // cruise obstacle type @@ -432,8 +368,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); } } - // wait for first self pose - self_pose_listener_.waitForFirstPose(); // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( @@ -454,14 +388,11 @@ ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlann rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( const std::vector & parameters) { - planner_ptr_->updateCommonParam(parameters); - planner_ptr_->updateParam(parameters); + planner_ptr_->onParam(parameters); tier4_autoware_utils::updateParam( parameters, "common.is_showing_debug_info", is_showing_debug_info_); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); tier4_autoware_utils::updateParam( parameters, "common.disable_stop_planning", disable_stop_planning_); @@ -516,59 +447,34 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( return result; } -void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr msg) -{ - in_objects_ptr_ = msg; -} - -void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) -{ - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = msg->header; - current_twist_ptr_->twist = msg->twist.twist; -} - -void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - current_accel_ptr_ = std::make_unique(); - current_accel_ptr_->header = msg->header; - current_accel_ptr_->accel = msg->accel.accel; -} - -void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - planner_ptr_->setSmoothedTrajectory(msg); -} - void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); - // check if subscribed variables are ready - if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { + if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_) { return; } stop_watch_.tic(__func__); + const auto current_pose = current_odom_ptr_->pose.pose; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + // Get Target Obstacles DebugData debug_data; const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; - const auto target_obstacles = getTargetObstacles( - *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, is_driving_forward_, - debug_data); + const auto target_obstacles = + getTargetObstacles(*msg, current_pose, current_vel, is_driving_forward_, debug_data); // create data for stop - const auto stop_data = - createStopData(*msg, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + const auto stop_data = createStopData(*msg, current_pose, target_obstacles, is_driving_forward_); // stop planning const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data); // create data for cruise const auto cruise_data = - createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + createCruiseData(stop_traj, current_pose, target_obstacles, is_driving_forward_); // cruise planning boost::optional vel_limit; @@ -610,8 +516,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -651,8 +557,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -697,8 +603,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto current_time = now(); const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp); - const size_t ego_idx = findExtendedNearestIndex( - traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + const size_t ego_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); // calculate decimated trajectory const auto trimmed_traj = trimTrajectoryFrom(traj, ego_idx); @@ -718,7 +624,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( std::vector target_obstacles; for (const auto & predicted_object : predicted_objects.objects) { - const auto object_id = toHexString(predicted_object.object_id).substr(0, 4); + const auto object_id = + tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); // filter object whose label is not cruised or stopped const bool is_target_obstacle = isStopObstacle(predicted_object.classification.front().label) || @@ -943,7 +850,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { - return obstacle_cruise_utils::toHexString(predicted_object.object_id) == + return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_obstacle_ptr_->uuid; }); @@ -1025,23 +932,28 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( const boost::optional & vel_limit) { if (vel_limit) { + // publish velocity limit vel_limit_pub_->publish(vel_limit.get()); need_to_clear_vel_limit_ = true; - } else { - if (need_to_clear_vel_limit_) { - const auto clear_vel_limit_msg = createVelocityLimitClearCommandMsg(now()); - clear_vel_limit_pub_->publish(clear_vel_limit_msg); - need_to_clear_vel_limit_ = false; - } + return; } + + if (!need_to_clear_vel_limit_) { + return; + } + + // clear velocity limit + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now()); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_ = false; } void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const { stop_watch_.tic(__func__); + // 1. publish debug marker MarkerArray debug_marker; - const auto current_time = now(); // obstacles to cruise for (size_t i = 0; i < debug_data.obstacles_to_cruise.size(); ++i) { @@ -1067,7 +979,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) { // footprint polygons auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, + "map", now(), "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)); @@ -1089,7 +1001,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) { // 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, Marker::SPHERE, + "map", now(), "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); @@ -1099,11 +1011,11 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) debug_marker_pub_->publish(debug_marker); - // virtual wall for cruise and stop + // 2. publish 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); - // print calculation time + // 3. print calculation time const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", @@ -1112,14 +1024,12 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) void ObstacleCruisePlannerNode::publishDebugInfo() const { - const auto current_time = now(); - // stop - const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time); + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); debug_stop_planning_info_pub_->publish(stop_debug_msg); // cruise - const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time); + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); debug_cruise_planning_info_pub_->publish(cruise_debug_msg); } diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 8948cd2de8eb3..262e2b619d36e 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -39,6 +39,10 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param) { + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + // parameter dense_resampling_time_interval_ = node.declare_parameter("optimization_based_planner.dense_resampling_time_interval"); @@ -85,8 +89,7 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); boundary_pub_ = node.create_publisher("~/boundary", 1); - debug_wall_marker_pub_ = - node.create_publisher("~/debug/wall_marker", 1); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); } Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( @@ -284,8 +287,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const auto & input_traj = planner_data.traj; const double vehicle_speed{std::abs(current_vel)}; const auto current_closest_point = motion_utils::calcInterpolatedPoint( - input_traj, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; double initial_vel{}; @@ -301,11 +304,11 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - *smoothed_trajectory_ptr_, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + *smoothed_trajectory_ptr_, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); } else { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - prev_traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + prev_traj, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); } // when velocity tracking deviation is large @@ -328,8 +331,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( - input_traj.points, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj.points, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { initial_vel = engage_velocity_; initial_acc = engage_acceleration_; @@ -363,8 +366,8 @@ bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerDa { // If goal is close and current velocity is low, we don't optimize trajectory const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) { return true; } @@ -437,7 +440,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( planner_data.traj.points, planner_data.current_pose.position, min_slow_down_point_length); if (marker_pose) { - visualization_msgs::msg::MarkerArray wall_msg; + MarkerArray wall_msg; if (obj.has_stopped) { const auto markers = motion_utils::createStopVirtualWallMarker( @@ -590,19 +593,18 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } boost::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose) + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) { const auto traj_length = motion_utils::calcSignedArcLength( - traj.points, current_pose, traj.points.size() - 1, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + traj.points, current_pose, traj.points.size() - 1, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!traj_length) { return {}; } const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( - traj.points, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { return std::min(traj_length.get(), dist_to_closest_stop_point.get()); } 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 cfd9dd24c543b..8925759ba7e47 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 @@ -82,6 +82,9 @@ PIDBasedPlanner::PIDBasedPlanner( 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"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); } { // velocity insertion based planner @@ -139,6 +142,7 @@ PIDBasedPlanner::PIDBasedPlanner( 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_vel_ptr_ = std::make_shared(0.5); lpf_output_acc_ptr_ = std::make_shared(0.5); lpf_output_jerk_ptr_ = std::make_shared(0.5); } @@ -300,10 +304,12 @@ Trajectory PIDBasedPlanner::planCruise( : 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 size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, dist_to_rss_wall, ego_idx); 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); + planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, + 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); // cruise obstalce @@ -324,6 +330,7 @@ Trajectory PIDBasedPlanner::planCruise( prev_target_acc_ = {}; lpf_normalized_error_cruise_dist_ptr_->reset(); lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); lpf_output_jerk_ptr_->reset(); // delete marker @@ -356,11 +363,15 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( return pid_output_vel; }(); - const double positive_target_vel = - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); + const double positive_target_vel = lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); // calculate target acceleration const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + double target_acc = p.vel_to_acc_weight * additional_vel; // apply acc limit @@ -450,11 +461,6 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory( 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( @@ -462,14 +468,14 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory( // 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_); - } + // 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_); + prev_traj_, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); }(); const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; const double a0 = prev_traj_closest_point.acceleration_mps2; @@ -580,9 +586,7 @@ Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory( } 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_); + const size_t ego_seg_idx = findEgoIndex(acc_limited_traj, start_pose); double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) { if (i != ego_seg_idx) { @@ -636,6 +640,10 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet 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); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", + p.disable_target_acceleration); } { // velocity insertion based planner diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index adee76f67c065..420b72d2bf0aa 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -35,26 +35,25 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( header.stamp = current_time; // create stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; + StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_points.front().point; stop_factor_point.z = stop_pose.position.z; stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; stop_reason_msg.stop_factors.emplace_back(stop_factor); // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; } -tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( - const rclcpp::Time & current_time) +StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) { // create header std_msgs::msg::Header header; @@ -62,11 +61,11 @@ tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( header.stamp = current_time; // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; @@ -145,8 +144,8 @@ Trajectory PlannerInterface::generateStopTrajectory( planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point); const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, 0, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!negative_dist_to_ego) { // delete marker const auto markers = @@ -161,9 +160,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // we set closest_obstacle_stop_distance to closest_behavior_stop_distance const double margin_from_obstacle = [&]() { const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + findEgoSegmentIndex(planner_data.traj, planner_data.current_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1); @@ -242,8 +239,9 @@ 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_DISTANCE, + closest_obstacle_dist - abs_ego_offset); // TODO(murooka) stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); @@ -264,7 +262,7 @@ double PlannerInterface::calcDistanceToCollisionPoint( const auto dist_to_collision_point = motion_utils::calcSignedArcLength( planner_data.traj.points, planner_data.current_pose, collision_point, - nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_collision_point) { return dist_to_collision_point.get() - offset; diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index d0b635951e074..28fa21646062a 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -88,9 +88,8 @@ Polygon2d createOneStepPolygon( namespace polygon_utils { boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_geom_points, const double max_dist) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose.pose, shape); @@ -128,10 +127,9 @@ boost::optional getCollisionIndex( } std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist, const double max_prediction_time_for_collision_check) @@ -172,11 +170,10 @@ std::vector getCollisionPoints( } std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward) { @@ -200,8 +197,8 @@ std::vector willCollideWithSurroundObstacle( } std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width) { std::vector polygons; @@ -223,8 +220,8 @@ std::vector createOneStepPolygons( geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward) + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward) { std::vector segment_points(2); if (first_within_idx == 0) { diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 8aecbf393bcc0..45cc0a2e6c89d 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -18,12 +18,6 @@ namespace obstacle_cruise_utils { -bool isVehicle(const uint8_t label) -{ - return label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || - label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE; -} - visualization_msgs::msg::Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b) @@ -41,8 +35,7 @@ visualization_msgs::msg::Marker getObjectMarker( } boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length) + const Trajectory & traj, const size_t start_idx, const double target_length) { if (traj.points.empty()) { return {}; @@ -77,8 +70,8 @@ boost::optional calcForwardPose( } boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { const double rel_time = (current_time - obj_base_time).seconds(); if (rel_time < 0.0) { @@ -89,8 +82,8 @@ boost::optional getCurrentObjectPoseFromPredictedPath( } boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { if (predicted_paths.empty()) { return boost::none; @@ -98,19 +91,14 @@ boost::optional getCurrentObjectPoseFromPredictedPaths // Get the most reliable path const auto predicted_path = std::max_element( predicted_paths.begin(), predicted_paths.end(), - []( - const autoware_auto_perception_msgs::msg::PredictedPath & a, - const autoware_auto_perception_msgs::msg::PredictedPath & b) { - return a.confidence < b.confidence; - }); + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); } geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction) + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction) { if (!use_prediction) { geometry_msgs::msg::PoseStamped current_pose; @@ -119,7 +107,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( return current_pose; } - std::vector predicted_paths; + std::vector predicted_paths; for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); } @@ -143,8 +131,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( } boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles) + const Trajectory & traj, const std::vector & target_obstacles) { if (target_obstacles.empty()) { return boost::none; @@ -167,13 +154,4 @@ boost::optional getClosestStopObstacle( } return closest_stop_obstacle; } - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} } // namespace obstacle_cruise_utils