From e9fd333ac15532e5320180644fbd4f6f9c9d0863 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 18 Dec 2022 16:14:05 +0900 Subject: [PATCH 1/6] refactor obstacle_cruise_planner Signed-off-by: Takayuki Murooka --- .../common_structs.hpp | 17 +--- .../include/obstacle_cruise_planner/node.hpp | 43 ++-------- .../optimization_based_planner.hpp | 12 +-- .../pid_based_planner/debug_values.hpp | 79 ------------------- .../planner_interface.hpp | 23 +----- .../obstacle_cruise_planner/polygon_utils.hpp | 35 ++++---- .../obstacle_cruise_planner/type_alias.hpp | 61 ++++++++++++++ .../include/obstacle_cruise_planner/utils.hpp | 32 +++----- planning/obstacle_cruise_planner/src/node.cpp | 50 ++++-------- .../optimization_based_planner.cpp | 8 +- .../src/planner_interface.cpp | 22 +++--- .../src/polygon_utils.cpp | 29 +++---- .../obstacle_cruise_planner/src/utils.cpp | 27 +++---- 13 files changed, 151 insertions(+), 287 deletions(-) delete mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp create mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp 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..bdad65c27ae2f 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,25 +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) @@ -85,7 +76,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; @@ -115,8 +106,8 @@ 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; }; 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..91bec46eb69b5 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); @@ -126,9 +95,9 @@ 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_; @@ -145,8 +114,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // 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_; 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..5301e7fea6326 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); 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/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index a01d3589245b5..9fea75c1270ee 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_ = @@ -126,7 +109,7 @@ class PlannerInterface 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_; 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..17fe73a10ef8d 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,34 +30,29 @@ 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); + const Trajectory & traj, const std::vector & target_obstacles); std::string toHexString(const unique_identifier_msgs::msg::UUID & id); diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 71ec3e5ef200c..c6e6c0f0a0ece 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; @@ -192,13 +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), - in_objects_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; @@ -211,13 +207,14 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & "/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); @@ -516,25 +513,6 @@ 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); @@ -545,9 +523,10 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms 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_ || !current_pose_ptr) { return; } + const double current_vel = current_odom_ptr_->twist.twist.linear.x; stop_watch_.tic(__func__); @@ -555,9 +534,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms 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_ptr->pose, current_vel, is_driving_forward_, debug_data); // create data for stop const auto stop_data = @@ -610,8 +588,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 +629,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; @@ -1029,7 +1007,7 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( need_to_clear_vel_limit_ = true; } else { if (need_to_clear_vel_limit_) { - const auto clear_vel_limit_msg = createVelocityLimitClearCommandMsg(now()); + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now()); clear_vel_limit_pub_->publish(clear_vel_limit_msg); need_to_clear_vel_limit_ = false; } 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..1d63c5af02c8a 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 @@ -85,8 +85,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( @@ -437,7 +436,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,8 +589,7 @@ 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_, diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index adee76f67c065..b93242e1aeae1 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; @@ -242,8 +241,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); 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..219a691b0fbc3 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -41,8 +41,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 +76,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 +88,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 +97,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 +113,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 +137,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; From d47132ca341ddb4a176d16286c7147732b472b8a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 19:13:18 +0900 Subject: [PATCH 2/6] update Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/README.md | 31 ++-- .../config/obstacle_cruise_planner.param.yaml | 2 - .../common_structs.hpp | 55 ++++++- .../include/obstacle_cruise_planner/node.hpp | 11 +- .../optimization_based_planner.hpp | 5 + .../planner_interface.hpp | 45 ++---- .../include/obstacle_cruise_planner/utils.hpp | 2 - planning/obstacle_cruise_planner/src/node.cpp | 147 +++++------------- .../optimization_based_planner.cpp | 28 ++-- .../pid_based_planner/pid_based_planner.cpp | 18 +-- .../src/planner_interface.cpp | 10 +- .../obstacle_cruise_planner/src/utils.cpp | 6 - 12 files changed, 153 insertions(+), 207 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 5f372a5baed34..084b538f39ddb 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 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..914e15a997096 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -12,8 +12,6 @@ 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] - 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] obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] 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 bdad65c27ae2f..ac9326ea7d16f 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 @@ -86,6 +86,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; @@ -94,9 +138,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; }; @@ -114,10 +162,13 @@ struct DebugData 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 91bec46eb69b5..e52c2671fea5e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -80,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_; @@ -103,15 +101,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node 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}; Odometry::ConstSharedPtr current_odom_ptr_{nullptr}; @@ -164,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 5301e7fea6326..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 @@ -94,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/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 9fea75c1270ee..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 @@ -46,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( @@ -63,31 +59,10 @@ class PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, DebugData & debug_data) = 0; - void updateCommonParam(const std::vector & parameters) + void onParam(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) - { - smoothed_trajectory_ptr_ = traj; + updateCommonParam(parameters); + updateParam(parameters); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -105,8 +80,6 @@ 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_; @@ -121,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); @@ -138,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/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 17fe73a10ef8d..01d3fc979c2cc 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -30,8 +30,6 @@ namespace obstacle_cruise_utils { -bool isVehicle(const uint8_t label); - 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); diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index c6e6c0f0a0ece..d91c30a4854d6 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -38,23 +38,11 @@ VelocityLimitClearCommand createVelocityLimitClearCommandMessage(const rclcpp::T 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)); } @@ -194,18 +182,14 @@ namespace motion_planning { ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), - self_pose_listener_(this), 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}, [this](const PredictedObjects::ConstSharedPtr msg) { in_objects_ptr_ = msg; }); @@ -222,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); @@ -231,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 longitudinal_info = LongitudinalInfo(*this); - 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"); - - 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"); @@ -354,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 @@ -429,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( @@ -451,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_); @@ -513,40 +447,34 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( return result; } -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_odom_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { + if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_) { return; } - const double current_vel = current_odom_ptr_->twist.twist.linear.x; 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_vel, is_driving_forward_, debug_data); + 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; @@ -675,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); @@ -1003,23 +931,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 = createVelocityLimitClearCommandMessage(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) { @@ -1045,7 +978,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)); @@ -1067,7 +1000,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); @@ -1077,11 +1010,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]", @@ -1090,14 +1023,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 1d63c5af02c8a..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"); @@ -283,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{}; @@ -300,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 @@ -327,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_; @@ -362,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; } @@ -592,15 +596,15 @@ boost::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurren 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..19bf69bc2b235 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 @@ -462,14 +462,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 +580,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) { diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index b93242e1aeae1..420b72d2bf0aa 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -144,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 = @@ -160,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); @@ -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/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 219a691b0fbc3..51a02aa776a40 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) From 5f2da1da2f0f39567c375a22dea89861e941845f Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 19:56:00 +0900 Subject: [PATCH 3/6] update Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/README.md | 27 ++++++++-- .../config/obstacle_cruise_planner.param.yaml | 51 ++++++++++++++----- .../common_structs.hpp | 14 +---- .../include/obstacle_cruise_planner/utils.hpp | 2 - planning/obstacle_cruise_planner/src/node.cpp | 5 +- .../pid_based_planner/pid_based_planner.cpp | 5 -- .../obstacle_cruise_planner/src/utils.cpp | 9 ---- 7 files changed, 66 insertions(+), 47 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 084b538f39ddb..17eafb7f5ce72 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -175,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_additional = v_pid > 0 ? v_pid * w_acc : v_pid +v_target = v_target = max(v_ego + v_additional, v_min_cruise) +$$ + +| Variable | Description | +| -------------- | --------------------------------------- | +| `d` | actual distane to obstacle | +| `d_rss` | ideal distance to obstacle based on RSS | +| `lpf(val)` | apply low-pass filter to `val` | +| `v_min_cruise` | `min_cruise_target_vel` | +| `w_acc` | `output_ratio_during_accel` | +| `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 914e15a997096..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,20 @@ 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] obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] @@ -23,7 +28,7 @@ bus: true trailer: true motorcycle: true - bicycle: false + bicycle: true pedestrian: false stop_obstacle_type: @@ -60,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 @@ -70,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 ac9326ea7d16f..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 @@ -25,18 +25,6 @@ #include #include -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( @@ -50,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) { 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 01d3fc979c2cc..b509192d34371 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -52,8 +52,6 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( boost::optional getClosestStopObstacle( const Trajectory & traj, const std::vector & target_obstacles); -std::string toHexString(const unique_identifier_msgs::msg::UUID & id); - template size_t getIndexWithLongitudinalOffset( const T & points, const double longitudinal_offset, boost::optional start_idx) diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index d91c30a4854d6..8b344affd9c7f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -624,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) || @@ -849,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; }); 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 19bf69bc2b235..56491004775f9 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 @@ -450,11 +450,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( diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 51a02aa776a40..45cc0a2e6c89d 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -154,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 From c0d68b5bb80be667c0f99c6dd7f7e4b77ea5a11e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 14 Dec 2022 08:38:32 +0900 Subject: [PATCH 4/6] add vel lpf and disable_target_acceleration Signed-off-by: Takayuki Murooka --- .../pid_based_planner/pid_based_planner.hpp | 2 ++ .../src/pid_based_planner/pid_based_planner.cpp | 16 +++++++++++++++- 2 files changed, 17 insertions(+), 1 deletion(-) 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/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 56491004775f9..0cb74755e6dac 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); } @@ -324,6 +328,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 @@ -357,10 +362,15 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( }(); const double positive_target_vel = - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_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 @@ -629,6 +639,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 From 52a587c3a9cbdec01a7ce3e1c8baf4ad3d8b194d Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 20:08:41 +0900 Subject: [PATCH 5/6] update Signed-off-by: Takayuki Murooka --- .../src/pid_based_planner/pid_based_planner.cpp | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) 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 0cb74755e6dac..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 @@ -83,8 +83,8 @@ PIDBasedPlanner::PIDBasedPlanner( 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_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); } { // velocity insertion based planner @@ -304,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 @@ -361,9 +363,8 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( return pid_output_vel; }(); - const double positive_target_vel = - lpf_output_vel_ptr_->filter( - 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 = [&]() { From 3e658a99d568eca8f95094333a4a4a4ae38556d4 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 20 Dec 2022 15:06:57 +0900 Subject: [PATCH 6/6] fix README.md Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/README.md | 30 +++++++++++----------- 1 file changed, 15 insertions(+), 15 deletions(-) diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 17eafb7f5ce72..e3ddbfb1b157a 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -175,7 +175,7 @@ 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_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}}, +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_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. @@ -190,22 +190,22 @@ These values are parameterized as follows. Other common values such as ego's min 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_additional = v_pid > 0 ? v_pid * w_acc : v_pid -v_target = v_target = max(v_ego + v_additional, v_min_cruise) +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 | -| `lpf(val)` | apply low-pass filter to `val` | -| `v_min_cruise` | `min_cruise_target_vel` | -| `w_acc` | `output_ratio_during_accel` | -| `pid(val)` | apply pid to `val` | +| 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