From fc1d820347eb5c44801fac1a7208636cfde8e85c Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Sat, 17 Dec 2022 20:28:47 +0900 Subject: [PATCH 01/37] refactor(multi_object_tracker): clean up noisy tf warning message on terminal (#2511) * refactor(multi_object_tracker): clean noisy tf warning message Signed-off-by: Takamasa Horibe * enable code Signed-off-by: Takamasa Horibe Signed-off-by: Takamasa Horibe --- .../multi_object_tracker/src/multi_object_tracker_core.cpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp index 0d88a905394e7..3ef9c66db9665 100644 --- a/perception/multi_object_tracker/src/multi_object_tracker_core.cpp +++ b/perception/multi_object_tracker/src/multi_object_tracker_core.cpp @@ -45,6 +45,13 @@ boost::optional getTransformAnonymous( const std::string & target_frame_id, const rclcpp::Time & time) { try { + // check if the frames are ready + std::string errstr; // This argument prevents error msg from being displayed in the terminal. + if (!tf_buffer.canTransform( + target_frame_id, source_frame_id, tf2::TimePointZero, tf2::Duration::zero(), &errstr)) { + return boost::none; + } + geometry_msgs::msg::TransformStamped self_transform_stamped; self_transform_stamped = tf_buffer.lookupTransform( /*target*/ target_frame_id, /*src*/ source_frame_id, time, From 9ecc3a26a142febc6d59de3891758a3f38cc4d20 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 18 Dec 2022 13:23:32 +0900 Subject: [PATCH 02/37] feat(obstacle_cruies_planner): improve pid_based cruise planner (#2507) * feat(obstacle_cruies_planner): improve pid_based cruise planner Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * update param in tier4_planning_launch Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 49 +- planning/obstacle_cruise_planner/README.md | 10 +- ...plot_juggler_obstacle_velocity_planner.xml | 36 +- .../common_structs.hpp | 6 +- .../include/obstacle_cruise_planner/node.hpp | 17 +- .../cruise_planning_debug_info.hpp | 97 +++ .../pid_based_planner/pid_based_planner.hpp | 96 ++- .../planner_interface.hpp | 15 + .../stop_planning_debug_info.hpp | 88 +++ planning/obstacle_cruise_planner/src/node.cpp | 62 +- .../pid_based_planner/pid_based_planner.cpp | 669 ++++++++++++++---- .../src/planner_interface.cpp | 16 + 12 files changed, 939 insertions(+), 222 deletions(-) create mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp create mode 100644 planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 520637abed775..dfc2b2e6ddb44 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -3,15 +3,18 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: true + is_showing_debug_info: false + disable_stop_planning: false # true # longitudinal info - idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] @@ -25,7 +28,7 @@ bus: true trailer: true motorcycle: true - bicycle: false + bicycle: true pedestrian: false stop_obstacle_type: @@ -62,7 +65,7 @@ stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle ignored_outside_obstacle_type: - unknown: false + unknown: true car: false truck: false bus: false @@ -72,21 +75,41 @@ pedestrian: true pid_based_planner: - # use_predicted_obstacle_pose: false + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic - # PID gains to keep safe distance with the front vehicle - kp: 2.5 - ki: 0.0 - kd: 2.3 + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + disable_target_acceleration: true + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 - lpf_cruise_gain: 0.2 + lpf_normalized_error_cruise_dist_gain: 0.2 optimization_based_planner: dense_resampling_time_interval: 0.2 diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md index 4d114c47bfaf0..5f372a5baed34 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -56,15 +56,15 @@ struct ObstacleCruisePlannerData struct TargetObstacle { rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; - Shape shape; + std::string uuid; std::vector predicted_paths; - geometry_msgs::msg::Point collision_point; + std::vector collision_points; + bool has_stopped; }; ``` diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml index 634268b93f2de..2e7eeee211963 100644 --- a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml @@ -10,17 +10,17 @@ - - - + + + - - + + @@ -29,17 +29,17 @@ - - - + + + - - + + @@ -56,17 +56,17 @@ - - - + + + - - + + @@ -75,9 +75,9 @@ - - - + + + diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 724e7dbf1372b..43949c55b21a5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -58,7 +58,6 @@ struct TargetObstacle pose = object.kinematics.initial_pose_with_covariance.pose; velocity_reliable = true; velocity = aligned_velocity; - is_classified = true; classification = object.classification.at(0); uuid = toHexString(object.object_id); @@ -72,11 +71,10 @@ struct TargetObstacle } rclcpp::Time time_stamp; - bool orientation_reliable; geometry_msgs::msg::Pose pose; + bool orientation_reliable; + double velocity; bool velocity_reliable; - float velocity; - bool is_classified; ObjectClassification classification; std::string uuid; std::vector predicted_paths; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 7570b1ad12274..8ce9ee3d7e925 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -100,7 +100,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const PredictedObject & predicted_object, const Trajectory & traj, const bool is_driving_forward); void publishVelocityLimit(const boost::optional & vel_limit); - void publishDebugData(const DebugData & debug_data) const; + void publishDebugMarker(const DebugData & debug_data) const; + void publishDebugInfo() const; void publishCalculationTime(const double calculation_time) const; bool isCruiseObstacle(const uint8_t label); @@ -128,6 +129,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_marker_pub_; rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber @@ -141,10 +144,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node tier4_autoware_utils::SelfPoseListener self_pose_listener_; // data for callback functions - PredictedObjects::ConstSharedPtr in_objects_ptr_; - geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_; - - geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_; + PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr}; + geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr}; + geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -160,7 +162,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node stop_watch_; // planner - std::unique_ptr planner_ptr_; + std::unique_ptr planner_ptr_{nullptr}; // previous closest obstacle std::shared_ptr prev_closest_obstacle_ptr_{nullptr}; @@ -195,8 +197,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool need_to_clear_vel_limit_{false}; bool is_driving_forward_{true}; + bool disable_stop_planning_{false}; std::vector prev_target_obstacles_; + + std::shared_ptr lpf_acc_ptr_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..3ab5dd0a011d9 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,97 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index a6ed9d3e66bdf..c2308adc1ca17 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -15,13 +15,12 @@ #ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ #define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ -#include "obstacle_cruise_planner/pid_based_planner/debug_values.hpp" +#include "obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" -#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -29,26 +28,25 @@ #include #include -using tier4_debug_msgs::msg::Float32MultiArrayStamped; - class PIDBasedPlanner : public PlannerInterface { public: struct CruiseObstacleInfo { CruiseObstacleInfo( - const TargetObstacle & obstacle_arg, const double dist_to_cruise_arg, - const double normalized_dist_to_cruise_arg, double dist_to_obstacle_arg) + const TargetObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) : obstacle(obstacle_arg), - dist_to_cruise(dist_to_cruise_arg), - normalized_dist_to_cruise(normalized_dist_to_cruise_arg), - dist_to_obstacle(dist_to_obstacle_arg) + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) { } + TargetObstacle obstacle; - double dist_to_cruise; - double normalized_dist_to_cruise; + double error_cruise_dist; double dist_to_obstacle; + double target_dist_to_obstacle; }; PIDBasedPlanner( @@ -66,39 +64,83 @@ class PIDBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & cruise_obstacle_info); - void planCruise( + Float32MultiArrayStamped getCruisePlanningDebugMessage( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convertToMessage(current_time); + } + +private: + boost::optional calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data); + Trajectory planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data); - VelocityLimit doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_walls_marker); - void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const; + // velocity limit based planner + VelocityLimit doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + + // velocityinsertion based planner + Trajectory doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, + const CruiseObstacleInfo & cruise_obstacle_info); + Trajectory getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const; // ROS parameters double min_accel_during_cruise_; - double vel_to_acc_weight_; double min_cruise_target_vel_; - // bool use_predicted_obstacle_pose_; - // pid controller - std::unique_ptr pid_controller_; - double output_ratio_during_accel_; + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; // stop watch tier4_autoware_utils::StopWatch< std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> stop_watch_; - // publisher - rclcpp::Publisher::SharedPtr debug_values_pub_; + boost::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + Trajectory prev_traj_; - boost::optional prev_target_vel_; + bool use_velocity_limit_based_planner_{true}; - DebugValues debug_values_; + double time_to_evaluate_rss_; - std::shared_ptr lpf_cruise_ptr_; + std::function error_func_; }; #endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 7fe64181ff420..a01d3589245b5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -17,6 +17,7 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" +#include "obstacle_cruise_planner/stop_planning_debug_info.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -38,6 +39,7 @@ using autoware_adapi_v1_msgs::msg::VelocityFactorArray; using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; @@ -105,6 +107,16 @@ class PlannerInterface smoothed_trajectory_ptr_ = traj; } + Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const + { + return stop_planning_debug_info_.convertToMessage(current_time); + } + virtual Float32MultiArrayStamped getCruisePlanningDebugMessage( + [[maybe_unused]] const rclcpp::Time & current_time) const + { + return Float32MultiArrayStamped{}; + } + protected: // Parameters bool is_showing_debug_info_{false}; @@ -123,6 +135,9 @@ class PlannerInterface EgoNearestParam ego_nearest_param_; + // debug info + StopPlanningDebugInfo stop_planning_debug_info_; + // TODO(shimizu) remove these parameters Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..7c682a1f63c01 --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -0,0 +1,88 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#define OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 39a4dd5de0815..71ec3e5ef200c 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -134,13 +134,13 @@ double calcAlignedAdaptiveCruise( return object_vel * std::cos(object_yaw - traj_yaw); } -double calcObjectMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape) +double calcObjectMaxLength(const Shape & shape) { - if (shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) { + if (shape.type == Shape::BOUNDING_BOX) { return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) { + } else if (shape.type == Shape::CYLINDER) { return shape.dimensions.x / 2.0; - } else if (shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) { + } else if (shape.type == Shape::POLYGON) { double max_length_to_point = 0.0; for (const auto rel_point : shape.footprint.points) { const double length_to_point = std::hypot(rel_point.x, rel_point.y); @@ -192,6 +192,9 @@ Trajectory extendTrajectory( namespace motion_planning { +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), self_pose_listener_(this), @@ -223,11 +226,13 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); - debug_cruise_wall_marker_pub_ = - create_publisher("~/debug/cruise/virtual_wall", 1); - debug_stop_wall_marker_pub_ = - create_publisher("~/virtual_wall", 1); - debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); + debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); + debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_stop_planning_info_pub_ = + create_publisher("~/debug/stop_planning_info", 1); + debug_cruise_planning_info_pub_ = + create_publisher("~/debug/cruise_planning_info", 1); // longitudinal_info const auto longitudinal_info = [&]() { @@ -248,6 +253,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & const double terminal_safe_distance_margin = declare_parameter("common.terminal_safe_distance_margin"); + lpf_acc_ptr_ = std::make_shared(0.2); + return LongitudinalInfo{ max_accel, min_accel, @@ -273,6 +280,7 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & }(); is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); + disable_stop_planning_ = declare_parameter("common.disable_stop_planning"); { // Obstacle filtering parameters obstacle_filtering_param_.rough_detection_area_expand_width = @@ -424,7 +432,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); } } - // wait for first self pose self_pose_listener_.waitForFirstPose(); @@ -456,6 +463,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + tier4_autoware_utils::updateParam( + parameters, "common.disable_stop_planning", disable_stop_planning_); + // obstacle_filtering tier4_autoware_utils::updateParam( parameters, "obstacle_filtering.rough_detection_area_expand_width", @@ -572,7 +582,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms trajectory_pub_->publish(output_traj); // publish debug data - publishDebugData(debug_data); + publishDebugMarker(debug_data); + publishDebugInfo(); // publish and print calculation time const double calculation_time = stop_watch_.toc(__func__); @@ -610,9 +621,10 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( planner_data.current_vel = current_vel; planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; + for (const auto & obstacle : obstacles) { // consider all target obstacles when driving backward - if (!planner_data.is_driving_forward || obstacle.has_stopped) { + if (!disable_stop_planning_ && (!planner_data.is_driving_forward || obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -651,7 +663,7 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( planner_data.current_acc = current_accel; planner_data.is_driving_forward = is_driving_forward; for (const auto & obstacle : obstacles) { - if (planner_data.is_driving_forward && !obstacle.has_stopped) { + if (disable_stop_planning_ || (planner_data.is_driving_forward && !obstacle.has_stopped)) { planner_data.target_obstacles.push_back(obstacle); } } @@ -1024,11 +1036,11 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( } } -void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) const +void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const { stop_watch_.tic(__func__); - visualization_msgs::msg::MarkerArray debug_marker; + MarkerArray debug_marker; const auto current_time = now(); // obstacles to cruise @@ -1055,7 +1067,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // footprint polygons auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "detection_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); @@ -1077,7 +1089,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c { // collision points for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "collision_points", i, visualization_msgs::msg::Marker::SPHERE, + "map", current_time, "collision_points", i, Marker::SPHERE, tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = debug_data.collision_points.at(i); @@ -1087,7 +1099,7 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c debug_marker_pub_->publish(debug_marker); - // wall for cruise and stop + // virtual wall for cruise and stop debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker); debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker); @@ -1098,6 +1110,19 @@ void ObstacleCruisePlannerNode::publishDebugData(const DebugData & debug_data) c __func__, calculation_time); } +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + const auto current_time = now(); + + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); +} + void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const { Float32Stamped calculation_time_msg; @@ -1106,5 +1131,6 @@ void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_ debug_calculation_time_pub_->publish(calculation_time_msg); } } // namespace motion_planning + #include RCLCPP_COMPONENTS_REGISTER_NODE(motion_planning::ObstacleCruisePlannerNode) diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 267ff1f4502f2..cfd9dd24c543b 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -14,6 +14,7 @@ #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "interpolation/spline_interpolation.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -40,15 +41,15 @@ VelocityLimit createVelocityLimitMsg( return msg; } -Float32MultiArrayStamped convertDebugValuesToMsg( - const rclcpp::Time & current_time, const DebugValues & debug_values) +template +T getSign(const T val) { - Float32MultiArrayStamped debug_msg{}; - debug_msg.stamp = current_time; - for (const auto & v : debug_values.getValues()) { - debug_msg.data.push_back(v); + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); } - return debug_msg; + return static_cast(0); } } // namespace @@ -60,30 +61,86 @@ PIDBasedPlanner::PIDBasedPlanner( min_accel_during_cruise_ = node.declare_parameter("pid_based_planner.min_accel_during_cruise"); - // pid controller - const double kp = node.declare_parameter("pid_based_planner.kp"); - const double ki = node.declare_parameter("pid_based_planner.ki"); - const double kd = node.declare_parameter("pid_based_planner.kd"); - pid_controller_ = std::make_unique(kp, ki, kd); - output_ratio_during_accel_ = - node.declare_parameter("pid_based_planner.output_ratio_during_accel"); - - // some parameters - // use_predicted_obstacle_pose_ = - // node.declare_parameter("pid_based_planner.use_predicted_obstacle_pose"); + use_velocity_limit_based_planner_ = + node.declare_parameter("pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = + node.declare_parameter("pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + } - vel_to_acc_weight_ = node.declare_parameter("pid_based_planner.vel_to_acc_weight"); + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = + node.declare_parameter("pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc"); + } min_cruise_target_vel_ = node.declare_parameter("pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = node.declare_parameter("pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = + node.declare_parameter("pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); // low pass filter - const double lpf_cruise_gain = - node.declare_parameter("pid_based_planner.lpf_cruise_gain"); - lpf_cruise_ptr_ = std::make_shared(lpf_cruise_gain); - - // publisher - debug_values_pub_ = node.create_publisher("~/debug/values", 1); + const double lpf_normalized_error_cruise_dist_gain = + node.declare_parameter("pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); } Trajectory PIDBasedPlanner::generateCruiseTrajectory( @@ -91,36 +148,35 @@ Trajectory PIDBasedPlanner::generateCruiseTrajectory( DebugData & debug_data) { stop_watch_.tic(__func__); - debug_values_.resetValues(); + cruise_planning_debug_info_.reset(); // calc obstacles to cruise - boost::optional cruise_obstacle_info; - calcObstaclesToCruise(planner_data, cruise_obstacle_info); + const auto cruise_obstacle_info = calcObstacleToCruise(planner_data); // plan cruise - planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); - - // publish debug values - publishDebugValues(planner_data); + const auto cruise_traj = planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); - return planner_data.traj; + prev_traj_ = cruise_traj; + return cruise_traj; } -void PIDBasedPlanner::calcObstaclesToCruise( - const ObstacleCruisePlannerData & planner_data, - boost::optional & cruise_obstacle_info) +boost::optional PIDBasedPlanner::calcObstacleToCruise( + const ObstacleCruisePlannerData & planner_data) { - debug_values_.setValues(DebugValues::TYPE::CURRENT_VELOCITY, planner_data.current_vel); - debug_values_.setValues(DebugValues::TYPE::CURRENT_ACCELERATION, planner_data.current_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.current_acc); auto modified_target_obstacles = planner_data.target_obstacles; // search highest probability obstacle for cruise + boost::optional cruise_obstacle_info; for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { const auto & obstacle = planner_data.target_obstacles.at(o_idx); @@ -129,35 +185,102 @@ void PIDBasedPlanner::calcObstaclesToCruise( } // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle const double dist_to_obstacle = - calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point); - - if (!obstacle.has_stopped) { // cruise - // calculate distance between ego and obstacle based on RSS - const double rss_dist = calcRSSDistance( - planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); - - // calculate error distance and normalized one - const double error_dist = dist_to_obstacle - rss_dist; - if (cruise_obstacle_info) { - if (error_dist > cruise_obstacle_info->dist_to_cruise) { - continue; - } + calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data.current_vel) * time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calcRSSDistance( + planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; } - const double normalized_dist_to_cruise = error_dist / dist_to_obstacle; - cruise_obstacle_info = - CruiseObstacleInfo(obstacle, error_dist, normalized_dist_to_cruise, dist_to_obstacle); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_VELOCITY, obstacle.velocity); - debug_values_.setValues(DebugValues::TYPE::CRUISE_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_OBJECT_DISTANCE, rss_dist); - debug_values_.setValues(DebugValues::TYPE::CRUISE_ERROR_OBJECT_DISTANCE, error_dist); } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); + } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = + planner_data.current_vel - cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); } + + return cruise_obstacle_info; } -void PIDBasedPlanner::planCruise( +Trajectory PIDBasedPlanner::planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data) { @@ -167,48 +290,68 @@ void PIDBasedPlanner::planCruise( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "cruise planning"); - vel_limit = doCruise( - planner_data, cruise_obstacle_info.get(), debug_data.obstacles_to_cruise, - debug_data.cruise_wall_marker); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::CRUISE_TARGET_VELOCITY, vel_limit->max_velocity); - debug_values_.setValues( - DebugValues::TYPE::CRUISE_TARGET_ACCELERATION, vel_limit->constraints.min_acceleration); - } else { - // reset previous target velocity if adaptive cruise is not enabled - prev_target_vel_ = {}; - lpf_cruise_ptr_->reset(); - - // delete marker - const auto markers = - motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + + const auto markers = motion_utils::createSlowDownVirtualWallMarker( + planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, + dist_to_rss_wall); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + // cruise obstalce + debug_data.obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = doCruiseWithTrajectory(planner_data, cruise_obstacle_info.get()); + return cruise_traj; + } + + vel_limit = doCruiseWithVelocityLimit(planner_data, cruise_obstacle_info.get()); + return planner_data.traj; } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); + + return planner_data.traj; } -VelocityLimit PIDBasedPlanner::doCruise( - const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info, - std::vector & debug_obstacles_to_cruise, - visualization_msgs::msg::MarkerArray & debug_wall_marker) +VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) { - const double dist_to_cruise = cruise_obstacle_info.dist_to_cruise; - const double filtered_normalized_dist_to_cruise = [&]() { - const double normalized_dist_to_cruise = cruise_obstacle_info.normalized_dist_to_cruise; - return lpf_cruise_ptr_->filter(normalized_dist_to_cruise); + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); }(); - const double dist_to_obstacle = cruise_obstacle_info.dist_to_obstacle; - const size_t ego_idx = findEgoIndex(planner_data.traj, planner_data.current_pose); + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); // calculate target velocity with acceleration limit by PID controller - const double pid_output_vel = pid_controller_->calc(filtered_normalized_dist_to_cruise); - [[maybe_unused]] const double prev_vel = - prev_target_vel_ ? prev_target_vel_.get() : planner_data.current_vel; - + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); const double additional_vel = [&]() { - if (filtered_normalized_dist_to_cruise > 0) { - return pid_output_vel * output_ratio_during_accel_; + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; } return pid_output_vel; }(); @@ -217,42 +360,251 @@ VelocityLimit PIDBasedPlanner::doCruise( std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); // calculate target acceleration - const double target_acc = vel_to_acc_weight_ * additional_vel; - const double target_acc_with_acc_limit = - std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + const double target_acc = [&]() { + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, "target_velocity %f", positive_target_vel); - prev_target_vel_ = positive_target_vel; - // set target longitudinal motion const auto vel_limit = createVelocityLimitMsg( - planner_data.current_time, positive_target_vel, target_acc_with_acc_limit, - longitudinal_info_.max_jerk, longitudinal_info_.min_jerk); + planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk, + longitudinal_info_.min_jerk); - // virtual wall marker for cruise - const double abs_ego_offset = planner_data.is_driving_forward - ? std::abs(vehicle_info_.max_longitudinal_offset_m) - : std::abs(vehicle_info_.min_longitudinal_offset_m); - const double dist_to_rss_wall = - std::min(dist_to_cruise + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + return vel_limit; +} - const auto markers = motion_utils::createSlowDownVirtualWallMarker( - planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, - dist_to_rss_wall); - tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); +Trajectory PIDBasedPlanner::doCruiseWithTrajectory( + const ObstacleCruisePlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; - debug_obstacles_to_cruise.push_back(cruise_obstacle_info.obstacle); + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); - return vel_limit; + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - prev_target_acc_.get()) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = prev_target_acc_.get() + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + /* + const double target_acc = vel_to_acc_weight_ * additional_vel; + const double target_acc_with_acc_limit = + std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); + */ + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + if (smoothed_trajectory_ptr_) { + return motion_utils::calcInterpolatedPoint( + *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + } + return motion_utils::calcInterpolatedPoint( + prev_traj_, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj = getAccelerationLimitedTrajectory( + planner_data.traj, planner_data.current_pose, v0, a0, target_acc, target_jerk_ratio); + + const auto zero_vel_idx_opt = motion_utils::searchZeroVelocityIndex(cruise_traj.points); + if (!zero_vel_idx_opt) { + return cruise_traj; + } + + for (size_t i = zero_vel_idx_opt.get(); i < cruise_traj.points.size(); ++i) { + cruise_traj.points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj; } -void PIDBasedPlanner::publishDebugValues(const ObstacleCruisePlannerData & planner_data) const +Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory( + const Trajectory traj, const geometry_msgs::msg::Pose & start_pose, const double v0, + const double a0, const double target_acc, const double target_jerk_ratio) const { - const auto debug_values_msg = convertDebugValuesToMsg(planner_data.current_time, debug_values_); - debug_values_pub_->publish(debug_values_msg); + // calculate vt function + const auto & i = longitudinal_info_; + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = motion_utils::calcArcLength(traj.points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of obstacle_cruise_planner may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = + target_acc > a1 ? i.max_jerk * target_jerk_ratio : i.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj; + } + + auto acc_limited_traj = traj; + const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + acc_limited_traj.points, start_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += tier4_autoware_utils::calcDistance2d( + acc_limited_traj.points.at(i - 1), acc_limited_traj.points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj.points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj.points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj; } void PIDBasedPlanner::updateParam(const std::vector & parameters) @@ -260,24 +612,79 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); - // pid controller - double kp = pid_controller_->getKp(); - double ki = pid_controller_->getKi(); - double kd = pid_controller_->getKd(); - - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kp", kp); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.ki", ki); - tier4_autoware_utils::updateParam(parameters, "pid_based_planner.kd", kd); - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.output_ratio_during_accel", output_ratio_during_accel_); + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kp", kp); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.ki", ki); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", + p.output_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } - // vel_to_acc_weight - tier4_autoware_utils::updateParam( - parameters, "pid_based_planner.vel_to_acc_weight", vel_to_acc_weight_); + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + tier4_autoware_utils::updateParam( + parameters, + "pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } // min_cruise_target_vel tier4_autoware_utils::updateParam( parameters, "pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); - - pid_controller_->updateParam(kp, ki, kd); + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); } diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index 45ed1d3f92d0e..adee76f67c065 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -106,6 +106,12 @@ double calcMinimumDistanceToStop( Trajectory PlannerInterface::generateStopTrajectory( const ObstacleCruisePlannerData & planner_data, DebugData & debug_data) { + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_vel); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.current_acc); + const double abs_ego_offset = planner_data.is_driving_forward ? std::abs(vehicle_info_.max_longitudinal_offset_m) : std::abs(vehicle_info_.min_longitudinal_offset_m); @@ -236,6 +242,16 @@ Trajectory PlannerInterface::generateStopTrajectory( stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); } + // stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, // + // TODO(murooka) + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); + + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, feasible_margin_from_obstacle); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); + return output_traj; } From 5f49fd77ba9722046c39c229d914db8eed0888ed Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Sun, 18 Dec 2022 16:46:05 +0900 Subject: [PATCH 03/37] feat(behavior_path_planner): remove unnecessary parameters (#2516) * feat(behavior_path_planner): remove unnecessary parameters Signed-off-by: yutaka * remove from static_centerline_optimizer Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner/behavior_path_planner.param.yaml | 5 ----- .../config/behavior_path_planner.param.yaml | 6 ------ .../include/behavior_path_planner/parameters.hpp | 5 ----- .../src/behavior_path_planner_node.cpp | 5 ----- planning/static_centerline_optimizer/src/utils.cpp | 3 --- 5 files changed, 24 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml index 6ed118001fa6a..5edecdde26a11 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml @@ -7,11 +7,6 @@ backward_length_buffer_for_end_of_pull_out: 5.0 minimum_lane_change_length: 12.0 minimum_pull_over_length: 16.0 - drivable_area_resolution: 0.1 - drivable_lane_forward_length: 50.0 - drivable_lane_backward_length: 5.0 - drivable_lane_margin: 3.0 - drivable_area_margin: 6.0 refine_goal_search_radius_range: 7.5 turn_signal_intersection_search_distance: 30.0 turn_signal_intersection_angle_threshold_deg: 15.0 diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index 68cc14a8299a1..da2342f6782d4 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -9,12 +9,6 @@ minimum_lane_change_prepare_distance: 2.0 # [m] minimum_pull_over_length: 16.0 - drivable_area_resolution: 0.1 - drivable_lane_forward_length: 50.0 - drivable_lane_backward_length: 5.0 - drivable_lane_margin: 3.0 - drivable_area_margin: 6.0 - refine_goal_search_radius_range: 7.5 # parameters for turn signal decider diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 132e01f5d91d3..8103092e23cdc 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -31,11 +31,6 @@ struct BehaviorPathPlannerParameters double minimum_pull_out_length; double drivable_area_resolution; - double drivable_lane_forward_length; - double drivable_lane_backward_length; - double drivable_lane_margin; - double drivable_area_margin; - double refine_goal_search_radius_range; double turn_signal_intersection_search_distance; diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 4da5933afa7af..80416ea437991 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -211,11 +211,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("minimum_lane_change_prepare_distance", 2.0); p.minimum_pull_over_length = declare_parameter("minimum_pull_over_length", 15.0); - p.drivable_area_resolution = declare_parameter("drivable_area_resolution"); - p.drivable_lane_forward_length = declare_parameter("drivable_lane_forward_length"); - p.drivable_lane_backward_length = declare_parameter("drivable_lane_backward_length"); - p.drivable_lane_margin = declare_parameter("drivable_lane_margin"); - p.drivable_area_margin = declare_parameter("drivable_area_margin"); p.refine_goal_search_radius_range = declare_parameter("refine_goal_search_radius_range", 7.5); p.turn_signal_intersection_search_distance = declare_parameter("turn_signal_intersection_search_distance", 30.0); diff --git a/planning/static_centerline_optimizer/src/utils.cpp b/planning/static_centerline_optimizer/src/utils.cpp index f97d08b113e2a..3999c5cbce50a 100644 --- a/planning/static_centerline_optimizer/src/utils.cpp +++ b/planning/static_centerline_optimizer/src/utils.cpp @@ -85,9 +85,6 @@ PathWithLaneId get_path_with_lane_id( auto planner_data = std::make_shared(); planner_data->route_handler = std::make_shared(route_handler); planner_data->self_pose = convert_to_pose_stamped(start_pose); - planner_data->parameters.drivable_lane_forward_length = std::numeric_limits::max(); - planner_data->parameters.drivable_lane_backward_length = std::numeric_limits::min(); - planner_data->parameters.drivable_lane_margin = 5.0; planner_data->parameters.ego_nearest_dist_threshold = ego_nearest_dist_threshold; planner_data->parameters.ego_nearest_yaw_threshold = ego_nearest_yaw_threshold; From b58083832fbfb50915fa5ddf23a2b9490306bc38 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sun, 18 Dec 2022 17:22:36 +0900 Subject: [PATCH 04/37] feat(obstacle_cruise_planner): update plot config (#2518) * update cruise plot juggler Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * rename Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../plot_juggler_obstacle_cruise_planner.xml | 247 ++++++++++++++++++ ...plot_juggler_obstacle_velocity_planner.xml | 150 ----------- 2 files changed, 247 insertions(+), 150 deletions(-) create mode 100644 planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml delete mode 100644 planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml new file mode 100644 index 0000000000000..e90c4f2a33c78 --- /dev/null +++ b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_cruise_planner.xml @@ -0,0 +1,247 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.9 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.12 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.1 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.7 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.8 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.0 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.6 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.5 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.4 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.3 + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.4 + + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/cruise_planning_info/data.6 + + + + + return value + /planning/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/debug/stop_planning_info/data.3 + + + + + diff --git a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml b/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml deleted file mode 100644 index 2e7eeee211963..0000000000000 --- a/planning/obstacle_cruise_planner/config/plot_juggler_obstacle_velocity_planner.xml +++ /dev/null @@ -1,150 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - From 418d8e529406626d2b7a2dd979979611b8450659 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 10:14:44 +0900 Subject: [PATCH 05/37] chore(tier4_autoware_utils): add maintainers (#2520) owner(tier4_autoware_utils): add maintainers Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- common/tier4_autoware_utils/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 1e9acb6101935..50feab806ee4c 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -6,6 +6,8 @@ The tier4_autoware_utils package Takamasa Horibe Kenji Miyake + Takayuki Murooka + Yutaka Shimizu Apache License 2.0 ament_cmake_auto From 25c674e56342299410c5b3609a292846958f5642 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 19 Dec 2022 18:20:33 +0900 Subject: [PATCH 06/37] feat(behavior_path_planner): output multiple candidate paths (#2486) * feat(behavior_path_planner): output multiple candidate paths Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): get path candidate in behavior tree manager Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): add create publisher method Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): move publishers to node Signed-off-by: Fumiya Watanabe * feature(behavior_path_planner): remove unnecessary publisher Signed-off-by: Fumiya Watanabe * feat(behavior_path_planner): move reset path candidate function to behavior tree manager Signed-off-by: Fumiya Watanabe Signed-off-by: Fumiya Watanabe --- .../behavior_path_planner_node.hpp | 27 +++--- .../behavior_tree_manager.hpp | 5 ++ .../scene_module/scene_module_interface.hpp | 10 ++- .../src/behavior_path_planner_node.cpp | 86 +++++++++---------- .../src/behavior_tree_manager.cpp | 15 ++++ .../avoidance/avoidance_module.cpp | 5 +- .../lane_change/lane_change_module.cpp | 5 +- .../scene_module/pull_out/pull_out_module.cpp | 4 +- .../pull_over/pull_over_module.cpp | 7 +- .../scene_module_bt_node_interface.cpp | 2 +- .../side_shift/side_shift_module.cpp | 4 +- 11 files changed, 103 insertions(+), 67 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 48ac09b77e803..1a0800b268c15 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -45,6 +45,7 @@ #include #include +#include #include #include #include @@ -98,12 +99,13 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr perception_subscriber_; rclcpp::Subscription::SharedPtr occupancy_grid_subscriber_; rclcpp::Publisher::SharedPtr path_publisher_; - rclcpp::Publisher::SharedPtr path_candidate_publisher_; rclcpp::Publisher::SharedPtr turn_signal_publisher_; rclcpp::Publisher::SharedPtr hazard_signal_publisher_; rclcpp::Publisher::SharedPtr bound_publisher_; rclcpp::TimerBase::SharedPtr timer_; + std::map::SharedPtr> path_candidate_publishers_; + std::shared_ptr planner_data_; std::shared_ptr bt_manager_; std::unique_ptr steering_factor_interface_ptr_; @@ -160,12 +162,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node PathWithLaneId::SharedPtr getPath( const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); - /** - * @brief extract path candidate from behavior tree output - */ - PathWithLaneId::SharedPtr getPathCandidate( - const BehaviorModuleOutput & bt_out, const std::shared_ptr planner_data); - /** * @brief skip smooth goal connection */ @@ -184,11 +180,6 @@ class BehaviorPathPlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; - /** - * @brief check path if it is unsafe or forced - */ - bool isForcedCandidatePath() const; - /** * @brief publish steering factor from intersection */ @@ -204,6 +195,18 @@ class BehaviorPathPlannerNode : public rclcpp::Node */ void publishSceneModuleDebugMsg(); + /** + * @brief publish path candidate + */ + void publishPathCandidate( + const std::vector> & scene_modules); + + /** + * @brief convert path with lane id to path for publish path candidate + */ + Path convertToPath( + const std::shared_ptr & path_candidate_ptr, const bool is_ready); + template size_t findEgoIndex(const std::vector & points) const { diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp index 3d9e2e43ff423..75d80206c4dde 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_tree_manager.hpp @@ -51,6 +51,10 @@ class BehaviorTreeManager BehaviorModuleOutput run(const std::shared_ptr & data); std::vector> getModulesStatus(); std::shared_ptr getAllSceneModuleDebugMsgData(); + std::vector> getSceneModules() const + { + return scene_modules_; + } AvoidanceDebugMsgArray getAvoidanceDebugMsgArray(); @@ -68,6 +72,7 @@ class BehaviorTreeManager BT::Blackboard::Ptr blackboard_; BT::NodeStatus checkForceApproval(const std::string & name); + void resetNotRunningModulePathCandidate(); // For Groot monitoring std::unique_ptr groot_monitor_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp index d86461911a0c2..91b851ace15b4 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp @@ -62,9 +62,6 @@ struct BehaviorModuleOutput // path planed by module PlanResult path{}; - // path candidate planed by module - PlanResult path_candidate{}; - TurnSignalInfo turn_signal_info{}; }; @@ -130,7 +127,7 @@ class SceneModuleInterface BehaviorModuleOutput out; out.path = util::generateCenterLinePath(planner_data_); const auto candidate = planCandidate(); - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); return out; } @@ -228,6 +225,10 @@ class SceneModuleInterface bool isWaitingApproval() const { return is_waiting_approval_; } + PlanResult getPathCandidate() const { return path_candidate_; } + + void resetPathCandidate() { path_candidate_.reset(); } + virtual void lockRTCCommand() { if (!rtc_interface_ptr_) { @@ -258,6 +259,7 @@ class SceneModuleInterface std::unique_ptr steering_factor_interface_ptr_; UUID uuid_; bool is_waiting_approval_; + PlanResult path_candidate_; void updateRTCStatus(const double start_distance, const double finish_distance) { diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 80416ea437991..732464ec82106 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -61,7 +61,6 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod // publisher path_publisher_ = create_publisher("~/output/path", 1); - path_candidate_publisher_ = create_publisher("~/output/path_candidate", 1); turn_signal_publisher_ = create_publisher("~/output/turn_indicators_cmd", 1); hazard_signal_publisher_ = create_publisher("~/output/hazard_lights_cmd", 1); @@ -115,6 +114,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod std::bind(&BehaviorPathPlannerNode::onSetParam, this, std::placeholders::_1)); // behavior tree manager { + const std::string path_candidate_name_space = "/planning/path_candidate/"; mutex_bt_.lock(); bt_manager_ = std::make_shared(*this, getBehaviorTreeManagerParam()); @@ -125,6 +125,8 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto avoidance_module = std::make_shared("Avoidance", *this, avoidance_param_ptr); + path_candidate_publishers_.emplace( + "Avoidance", create_publisher(path_candidate_name_space + "avoidance", 1)); bt_manager_->registerSceneModule(avoidance_module); auto lane_following_module = @@ -133,12 +135,18 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod auto lane_change_module = std::make_shared("LaneChange", *this, lane_change_param_ptr); + path_candidate_publishers_.emplace( + "LaneChange", create_publisher(path_candidate_name_space + "lane_change", 1)); bt_manager_->registerSceneModule(lane_change_module); auto pull_over_module = std::make_shared("PullOver", *this, getPullOverParam()); + path_candidate_publishers_.emplace( + "PullOver", create_publisher(path_candidate_name_space + "pull_over", 1)); bt_manager_->registerSceneModule(pull_over_module); auto pull_out_module = std::make_shared("PullOut", *this, getPullOutParam()); + path_candidate_publishers_.emplace( + "PullOut", create_publisher(path_candidate_name_space + "pull_out", 1)); bt_manager_->registerSceneModule(pull_out_module); bt_manager_->createBehaviorTree(); @@ -643,8 +651,7 @@ void BehaviorPathPlannerNode::run() get_logger(), *get_clock(), 5000, "behavior path output is empty! Stop publish."); } - const auto path_candidate = getPathCandidate(output, planner_data); - path_candidate_publisher_->publish(util::toPath(*path_candidate)); + publishPathCandidate(bt_manager_->getSceneModules()); publishSceneModuleDebugMsg(); @@ -758,6 +765,39 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg() } } +void BehaviorPathPlannerNode::publishPathCandidate( + const std::vector> & scene_modules) +{ + for (auto & module : scene_modules) { + if (path_candidate_publishers_.count(module->name()) != 0) { + path_candidate_publishers_.at(module->name()) + ->publish(convertToPath(module->getPathCandidate(), module->isExecutionReady())); + } + } +} + +Path BehaviorPathPlannerNode::convertToPath( + const std::shared_ptr & path_candidate_ptr, const bool is_ready) +{ + Path output; + output.header = planner_data_->route_handler->getRouteHeader(); + output.header.stamp = this->now(); + + if (!path_candidate_ptr) { + return output; + } + + output = util::toPath(*path_candidate_ptr); + + if (!is_ready) { + for (auto & point : output.points) { + point.longitudinal_velocity_mps = 0.0; + } + } + + return output; +} + PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) { @@ -782,46 +822,6 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( return std::make_shared(resampled_path); } -PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPathCandidate( - const BehaviorModuleOutput & bt_output, const std::shared_ptr planner_data) -{ - auto path_candidate = - bt_output.path_candidate ? bt_output.path_candidate : std::make_shared(); - - if (isForcedCandidatePath()) { - for (auto & path_point : path_candidate->points) { - path_point.point.longitudinal_velocity_mps = 0.0; - } - } - - path_candidate->header = planner_data->route_handler->getRouteHeader(); - path_candidate->header.stamp = this->now(); - RCLCPP_DEBUG( - get_logger(), "BehaviorTreeManager: path candidate is %s.", - bt_output.path_candidate ? "FOUND" : "NOT FOUND"); - return path_candidate; -} - -bool BehaviorPathPlannerNode::isForcedCandidatePath() const -{ - const auto & module_status_ptr_vec = bt_manager_->getModulesStatus(); - for (const auto & module_status_ptr : module_status_ptr_vec) { - if (!module_status_ptr) { - continue; - } - if (module_status_ptr->module_name != "LaneChange") { - continue; - } - const auto & is_waiting_approval = module_status_ptr->is_waiting_approval; - const auto & is_execution_ready = module_status_ptr->is_execution_ready; - if (is_waiting_approval && !is_execution_ready) { - return true; - } - break; - } - return false; -} - bool BehaviorPathPlannerNode::skipSmoothGoalConnection( const std::vector> & statuses) const { diff --git a/planning/behavior_path_planner/src/behavior_tree_manager.cpp b/planning/behavior_path_planner/src/behavior_tree_manager.cpp index 662a7613669cd..54b9afe354591 100644 --- a/planning/behavior_path_planner/src/behavior_tree_manager.cpp +++ b/planning/behavior_path_planner/src/behavior_tree_manager.cpp @@ -98,6 +98,8 @@ BehaviorModuleOutput BehaviorTreeManager::run(const std::shared_ptr RCLCPP_DEBUG(logger_, "BehaviorPathPlanner::run end status = %s", BT::toStr(res).c_str()); + resetNotRunningModulePathCandidate(); + std::for_each(scene_modules_.begin(), scene_modules_.end(), [](const auto & m) { m->publishDebugMarker(); if (!m->isExecutionRequested()) { @@ -127,6 +129,19 @@ BT::NodeStatus BehaviorTreeManager::checkForceApproval(const std::string & name) return approval.module_name == name ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE; } +void BehaviorTreeManager::resetNotRunningModulePathCandidate() +{ + const bool is_any_module_running = std::any_of( + scene_modules_.begin(), scene_modules_.end(), + [](const auto & module) { return module->current_state_ == BT::NodeStatus::RUNNING; }); + + for (auto & module : scene_modules_) { + if (is_any_module_running && (module->current_state_ != BT::NodeStatus::RUNNING)) { + module->resetPathCandidate(); + } + } +} + void BehaviorTreeManager::resetBehaviorTree() { bt_tree_.haltTree(); } void BehaviorTreeManager::addGrootMonitoring( diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 77e6180f3eb82..1f7979c9b726c 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -2219,6 +2219,8 @@ BehaviorModuleOutput AvoidanceModule::plan() { DEBUG_PRINT("AVOIDANCE plan"); + resetPathCandidate(); + const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_); const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_); @@ -2366,7 +2368,7 @@ BehaviorModuleOutput AvoidanceModule::planWaitingApproval() clearWaitingApproval(); removeCandidateRTCStatus(); } - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); return out; } @@ -2773,6 +2775,7 @@ void AvoidanceModule::initVariables() debug_data_ = DebugData(); debug_marker_.markers.clear(); + resetPathCandidate(); registered_raw_shift_lines_ = {}; current_raw_shift_lines_ = {}; original_unique_id = 0; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index dc9a19e4da7d4..135c26d3dfe3a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -143,6 +143,8 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { + resetPathCandidate(); + auto path = status_.lane_change_path.path; if (!isValidPath(path)) { @@ -201,7 +203,7 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() BehaviorModuleOutput out; out.path = std::make_shared(getReferencePath()); const auto candidate = planCandidate(); - out.path_candidate = std::make_shared(candidate.path_candidate); + path_candidate_ = std::make_shared(candidate.path_candidate); updateRTCStatus(candidate); waitApproval(); return out; @@ -640,6 +642,7 @@ void LaneChangeModule::resetParameters() steering_factor_interface_ptr_->clearSteeringFactors(); object_debug_.clear(); debug_marker_.markers.clear(); + resetPathCandidate(); } void LaneChangeModule::acceptVisitor(const std::shared_ptr & visitor) const diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index 5ebbf2e415470..e89ad45cdadd9 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -100,6 +100,7 @@ void PullOutModule::onExit() clearWaitingApproval(); removeRTCStatus(); steering_factor_interface_ptr_->clearSteeringFactors(); + resetPathCandidate(); current_state_ = BT::NodeStatus::SUCCESS; RCLCPP_DEBUG(getLogger(), "PULL_OUT onExit"); } @@ -165,6 +166,7 @@ BehaviorModuleOutput PullOutModule::plan() { if (isWaitingApproval()) { clearWaitingApproval(); + resetPathCandidate(); // save current_pose when approved for start_point of turn_signal for backward driving last_approved_pose_ = std::make_unique(planner_data_->self_pose->pose); } @@ -295,7 +297,7 @@ BehaviorModuleOutput PullOutModule::planWaitingApproval() output.path = std::make_shared(stop_path); output.turn_signal_info = calcTurnSignalInfo(); - output.path_candidate = std::make_shared(candidate_path); + path_candidate_ = std::make_shared(candidate_path); const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 29fcf8e12233c..54b78686b1e2a 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -181,6 +181,7 @@ void PullOverModule::onExit() RCLCPP_DEBUG(getLogger(), "PULL_OVER onExit"); clearWaitingApproval(); removeRTCStatus(); + resetPathCandidate(); steering_factor_interface_ptr_->clearSteeringFactors(); // A child node must never return IDLE @@ -346,6 +347,8 @@ BehaviorModuleOutput PullOverModule::plan() { const auto & current_pose = planner_data_->self_pose->pose; + resetPathCandidate(); + status_.current_lanes = util::getExtendedCurrentLanes(planner_data_); status_.pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); status_.lanes = @@ -449,7 +452,7 @@ BehaviorModuleOutput PullOverModule::plan() // safe: use pull over path if (status_.is_safe) { output.path = std::make_shared(getCurrentPath()); - output.path_candidate = std::make_shared(getFullPath()); + path_candidate_ = std::make_shared(getFullPath()); } else { RCLCPP_WARN_THROTTLE( getLogger(), *clock_, 5000, "Not found safe pull_over path. Stop in road lane."); @@ -521,7 +524,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() BehaviorModuleOutput out; plan(); // update status_ out.path = std::make_shared(getReferencePath()); - out.path_candidate = status_.is_safe ? std::make_shared(getFullPath()) : out.path; + path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); updateRTCStatus(distance_to_path_change.first, distance_to_path_change.second); diff --git a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp index e4da7f72dea18..116663ad3d797 100644 --- a/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/scene_module_bt_node_interface.cpp @@ -68,7 +68,7 @@ BT::NodeStatus SceneModuleBTNodeInterface::tick() // std::exit(EXIT_FAILURE); // TODO(Horibe) do appropriate handing } scene_module_->unlockRTCCommand(); - return BT::NodeStatus::SUCCESS; + return BT::NodeStatus::FAILURE; } while (rclcpp::ok()) { diff --git a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp index f6c980871f866..c68934d06be7e 100644 --- a/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/side_shift/side_shift_module.cpp @@ -53,6 +53,7 @@ void SideShiftModule::initVariables() prev_output_ = ShiftedPath{}; prev_shift_line_ = ShiftLine{}; path_shifter_ = PathShifter{}; + resetPathCandidate(); } void SideShiftModule::onEntry() @@ -65,7 +66,6 @@ void SideShiftModule::onExit() { // write me... initVariables(); - current_state_ = BT::NodeStatus::SUCCESS; } @@ -284,7 +284,7 @@ BehaviorModuleOutput SideShiftModule::planWaitingApproval() BehaviorModuleOutput output; output.path = std::make_shared(shifted_path.path); - output.path_candidate = std::make_shared(planCandidate().path_candidate); + path_candidate_ = std::make_shared(planCandidate().path_candidate); prev_output_ = shifted_path; From c6d63b75cb70bcffa9f27a39b77c8f54c6398c94 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Mon, 19 Dec 2022 18:44:47 +0900 Subject: [PATCH 07/37] feat(tier4_autoware_utils): add toHexString (#2519) * feat(tier4_autoware_utils): add toHexString Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../tier4_autoware_utils/ros/uuid_helper.hpp | 34 +++++++++++++++++++ .../tier4_autoware_utils.hpp | 1 + common/tier4_autoware_utils/package.xml | 1 + .../src/map_based_prediction_node.cpp | 22 +++--------- .../crosswalk/scene_crosswalk.cpp | 9 +---- 5 files changed, 42 insertions(+), 25 deletions(-) create mode 100644 common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp new file mode 100644 index 0000000000000..5b2f197c18827 --- /dev/null +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/ros/uuid_helper.hpp @@ -0,0 +1,34 @@ +// 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 TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ +#define TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ + +#include + +#include + +namespace tier4_autoware_utils +{ +inline 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 tier4_autoware_utils + +#endif // TIER4_AUTOWARE_UTILS__ROS__UUID_HELPER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp index 35b4c7b836b46..b1cb540ee7a77 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/tier4_autoware_utils.hpp @@ -33,6 +33,7 @@ #include "tier4_autoware_utils/ros/self_pose_listener.hpp" #include "tier4_autoware_utils/ros/transform_listener.hpp" #include "tier4_autoware_utils/ros/update_param.hpp" +#include "tier4_autoware_utils/ros/uuid_helper.hpp" #include "tier4_autoware_utils/ros/wait_for_param.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" diff --git a/common/tier4_autoware_utils/package.xml b/common/tier4_autoware_utils/package.xml index 50feab806ee4c..9e648c49e067b 100644 --- a/common/tier4_autoware_utils/package.xml +++ b/common/tier4_autoware_utils/package.xml @@ -25,6 +25,7 @@ tf2 tf2_geometry_msgs tier4_debug_msgs + unique_identifier_msgs visualization_msgs ament_cmake_ros diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index c5159387787ed..fb553dad8b409 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -37,17 +37,6 @@ namespace map_based_prediction { -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(); -} - lanelet::ConstLanelets getLanelets(const map_based_prediction::LaneletsData & data) { lanelet::ConstLanelets lanelets; @@ -246,7 +235,6 @@ bool hasPotentialToReach( return false; } -} // namespace MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_options) : Node("map_based_prediction", node_options), debug_accumulated_time_(0.0) @@ -364,7 +352,7 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt visualization_msgs::msg::MarkerArray debug_markers; for (const auto & object : in_objects->objects) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); TrackedObject transformed_object = object; // transform object frame if it's based on map frame @@ -744,7 +732,7 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition( // If the object is in the objects history, we check if the target lanelet is // inside the current lanelets id or following lanelets - const std::string object_id = toHexString(object.object_id); + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) != 0) { const std::vector & possible_lanelet = objects_history_.at(object_id).back().future_possible_lanelets; @@ -817,7 +805,7 @@ void MapBasedPredictionNode::updateObjectsHistory( const std_msgs::msg::Header & header, const TrackedObject & object, const LaneletsData & current_lanelets_data) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); const auto current_lanelets = getLanelets(current_lanelets_data); ObjectData single_object_data; @@ -904,7 +892,7 @@ Maneuver MapBasedPredictionNode::predictObjectManeuver( const double object_detected_time) { // Step1. Check if we have the object in the buffer - const std::string object_id = toHexString(object.object_id); + const std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) == 0) { return Maneuver::LANE_FOLLOW; } @@ -1063,7 +1051,7 @@ double MapBasedPredictionNode::calcLeftLateralOffset( void MapBasedPredictionNode::updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths) { - std::string object_id = toHexString(object.object_id); + std::string object_id = tier4_autoware_utils::toHexString(object.object_id); if (objects_history_.count(object_id) == 0) { return; } diff --git a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp index 9b40751449dd6..2dabbdc537b99 100644 --- a/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_crosswalk.cpp @@ -39,17 +39,10 @@ using tier4_autoware_utils::createPoint; using tier4_autoware_utils::getPoint; using tier4_autoware_utils::getPose; using tier4_autoware_utils::pose2transform; +using tier4_autoware_utils::toHexString; 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(); -} geometry_msgs::msg::Point32 createPoint32(const double x, const double y, const double z) { geometry_msgs::msg::Point32 p; From e7e461eed2bfe759ae24e0af14f1cb9ea2053e1b Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 19 Dec 2022 19:46:20 +0900 Subject: [PATCH 08/37] fix(map_based_prediction): return non-empty predicted path (#2525) Signed-off-by: yutaka Signed-off-by: yutaka --- .../map_based_prediction_node.hpp | 1 - .../src/map_based_prediction_node.cpp | 70 +++---------------- .../src/path_generator.cpp | 6 +- 3 files changed, 10 insertions(+), 67 deletions(-) diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 9e66b40367518..81ed9af3e2aa2 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -190,7 +190,6 @@ class MapBasedPredictionNode : public rclcpp::Node const float path_probability, const ManeuverProbability & maneuver_probability, const Maneuver & maneuver, std::vector & reference_paths); std::vector convertPathType(const lanelet::routing::LaneletPaths & paths); - PosePath resamplePath(const PosePath & base_path) const; void updateFuturePossibleLanelets( const TrackedObject & object, const lanelet::routing::LaneletPaths & paths); diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index fb553dad8b409..82fe883f379ab 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -15,6 +15,7 @@ #include "map_based_prediction/map_based_prediction_node.hpp" #include +#include #include #include @@ -451,6 +452,9 @@ void MapBasedPredictionNode::objectsCallback(const TrackedObjects::ConstSharedPt for (const auto & ref_path : ref_paths) { PredictedPath predicted_path = path_generator_->generatePathForOnLaneVehicle(transformed_object, ref_path.path); + if (predicted_path.path.empty()) { + continue; + } predicted_path.confidence = ref_path.probability; predicted_paths.push_back(predicted_path); @@ -1171,8 +1175,7 @@ std::vector MapBasedPredictionNode::convertPathType( lanelet::ConstLanelet prev_lanelet = prev_lanelets.front(); for (const auto & lanelet_p : prev_lanelet.centerline()) { geometry_msgs::msg::Pose current_p; - current_p.position = - tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p); const double lane_yaw = lanelet::utils::getLaneletAngle(prev_lanelet, current_p.position); current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); converted_path.push_back(current_p); @@ -1183,8 +1186,7 @@ std::vector MapBasedPredictionNode::convertPathType( for (const auto & lanelet : path) { for (const auto & lanelet_p : lanelet.centerline()) { geometry_msgs::msg::Pose current_p; - current_p.position = - tier4_autoware_utils::createPoint(lanelet_p.x(), lanelet_p.y(), lanelet_p.z()); + current_p.position = lanelet::utils::conversion::toGeomMsgPt(lanelet_p); const double lane_yaw = lanelet::utils::getLaneletAngle(lanelet, current_p.position); current_p.orientation = tier4_autoware_utils::createQuaternionFromYaw(lane_yaw); @@ -1202,70 +1204,14 @@ std::vector MapBasedPredictionNode::convertPathType( } // Resample Path - const auto resampled_converted_path = resamplePath(converted_path); + const auto resampled_converted_path = + motion_utils::resamplePoseVector(converted_path, reference_path_resolution_); converted_paths.push_back(resampled_converted_path); } return converted_paths; } -PosePath MapBasedPredictionNode::resamplePath(const PosePath & base_path) const -{ - std::vector base_s(base_path.size()); - std::vector base_x(base_path.size()); - std::vector base_y(base_path.size()); - std::vector base_z(base_path.size()); - for (size_t i = 0; i < base_path.size(); ++i) { - base_x.at(i) = base_path.at(i).position.x; - base_y.at(i) = base_path.at(i).position.y; - base_z.at(i) = base_path.at(i).position.z; - base_s.at(i) = motion_utils::calcSignedArcLength(base_path, 0, i); - } - const double base_path_len = base_s.back(); - - std::vector resampled_s; - for (double s = 0.0; s <= base_path_len; s += reference_path_resolution_) { - resampled_s.push_back(s); - } - - if (resampled_s.empty()) { - return base_path; - } - - // Insert End Point - const double epsilon = 0.01; - if (std::fabs(resampled_s.back() - base_path_len) < epsilon) { - resampled_s.back() = base_path_len; - } else { - resampled_s.push_back(base_path_len); - } - - if (resampled_s.size() < 2) { - return base_path; - } - - const auto resampled_x = interpolation::lerp(base_s, base_x, resampled_s); - const auto resampled_y = interpolation::lerp(base_s, base_y, resampled_s); - const auto resampled_z = interpolation::lerp(base_s, base_z, resampled_s); - - PosePath resampled_path(resampled_x.size()); - // Position - for (size_t i = 0; i < resampled_path.size(); ++i) { - resampled_path.at(i).position = - tier4_autoware_utils::createPoint(resampled_x.at(i), resampled_y.at(i), resampled_z.at(i)); - } - // Orientation - for (size_t i = 0; i < resampled_path.size() - 1; ++i) { - const auto curr_p = resampled_path.at(i).position; - const auto next_p = resampled_path.at(i + 1).position; - const double yaw = std::atan2(next_p.y - curr_p.y, next_p.x - curr_p.x); - resampled_path.at(i).orientation = tier4_autoware_utils::createQuaternionFromYaw(yaw); - } - resampled_path.back().orientation = resampled_path.at(resampled_path.size() - 2).orientation; - - return resampled_path; -} - bool MapBasedPredictionNode::isDuplicated( const std::pair & target_lanelet, const LaneletsData & lanelets_data) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index d4306eee4431d..7b2e3fa0e3d04 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -139,8 +139,7 @@ PredictedPath PathGenerator::generatePathForOnLaneVehicle( const TrackedObject & object, const PosePath & ref_paths) { if (ref_paths.size() < 2) { - const PredictedPath empty_path; - return empty_path; + return generateStraightPath(object); } return generatePolynomialPath(object, ref_paths); @@ -190,8 +189,7 @@ PredictedPath PathGenerator::generatePolynomialPath( const auto interpolated_ref_path = interpolateReferencePath(ref_path, frenet_predicted_path); if (frenet_predicted_path.size() < 2 || interpolated_ref_path.size() < 2) { - const PredictedPath empty_path; - return empty_path; + return generateStraightPath(object); } // Step4. Convert predicted trajectory from Frenet to Cartesian coordinate From 2125a11d2b7fb3b8b9bea0f90c591e7b9a581a6f Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Mon, 19 Dec 2022 19:55:51 +0900 Subject: [PATCH 09/37] feat(map_based_prediction): add maintainers (#2527) Signed-off-by: yutaka Signed-off-by: yutaka --- perception/map_based_prediction/package.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/perception/map_based_prediction/package.xml b/perception/map_based_prediction/package.xml index 98e7e125f78af..74742179a1577 100644 --- a/perception/map_based_prediction/package.xml +++ b/perception/map_based_prediction/package.xml @@ -5,6 +5,8 @@ 0.1.0 This package implements a map_based_prediction Yutaka Shimizu + Tomoya Kimura + Shunsuke Miura Apache License 2.0 ament_cmake From f8af70c4af2ff7d5b90144d51accf83d9a37305b Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Mon, 19 Dec 2022 20:45:09 +0900 Subject: [PATCH 10/37] feat(operation_mode_transition_manager): add parameter about nearest search (#2523) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- ...eration_mode_transition_manager.param.yaml | 2 ++ .../src/state.cpp | 20 +++++++++---------- .../src/state.hpp | 4 +++- 3 files changed, 15 insertions(+), 11 deletions(-) diff --git a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml index a0ddef6e363cc..a86443f5cabdb 100644 --- a/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml +++ b/control/operation_mode_transition_manager/config/operation_mode_transition_manager.param.yaml @@ -3,6 +3,8 @@ transition_timeout: 10.0 frequency_hz: 10.0 check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 diff --git a/control/operation_mode_transition_manager/src/state.cpp b/control/operation_mode_transition_manager/src/state.cpp index 775083908cdfd..8980f665d916d 100644 --- a/control/operation_mode_transition_manager/src/state.cpp +++ b/control/operation_mode_transition_manager/src/state.cpp @@ -43,6 +43,10 @@ AutonomousMode::AutonomousMode(rclcpp::Node * node) "trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = *msg; }); check_engage_condition_ = node->declare_parameter("check_engage_condition"); + nearest_dist_deviation_threshold_ = + node->declare_parameter("nearest_dist_deviation_threshold"); + nearest_yaw_deviation_threshold_ = + node->declare_parameter("nearest_yaw_deviation_threshold"); // params for mode change available { @@ -86,9 +90,6 @@ bool AutonomousMode::isModeChangeCompleted() return true; } - constexpr auto dist_max = 5.0; - constexpr auto yaw_max = M_PI_4; - const auto current_speed = kinematics_.twist.twist.linear.x; const auto & param = engage_acceptable_param_; @@ -107,8 +108,9 @@ bool AutonomousMode::isModeChangeCompleted() return unstable(); } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Not stable yet: closest point not found"); return unstable(); @@ -199,9 +201,6 @@ bool AutonomousMode::isModeChangeAvailable() return true; } - constexpr auto dist_max = 100.0; - constexpr auto yaw_max = M_PI_4; - const auto current_speed = kinematics_.twist.twist.linear.x; const auto target_control_speed = control_cmd_.longitudinal.speed; const auto & param = engage_acceptable_param_; @@ -213,8 +212,9 @@ bool AutonomousMode::isModeChangeAvailable() return false; } - const auto closest_idx = - findNearestIndex(trajectory_.points, kinematics_.pose.pose, dist_max, yaw_max); + const auto closest_idx = findNearestIndex( + trajectory_.points, kinematics_.pose.pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); if (!closest_idx) { RCLCPP_INFO(logger_, "Engage unavailable: closest point not found"); debug_info_ = DebugInfo{}; // all false diff --git a/control/operation_mode_transition_manager/src/state.hpp b/control/operation_mode_transition_manager/src/state.hpp index dc72d57aed231..9d384857bbe3d 100644 --- a/control/operation_mode_transition_manager/src/state.hpp +++ b/control/operation_mode_transition_manager/src/state.hpp @@ -72,7 +72,9 @@ class AutonomousMode : public ModeChangeBase rclcpp::Logger logger_; rclcpp::Clock::SharedPtr clock_; - bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + bool check_engage_condition_ = true; // if false, the vehicle is engaged without any checks. + double nearest_dist_deviation_threshold_; // [m] for finding nearest index + double nearest_yaw_deviation_threshold_; // [rad] for finding nearest index EngageAcceptableParam engage_acceptable_param_; StableCheckParam stable_check_param_; AckermannControlCommand control_cmd_; From ee9a38959181ff4850d3d300e2816ac75a5b4fa1 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Dec 2022 00:39:25 +0900 Subject: [PATCH 11/37] feat(behavior_path_planner): keep original points in resampling of pull over (#2478) * feat(behavior_path_planner): keep original points in resampling Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/path_utilities.cpp Signed-off-by: kosuke55 * change motion_utils::overlap_threshold to 0.2 and use it Signed-off-by: kosuke55 * Revert "change motion_utils::overlap_threshold to 0.2 and use it" This reverts commit edfb2d59df8446e50bb582b83ccc8f2522e298fe. Signed-off-by: kosuke55 * keep input points only in pull over Signed-off-by: kosuke55 * Add comment of temporary processe Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner_node.hpp | 2 ++ .../behavior_path_planner/path_utilities.hpp | 3 ++- .../src/behavior_path_planner_node.cpp | 21 +++++++++++++++++-- .../src/path_utilities.cpp | 8 +++++-- 4 files changed, 29 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 1a0800b268c15..75d1f798d8b65 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -168,6 +168,8 @@ class BehaviorPathPlannerNode : public rclcpp::Node bool skipSmoothGoalConnection( const std::vector> & statuses) const; + bool keepInputPoints(const std::vector> & statuses) const; + /** * @brief skip smooth goal connection */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp index 5ab15f17306aa..2d6f44633cc5d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/path_utilities.hpp @@ -41,7 +41,8 @@ std::vector calcPathArcLengthArray( const PathWithLaneId & path, const size_t start = 0, const size_t end = std::numeric_limits::max(), const double offset = 0.0); -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval); +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points = false); Path toPath(const PathWithLaneId & input); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 732464ec82106..58afd67a77f14 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -817,8 +817,9 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( connected_path = modifyPathForSmoothGoalConnection(*path); } - const auto resampled_path = - util::resamplePathWithSpline(connected_path, planner_data_->parameters.path_interval); + const auto resampled_path = util::resamplePathWithSpline( + connected_path, planner_data_->parameters.path_interval, + keepInputPoints(module_status_ptr_vec)); return std::make_shared(resampled_path); } @@ -837,6 +838,22 @@ bool BehaviorPathPlannerNode::skipSmoothGoalConnection( return false; } +// This is a temporary process until motion planning can take the terminal pose into account +bool BehaviorPathPlannerNode::keepInputPoints( + const std::vector> & statuses) const +{ + const auto target_module = "PullOver"; + + for (auto & status : statuses) { + if (status->is_waiting_approval || status->status == BT::NodeStatus::RUNNING) { + if (target_module == status->module_name) { + return true; + } + } + } + return false; +} + void BehaviorPathPlannerNode::onVelocity(const Odometry::ConstSharedPtr msg) { std::lock_guard lock(mutex_pd_); diff --git a/planning/behavior_path_planner/src/path_utilities.cpp b/planning/behavior_path_planner/src/path_utilities.cpp index 20d9cfc0fc3b8..457687a6667d3 100644 --- a/planning/behavior_path_planner/src/path_utilities.cpp +++ b/planning/behavior_path_planner/src/path_utilities.cpp @@ -55,7 +55,8 @@ std::vector calcPathArcLengthArray( /** * @brief resamplePathWithSpline */ -PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interval) +PathWithLaneId resamplePathWithSpline( + const PathWithLaneId & path, const double interval, const bool keep_input_points) { if (path.points.size() < 2) { return path; @@ -66,7 +67,7 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv transformed_path.at(i) = path.points.at(i).point; } - constexpr double epsilon = 0.01; + constexpr double epsilon = 0.2; const auto has_almost_same_value = [&](const auto & vec, const auto x) { if (vec.empty()) return false; const auto has_close = [&](const auto v) { return std::abs(v - x) < epsilon; }; @@ -79,6 +80,9 @@ PathWithLaneId resamplePathWithSpline(const PathWithLaneId & path, double interv for (size_t i = 0; i < path.points.size(); ++i) { const double s = motion_utils::calcSignedArcLength(transformed_path, 0, i); for (const auto & lane_id : path.points.at(i).lane_ids) { + if (keep_input_points && !has_almost_same_value(s_in, s)) { + s_in.push_back(s); + } if ( std::find(unique_lane_ids.begin(), unique_lane_ids.end(), lane_id) != unique_lane_ids.end()) { From f67d7337d4975a10925d53e7967a042587b84315 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 11:25:04 +0900 Subject: [PATCH 12/37] feat(behavior_path_planner): enable generating backward drivable area (#2512) Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner/utilities.hpp | 2 +- .../behavior_path_planner/src/utilities.cpp | 142 ++++++++---------- 2 files changed, 65 insertions(+), 79 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 3697ad5576b1c..6b684998141c5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -294,7 +294,7 @@ std::vector cutOverlappedLanes( void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data); + const std::shared_ptr planner_data, const bool is_driving_forward = true); lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( const std::shared_ptr & planner_data); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 500eb5f90c774..d335c6c13b582 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -76,15 +76,15 @@ size_t findNearestSegmentIndex( template size_t findNearestSegmentIndexFromLateralDistance( - const std::vector & points, const geometry_msgs::msg::Pose & pose) + const std::vector & points, const geometry_msgs::msg::Point & target_point) { - size_t closest_idx = motion_utils::findNearestSegmentIndex(points, pose.position); + size_t closest_idx = motion_utils::findNearestSegmentIndex(points, target_point); double min_lateral_dist = - std::fabs(motion_utils::calcLateralOffset(points, pose.position, closest_idx)); + std::fabs(motion_utils::calcLateralOffset(points, target_point, closest_idx)); for (size_t seg_idx = 0; seg_idx < points.size() - 1; ++seg_idx) { const double lon_dist = - motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, pose.position); + motion_utils::calcLongitudinalOffsetToSegment(points, seg_idx, target_point); const double segment_length = tier4_autoware_utils::calcDistance2d(points.at(seg_idx), points.at(seg_idx + 1)); if (lon_dist < 0.0 || segment_length < lon_dist) { @@ -92,7 +92,7 @@ size_t findNearestSegmentIndexFromLateralDistance( } const double lat_dist = - std::fabs(motion_utils::calcLateralOffset(points, pose.position, seg_idx)); + std::fabs(motion_utils::calcLateralOffset(points, target_point, seg_idx)); if (lat_dist < min_lateral_dist) { closest_idx = seg_idx; min_lateral_dist = lat_dist; @@ -1103,36 +1103,36 @@ std::vector cutOverlappedLanes( return shorten_lanes; } -geometry_msgs::msg::Pose calcLongitudinalOffsetStartPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetStartPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx); } -geometry_msgs::msg::Pose calcLongitudinalOffsetGoalPose( - const std::vector & points, const geometry_msgs::msg::Pose & pose, +geometry_msgs::msg::Point calcLongitudinalOffsetGoalPoint( + const std::vector & points, const geometry_msgs::msg::Pose & pose, const size_t nearest_segment_idx, const double offset) { const double offset_length = motion_utils::calcLongitudinalOffsetToSegment(points, nearest_segment_idx, pose.position); - const auto offset_pose = - motion_utils::calcLongitudinalOffsetPose(points, nearest_segment_idx, offset_length + offset); + const auto offset_point = + motion_utils::calcLongitudinalOffsetPoint(points, nearest_segment_idx, offset_length + offset); - return offset_pose ? offset_pose.get() : points.at(nearest_segment_idx + 1); + return offset_point ? offset_point.get() : points.at(nearest_segment_idx + 1); } void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, - const std::shared_ptr planner_data) + const std::shared_ptr planner_data, const bool is_driving_forward) { - std::vector left_bound; - std::vector right_bound; + std::vector left_bound; + std::vector right_bound; // extract data const auto transformed_lanes = util::transformToLanelets(lanes); @@ -1140,38 +1140,17 @@ void generateDrivableArea( const auto route_handler = planner_data->route_handler; constexpr double overlap_threshold = 0.01; - auto addLeftBoundPoints = [&left_bound](const lanelet::ConstLanelet & lane) { - for (const auto & lp : lane.leftBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(lp); - if (left_bound.empty()) { - left_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, left_bound.back()) > overlap_threshold) { - left_bound.push_back(current_pose); - } - } - }; - - auto addRightBoundPoints = [&right_bound](const lanelet::ConstLanelet & lane) { - for (const auto & rp : lane.rightBound()) { - geometry_msgs::msg::Pose current_pose; - current_pose.position = lanelet::utils::conversion::toGeomMsgPt(rp); - if (right_bound.empty()) { - right_bound.push_back(current_pose); - } else if ( - tier4_autoware_utils::calcDistance2d(current_pose, right_bound.back()) > - overlap_threshold) { - right_bound.push_back(current_pose); + auto addPoints = + [](const lanelet::ConstLineString3d & points, std::vector & bound) { + for (const auto & bound_p : points) { + const auto cp = lanelet::utils::conversion::toGeomMsgPt(bound_p); + if (bound.empty()) { + bound.push_back(cp); + } else if (tier4_autoware_utils::calcDistance2d(cp, bound.back()) > overlap_threshold) { + bound.push_back(cp); + } } - } - }; - - // Insert Position - for (const auto & lane : lanes) { - addLeftBoundPoints(lane.left_lane); - addRightBoundPoints(lane.right_lane); - } + }; const auto has_overlap = [&](const lanelet::ConstLanelet & lane, const lanelet::Id & ignore_lane_id = lanelet::InvalId) { @@ -1187,6 +1166,12 @@ void generateDrivableArea( return false; }; + // Insert Position + for (const auto & lane : lanes) { + addPoints(lane.left_lane.leftBound3d(), left_bound); + addPoints(lane.right_lane.rightBound3d(), right_bound); + } + // Insert points after goal lanelet::ConstLanelet goal_lanelet; if ( @@ -1208,64 +1193,65 @@ void generateDrivableArea( continue; } - addLeftBoundPoints(lane); - addRightBoundPoints(lane); + addPoints(lane.leftBound3d(), left_bound); + addPoints(lane.rightBound3d(), right_bound); } } - // Give Orientation - motion_utils::insertOrientation(left_bound, true); - motion_utils::insertOrientation(right_bound, true); + if (!is_driving_forward) { + std::reverse(left_bound.begin(), left_bound.end()); + std::reverse(right_bound.begin(), right_bound.end()); + } // Get Closest segment for the start point constexpr double front_length = 0.5; const auto front_pose = path.points.empty() ? current_pose->pose : path.points.front().point.pose; const size_t front_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, front_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, front_pose.position); const size_t front_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, front_pose); - const auto left_start_pose = - calcLongitudinalOffsetStartPose(left_bound, front_pose, front_left_start_idx, -front_length); - const auto right_start_pose = - calcLongitudinalOffsetStartPose(right_bound, front_pose, front_right_start_idx, -front_length); + findNearestSegmentIndexFromLateralDistance(right_bound, front_pose.position); + const auto left_start_point = + calcLongitudinalOffsetStartPoint(left_bound, front_pose, front_left_start_idx, -front_length); + const auto right_start_point = + calcLongitudinalOffsetStartPoint(right_bound, front_pose, front_right_start_idx, -front_length); const size_t left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_start_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_start_point); const size_t right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_start_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_start_point); // Get Closest segment for the goal point const auto goal_pose = path.points.empty() ? current_pose->pose : path.points.back().point.pose; const size_t goal_left_start_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, goal_pose.position); const size_t goal_right_start_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose); - const auto left_goal_pose = - calcLongitudinalOffsetGoalPose(left_bound, goal_pose, goal_left_start_idx, vehicle_length); - const auto right_goal_pose = - calcLongitudinalOffsetGoalPose(right_bound, goal_pose, goal_right_start_idx, vehicle_length); + findNearestSegmentIndexFromLateralDistance(right_bound, goal_pose.position); + const auto left_goal_point = + calcLongitudinalOffsetGoalPoint(left_bound, goal_pose, goal_left_start_idx, vehicle_length); + const auto right_goal_point = + calcLongitudinalOffsetGoalPoint(right_bound, goal_pose, goal_right_start_idx, vehicle_length); const size_t left_goal_idx = - findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_pose); + findNearestSegmentIndexFromLateralDistance(left_bound, left_goal_point); const size_t right_goal_idx = - findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_pose); + findNearestSegmentIndexFromLateralDistance(right_bound, right_goal_point); // Store Data path.left_bound.clear(); path.right_bound.clear(); // Insert a start point - path.left_bound.push_back(left_start_pose.position); - path.right_bound.push_back(right_start_pose.position); + path.left_bound.push_back(left_start_point); + path.right_bound.push_back(right_start_point); // Insert middle points for (size_t i = left_start_idx + 1; i <= left_goal_idx; ++i) { - const auto & next_point = left_bound.at(i).position; + const auto & next_point = left_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.left_bound.back(), next_point); if (dist > overlap_threshold) { path.left_bound.push_back(next_point); } } for (size_t i = right_start_idx + 1; i <= right_goal_idx; ++i) { - const auto & next_point = right_bound.at(i).position; + const auto & next_point = right_bound.at(i); const double dist = tier4_autoware_utils::calcDistance2d(path.right_bound.back(), next_point); if (dist > overlap_threshold) { path.right_bound.push_back(next_point); @@ -1274,14 +1260,14 @@ void generateDrivableArea( // Insert a goal point if ( - tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.left_bound.back(), left_goal_point) > overlap_threshold) { - path.left_bound.push_back(left_goal_pose.position); + path.left_bound.push_back(left_goal_point); } if ( - tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_pose.position) > + tier4_autoware_utils::calcDistance2d(path.right_bound.back(), right_goal_point) > overlap_threshold) { - path.right_bound.push_back(right_goal_pose.position); + path.right_bound.push_back(right_goal_point); } } From b970720117c3f0b272148d5ff1ce79337b947871 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 15:31:58 +0900 Subject: [PATCH 13/37] feat(behavior_path_planner): add a new document for the drivable area generation (#2532) * feat(behavior_path_planner): add drivable area document Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka * add expanded lanes explanation Signed-off-by: yutaka * fix format Signed-off-by: yutaka * update Signed-off-by: yutaka * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_avoidance-design.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe * Update planning/behavior_path_planner/behavior_path_planner_drivable_area.md Co-authored-by: Fumiya Watanabe Signed-off-by: yutaka Co-authored-by: Fumiya Watanabe --- planning/behavior_path_planner/README.md | 22 - .../behavior_path_planner_avoidance-design.md | 1 + .../behavior_path_planner_drivable_area.md | 78 ++++ .../drivable_area/drivable_lines.drawio.svg | 241 +++++++++++ .../drivable_area/expanded_lanes.drawio.svg | 115 +++++ .../drivable_area/sorted_lanes.drawio.svg | 409 ++++++++++++++++++ 6 files changed, 844 insertions(+), 22 deletions(-) create mode 100644 planning/behavior_path_planner/behavior_path_planner_drivable_area.md create mode 100644 planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg create mode 100644 planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg create mode 100644 planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d0e4a49605dd9..d6bf2067c9cb7 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -54,28 +54,6 @@ The following modules are currently supported: ## Inner-workings / Algorithms -### Drivable Area Generation - -Drivable lanes are quantized and drawn on an image as a drivable area, whose resolution is `drivable_area_resolution`. -To prevent the quantization from causing instability to the planning modules, drivable area's pose follows the rules below. - -- Drivable area is generated in the map coordinate. -- Its position is quantized with `drivable_area_resolution`. -- Its orientation is 0. - -The size of the drivable area changes dynamically to realize both decreasing the computation cost and covering enough lanes to follow. -For the second purpose, the drivable area covers a certain length forward and backward lanes with some margins defined by parameters. - -#### Parameters for drivable area generation - -| Name | Unit | Type | Description | Default value | -| :---------------------------- | :--- | :----- | :------------------------------------------------------------------------- | :------------ | -| drivable_area_resolution | [m] | double | resolution of the image of the drivable area | 0.1 | -| drivable_lane_forward_length | [m] | double | length of the forward lane from the ego covered by the drivable area | 50.0 | -| drivable_lane_backward_length | [m] | double | length of the backward lane from the ego covered by the drivable area | 5.0 | -| drivable_lane_margin | [m] | double | forward and backward lane margin from the ego covered by the drivable area | 3.0 | -| drivable_area_margin | [m] | double | margin of width and height of the drivable area | 6.0 | - ### Behavior Tree In the behavior path planner, the behavior tree mechanism is used to manage which modules are activated in which situations. In general, this "behavior manager" like function is expected to become bigger as more and more modules are added in the future. To improve maintainability, we adopted the behavior tree. The behavior tree has the following advantages: easy visualization, easy configuration management (behaviors can be changed by replacing configuration files), and high scalability compared to the state machine. diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index cefc482be4896..821190b62956b 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -422,6 +422,7 @@ The avoidance specific parameter configuration file can be located at `src/autow | enable_avoidance_over_same_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has same direction. If false, avoidance only happen in current lane. | true | | enable_avoidance_over_opposite_direction | [-] | bool | Extend avoidance trajectory to adjacent lanes that has opposite direction. `enable_avoidance_over_same_direction` must be `true` to take effects | true | | enable_update_path_when_object_is_gone | [-] | bool | Reset trajectory when avoided objects are gone. If false, shifted path points remain same even though the avoided objects are gone. | false | +| enable_bound_clipping | `true` | bool | Enable clipping left and right bound of drivable area when obstacles are in the drivable area | false | (\*2) If there are multiple vehicles in a row to be avoided, no new avoidance path will be generated unless their lateral margin difference exceeds this value. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md new file mode 100644 index 0000000000000..88dcf30299a59 --- /dev/null +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -0,0 +1,78 @@ +# Drivable Area + +Drivable Area represents the area where ego vehicle can pass. + +## Purpose / Role + +In order to defined the area that ego vehicle can travel safely, we generate drivable area in behavior path planner module. Our drivable area is represented by two line strings, which are `left_bound` line and `right_bound` line respectively. Both `left_bound` and `right_bound` are created from left and right boundaries of lanelets. Note that `left_bound` and `right bound` are generated by `generateDrivableArea` function. + +## Assumption + +Our drivable area has several assumptions. + +- Drivable Area should have all of the necessary area but should not represent unnecessary area for current behaviors. For example, when ego vehicle is in `follow lane` mode, drivable area should not contain adjacent lanes. + +- When generating a drivable area, lanes need to be arranged in the order in which cars pass by (More details can be found in following sections). + +- Both left and right bounds should cover the front of the path and the end of the path. + +## Limitations + +Currently, when clipping left bound or right bound, it can clip the bound more than necessary and the generated path might be conservative. + +## Parameters for drivable area generation + +| Name | Unit | Type | Description | Default value | +| :------------------------------- | :--- | :----- | :------------------------------------------ | :------------ | +| drivable_area_right_bound_offset | [m] | double | right offset length to expand drivable area | 5.0 | +| drivable_area_left_bound_offset | [m] | double | left offset length to expand drivable area | 5.0 | + +Note that default values can be varied by the module. Please refer to the `config/drivable_area_expansion.yaml` file. + +## Inner-workings / Algorithms + +This section gives details of the generation of the drivable area (`left_bound` and `right_bound`). + +### Drivable Lanes Generation + +Before generating drivable areas, drivable lanes need to be sorted. Drivable Lanes are selected in each module (`Lane Follow`, `Avoidance`, `Lane Change`, `Pull Over`, `Pull Out` and etc.), so more details about selection of drivable lanes can be found in each module's document. We use the following structure to define the drivable lanes. + +```plantuml +struct DrivalbleLanes +{ + lanelet::ConstLanelet right_lanelet; // right most lane + lanelet::ConstLanelet left_lanelet; // left most lane + lanelet::ConstLanelets middle_lanelets; // middle lanes +}; +``` + +The image of the sorted drivable lanes is depicted in the following picture. + +![sorted_lanes](./image/drivable_area/sorted_lanes.drawio.svg) + +Note that, the order of drivable lanes become + +```plantuml +drivable_lanes = {DrivableLane1, DrivableLanes2, DrivableLanes3, DrivableLanes4, DrivableLanes5} +``` + +### Drivable Area Expansion + +Each module can expand the drivable area based on parameters. It expands right bound and left bound of target lanes. This enables large vehicles to pass narrow curve. The image of this process can be described as + +![expanded_lanes](./image/drivable_area/expanded_lanes.drawio.svg) + +Note that we only expand right bound of the rightmost lane and left bound of the leftmost lane. + +### Drivable Area Generation + +In this section, a drivable area is created using drivable lanes arranged in the order in which vehicles pass by. We created `left_bound` from left boundary of the leftmost lanelet and `right_bound` from right boundary of the rightmost lanelet. The image of the created drivable area will be the following blue lines. Note that the drivable area is defined in the `Path` and `PathWithLaneId` messages as + +```plantuml +std::vector left_bound; +std::vector right_bound; +``` + +and each point of right bound and left bound has a position in the absolute coordinate system. + +![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg) diff --git a/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg new file mode 100644 index 0000000000000..fd1729013c1ba --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/drivable_lines.drawio.svg @@ -0,0 +1,241 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + + + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg new file mode 100644 index 0000000000000..c1bd0ccdd0e09 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/expanded_lanes.drawio.svg @@ -0,0 +1,115 @@ + + + + + + + + + + +
+
+
+ + Lane1 +
+ +
+
+
+ + Lane1 + + + + + + + + + +
+
+
+ expanded lanes +
+
+
+ + expanded lanes + + + + + + + + + + +
+
+
+ + Lane2 +
+ +
+
+
+ + Lane2 + + + + + + +
+
+
+ + Lane3 +
+ +
+
+
+ + Lane3 + + + + + + + + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg new file mode 100644 index 0000000000000..3c6bc512ff328 --- /dev/null +++ b/planning/behavior_path_planner/image/drivable_area/sorted_lanes.drawio.svg @@ -0,0 +1,409 @@ + + + + + + + + + + +
+
+
+ Lane 3 +
+
+
+ + Lane 3 + + + + + + +
+
+
+ Lane 7 +
+
+
+ + Lane 7 + + + + + + +
+
+
+ Lane 2 +
+
+
+ + Lane 2 + + + + + + +
+
+
+ Lane 6 +
+
+
+ + Lane 6 + + + + + + + + + +
+
+
+ Lane 4 +
+
+
+ + Lane 4 + + + + + + +
+
+
+ Lane 8 +
+
+
+ + Lane 8 + + + + + + +
+
+
+ Lane 10 +
+
+
+ + Lane 10 + + + + + + +
+
+
+ Lane 9 +
+
+
+ + Lane 9 + + + + + + +
+
+
+ Lane 11 +
+
+
+ + Lane 11 + + + + + + +
+
+
+ + Lane 5 + +
+
+
+ + Lane 5 + + + + + + + + + + +
+
+
+ Lane 1 +
+
+
+ + Lane 1 + + + + + + +
+
+
+
+ DrivableLanes1 +
+
+ { +
+
+ right_lane = 1 +
+
+ left_lane = 1 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes1... + + + + + + +
+
+
+
+ DrivableLanes2 +
+
+ { +
+
+ right_lane = 4 +
+
+ left_lane = 2 +
+
+ middle_lanes = {3} +
+
+ } +
+
+
+
+ + DrivableLanes2... + + + + + + + + + + +
+
+
+
+ DrivableLanes3 +
+
+ { +
+
+ right_lane = 8 +
+
+ left_lane = 5 +
+
+ middle_lanes = {6,7} +
+
+ } +
+
+
+
+ + DrivableLanes3... + + + + + + + + +
+
+
+
+ DrivableLanes4 +
+
+ { +
+
+ right_lane = 10 +
+
+ left_lane = 9 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes4... + + + + + + +
+
+
+
+ DrivableLanes5 +
+
+ { +
+
+ right_lane = 11 +
+
+ left_lane = 11 +
+
+ middle_lanes = {} +
+
+ } +
+
+
+
+ + DrivableLanes5... + + + + + + + + + Text is not SVG - cannot display + + + From 2be41d7bdc658aa2810f9903f938bbb65c2d01f5 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=2E=20Fatih=20C=C4=B1r=C4=B1t?= Date: Tue, 20 Dec 2022 10:50:55 +0300 Subject: [PATCH 14/37] fix(tensorrt): update tensorrt code of traffic_light_classifier (#2325) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: M. Fatih Cırıt --- .../utils/trt_common.cpp | 16 ++++++++++------ .../utils/trt_common.hpp | 2 -- 2 files changed, 10 insertions(+), 8 deletions(-) diff --git a/perception/traffic_light_classifier/utils/trt_common.cpp b/perception/traffic_light_classifier/utils/trt_common.cpp index 4aae3e0ece5e1..adb2fbe037a31 100644 --- a/perception/traffic_light_classifier/utils/trt_common.cpp +++ b/perception/traffic_light_classifier/utils/trt_common.cpp @@ -75,8 +75,16 @@ void TrtCommon::setup() } context_ = UniquePtr(engine_->createExecutionContext()); - input_dims_ = engine_->getBindingDimensions(getInputBindingIndex()); - output_dims_ = engine_->getBindingDimensions(getOutputBindingIndex()); + +#if (NV_TENSORRT_MAJOR * 10000) + (NV_TENSORRT_MINOR * 100) + NV_TENSOR_PATCH >= 80500 + input_dims_ = engine_->getTensorShape(input_name_.c_str()); + output_dims_ = engine_->getTensorShape(output_name_.c_str()); +#else + // Deprecated since 8.5 + input_dims_ = engine_->getBindingDimensions(engine_->getBindingIndex(input_name_.c_str())); + output_dims_ = engine_->getBindingDimensions(engine_->getBindingIndex(output_name_.c_str())); +#endif + is_initialized_ = true; } @@ -155,8 +163,4 @@ int TrtCommon::getNumOutput() output_dims_.d, output_dims_.d + output_dims_.nbDims, 1, std::multiplies()); } -int TrtCommon::getInputBindingIndex() { return engine_->getBindingIndex(input_name_.c_str()); } - -int TrtCommon::getOutputBindingIndex() { return engine_->getBindingIndex(output_name_.c_str()); } - } // namespace Tn diff --git a/perception/traffic_light_classifier/utils/trt_common.hpp b/perception/traffic_light_classifier/utils/trt_common.hpp index 9577dfd2262a6..7fc3d3b3e46d9 100644 --- a/perception/traffic_light_classifier/utils/trt_common.hpp +++ b/perception/traffic_light_classifier/utils/trt_common.hpp @@ -118,8 +118,6 @@ class TrtCommon bool isInitialized(); int getNumInput(); int getNumOutput(); - int getInputBindingIndex(); - int getOutputBindingIndex(); UniquePtr context_; From ecf39cef52ad8b295916ebe59584bd6f60cbf436 Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Dec 2022 17:34:14 +0900 Subject: [PATCH 15/37] fix(intersection): always set RTC distance to default stop line (#2529) always set RTC distance to default stop line Signed-off-by: Mamoru Sobue Signed-off-by: Mamoru Sobue --- .../scene_module/intersection/scene_intersection.cpp | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index d3c20427f118e..6072745b7fe07 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -236,13 +236,9 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * logger_.get_child("state_machine"), *clock_); setSafe(state_machine_.getState() == StateMachine::State::GO); - if (is_entry_prohibited) { - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx_final).point.pose.position)); - } else { - setDistance(std::numeric_limits::lowest()); - } + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); if (!isActivated()) { // if RTC says intersection entry is 'dangerous', insert stop_line(v == 0.0) in this block From 067a4ba5ec94d75b2680fd9f1598d1c2267bdc2b Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Tue, 20 Dec 2022 18:10:08 +0900 Subject: [PATCH 16/37] fix(intersection): fixed stuck vehicle detection area (#2463) Signed-off-by: Mamoru Sobue --- .../intersection.param.yaml | 2 +- .../config/intersection.param.yaml | 2 +- .../intersection/scene_intersection.hpp | 2 +- .../intersection/scene_intersection.cpp | 146 +++++++++--------- .../src/scene_module/intersection/util.cpp | 9 +- 5 files changed, 76 insertions(+), 85 deletions(-) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/intersection.param.yaml @@ -6,7 +6,7 @@ keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss diff --git a/planning/behavior_velocity_planner/config/intersection.param.yaml b/planning/behavior_velocity_planner/config/intersection.param.yaml index 79eb257de6889..220c087d18de0 100644 --- a/planning/behavior_velocity_planner/config/intersection.param.yaml +++ b/planning/behavior_velocity_planner/config/intersection.param.yaml @@ -6,7 +6,7 @@ keep_detection_line_margin: 1.0 # distance (toward path end) from generated stop line. keep detection if ego is before this line and ego.vel < keep_detection_vel_thr keep_detection_vel_thr: 0.833 # == 3.0km/h stuck_vehicle_detect_dist: 3.0 # this should be the length between cars when they are stopped. The actual stuck vehicle detection length will be this value + vehicle_length. - stuck_vehicle_ignore_dist: 7.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) + stuck_vehicle_ignore_dist: 10.0 # obstacle stop max distance(5.0m) + stuck vehicle size / 2 (0.0m-) stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h intersection_velocity: 2.778 # 2.778m/s = 10.0km/h intersection_max_accel: 0.5 # m/ss diff --git a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp index 8decae844adb7..c1097f39bbf01 100644 --- a/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/intersection/scene_intersection.hpp @@ -166,7 +166,7 @@ class IntersectionModule : public SceneModuleInterface Polygon2d generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const; + const double extra_dist, const double ignore_dist) const; /** * @brief Modify objects predicted path. remove path point if the time exceeds timer_thr. diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp index 6072745b7fe07..01a38f42f4d49 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/scene_intersection.cpp @@ -150,87 +150,83 @@ bool IntersectionModule::modifyPathVelocity(PathWithLaneId * path, StopReason * } const size_t closest_idx = closest_idx_opt.get(); - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines = stop_lines_idx_opt.value(); - const size_t stop_line_idx = stop_lines.stop_line; - const size_t pass_judge_line_idx = stop_lines.pass_judge_line; - const size_t keep_detection_line_idx = stop_lines.keep_detection_line; - - const bool is_over_pass_judge_line = - util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx); - const bool is_before_keep_detection_line = - util::isBeforeTargetIndex(*path, closest_idx, current_pose.pose, keep_detection_line_idx); - const bool keep_detection = is_before_keep_detection_line && - std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; - - if (is_over_pass_judge_line && keep_detection) { - // in case ego could not stop exactly before the stop line, but with some overshoot, - // keep detection within some margin under low velocity threshold - RCLCPP_DEBUG( - logger_, - "over the pass judge line, but before keep detection line and low speed, " - "continue planning"); - // no return here, keep planning - } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { - RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(motion_utils::calcSignedArcLength( - path->points, planner_data_->current_pose.pose.position, - path->points.at(stop_line_idx).point.pose.position)); - // no plan needed. - return true; - } - } + /* collision checking */ + bool is_entry_prohibited = false; /* get dynamic object */ const auto objects_ptr = planner_data_->predicted_objects; - /* calculate final stop lines */ - bool is_entry_prohibited = false; - const double detect_length = + /* check stuck vehicle */ + const double ignore_length = + planner_param_.stuck_vehicle_ignore_dist + planner_data_->vehicle_info_.vehicle_length_m; + const double detect_dist = planner_param_.stuck_vehicle_detect_dist + planner_data_->vehicle_info_.vehicle_length_m; const auto stuck_vehicle_detect_area = generateEgoIntersectionLanePolygon( - lanelet_map_ptr, *path, closest_idx, stuck_line_idx, detect_length, - planner_param_.stuck_vehicle_detect_dist); + lanelet_map_ptr, *path, closest_idx, detect_dist, ignore_length); const bool is_stuck = checkStuckVehicleInIntersection(objects_ptr, stuck_vehicle_detect_area); - int stop_line_idx_final = stuck_line_idx; - int pass_judge_line_idx_final = stuck_line_idx; + + /* calculate dynamic collision around detection area */ + const bool has_collision = checkCollision( + lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, + closest_idx, stuck_vehicle_detect_area); + + /* calculate final stop lines */ + int stop_line_idx_final = -1; + int pass_judge_line_idx_final = -1; if (external_go) { is_entry_prohibited = false; } else if (external_stop) { is_entry_prohibited = true; - } else if (is_stuck) { + } else if (is_stuck || has_collision) { is_entry_prohibited = true; - stop_line_idx_final = stuck_line_idx; - pass_judge_line_idx_final = stuck_line_idx; - } else { - /* calculate dynamic collision around detection area */ - const bool has_collision = checkCollision( - lanelet_map_ptr, *path, detection_lanelets, adjacent_lanelets, intersection_area, objects_ptr, - closest_idx, stuck_vehicle_detect_area); - is_entry_prohibited = has_collision; - if (stop_lines_idx_opt.has_value()) { - const auto & stop_lines_idx = stop_lines_idx_opt.value(); - stop_line_idx_final = stop_lines_idx.stop_line; - pass_judge_line_idx_final = stop_lines_idx.pass_judge_line; - } else { - if (has_collision) { - RCLCPP_ERROR(logger_, "generateStopLine() failed for detected objects"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return false; - } else { - RCLCPP_DEBUG(logger_, "no need to stop"); - RCLCPP_DEBUG(logger_, "===== plan end ====="); - setSafe(true); - setDistance(std::numeric_limits::lowest()); - return true; - } + const double dist_stuck_stopline = motion_utils::calcSignedArcLength( + path->points, path->points.at(stuck_line_idx).point.pose.position, + path->points.at(closest_idx).point.pose.position); + const double eps = 1e-1; // NOTE: check if sufficiently over the stuck stopline + const bool is_over_stuck_stopline = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, stuck_line_idx) && + dist_stuck_stopline > eps; + if (is_stuck && !is_over_stuck_stopline) { + stop_line_idx_final = stuck_line_idx; + pass_judge_line_idx_final = stuck_line_idx; + } else if ( + ((is_stuck && is_over_stuck_stopline) || has_collision) && stop_lines_idx_opt.has_value()) { + stop_line_idx_final = stop_lines_idx_opt.value().stop_line; + pass_judge_line_idx_final = stop_lines_idx_opt.value().pass_judge_line; } } + if (stop_line_idx_final == -1) { + RCLCPP_DEBUG(logger_, "detection_area is empty, no plan needed"); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(std::numeric_limits::lowest()); + return false; + } + + const bool is_over_pass_judge_line = + util::isOverTargetIndex(*path, closest_idx, current_pose.pose, pass_judge_line_idx_final); + const bool is_before_keep_detection_line = + stop_lines_idx_opt.has_value() + ? util::isBeforeTargetIndex( + *path, closest_idx, current_pose.pose, stop_lines_idx_opt.value().keep_detection_line) + : false; + const bool keep_detection = + is_before_keep_detection_line && std::fabs(current_vel) < planner_param_.keep_detection_vel_thr; + + if (is_over_pass_judge_line && keep_detection) { + // in case ego could not stop exactly before the stop line, but with some overshoot, + // keep detection within some margin under low velocity threshold + } else if (is_over_pass_judge_line && is_go_out_ && !external_stop) { + RCLCPP_DEBUG(logger_, "over the keep_detection line and not low speed. no plan needed."); + RCLCPP_DEBUG(logger_, "===== plan end ====="); + setSafe(true); + setDistance(motion_utils::calcSignedArcLength( + path->points, planner_data_->current_pose.pose.position, + path->points.at(stop_line_idx_final).point.pose.position)); + return true; + } + state_machine_.setStateWithMarginTime( is_entry_prohibited ? StateMachine::State::STOP : StateMachine::State::GO, logger_.get_child("state_machine"), *clock_); @@ -314,8 +310,11 @@ bool IntersectionModule::checkCollision( using lanelet::utils::getPolygonFromArcLength; /* generate ego-lane polygon */ - const auto ego_poly = - generateEgoIntersectionLanePolygon(lanelet_map_ptr, path, closest_idx, closest_idx, 0.0, 0.0); + const auto ego_lane_poly = lanelet_map_ptr->laneletLayer.get(module_id_).polygon2d(); + Polygon2d ego_poly{}; + for (const auto & p : ego_lane_poly) { + ego_poly.outer().emplace_back(p.x(), p.y()); + } lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); lanelet::ConstLanelet closest_lanelet; lanelet::utils::query::getClosestLanelet( @@ -475,7 +474,7 @@ bool IntersectionModule::checkCollision( Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::LaneletMapConstPtr lanelet_map_ptr, const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const int closest_idx, - const int start_idx, const double extra_dist, const double ignore_dist) const + const double extra_dist, const double ignore_dist) const { using lanelet::utils::getArcCoordinates; using lanelet::utils::getLaneletLength3d; @@ -484,15 +483,14 @@ Polygon2d IntersectionModule::generateEgoIntersectionLanePolygon( lanelet::ConstLanelets ego_lane_with_next_lane = getEgoLaneWithNextLane(lanelet_map_ptr, path); - const auto start_arc_coords = getArcCoordinates( - ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(start_idx).point)); + const double intersection_exit_length = getLaneletLength3d(ego_lane_with_next_lane.front()); const auto closest_arc_coords = getArcCoordinates( ego_lane_with_next_lane, tier4_autoware_utils::getPose(path.points.at(closest_idx).point)); - const double start_arc_length = start_arc_coords.length + ignore_dist < closest_arc_coords.length - ? closest_arc_coords.length - : start_arc_coords.length + ignore_dist; + const double start_arc_length = intersection_exit_length - ignore_dist > closest_arc_coords.length + ? intersection_exit_length - ignore_dist + : closest_arc_coords.length; const double end_arc_length = getLaneletLength3d(ego_lane_with_next_lane.front()) + extra_dist; diff --git a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp index a7e1739c38119..1f4cba618fe28 100644 --- a/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/intersection/util.cpp @@ -148,7 +148,7 @@ std::pair, std::optional> generateStopLine( const double keep_detection_line_margin, const bool use_stuck_stopline, autoware_auto_planning_msgs::msg::PathWithLaneId * original_path, const autoware_auto_planning_msgs::msg::PathWithLaneId & target_path, const rclcpp::Logger logger, - const rclcpp::Clock::SharedPtr clock) + [[maybe_unused]] const rclcpp::Clock::SharedPtr clock) { /* set judge line dist */ const double current_vel = planner_data->current_velocity->twist.linear.x; @@ -196,13 +196,6 @@ std::pair, std::optional> generateStopLine( if (use_stuck_stopline) { // the first point in intersection lane stuck_stop_line_idx_ip = lane_interval_ip_start; - if (stuck_stop_line_idx_ip == 0) { - RCLCPP_WARN_SKIPFIRST_THROTTLE( - logger, *clock, 1000 /* ms */, - "use_stuck_stopline, but ego is already in the intersection, not generating stuck stop " - "line"); - return {std::nullopt, std::nullopt}; - } } else { const auto stuck_stop_line_idx_ip_opt = util::getFirstPointInsidePolygons( path_ip, lane_interval_ip_start, lane_interval_ip_end, lane_id, conflicting_areas); From fb4164f1d6a74e96603c657d764b4a5587d08c84 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Tue, 20 Dec 2022 18:11:29 +0900 Subject: [PATCH 17/37] fix(behavior_path_planner): candidate path header (#2536) Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../behavior_path_planner/src/behavior_path_planner_node.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 58afd67a77f14..f9d98b0241629 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -788,6 +788,9 @@ Path BehaviorPathPlannerNode::convertToPath( } output = util::toPath(*path_candidate_ptr); + // header is replaced by the input one, so it is substituted again + output.header = planner_data_->route_handler->getRouteHeader(); + output.header.stamp = this->now(); if (!is_ready) { for (auto & point : output.points) { From 9085f2f31f0d8cbba6cbe3ffc3b8ca447631ad32 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Tue, 20 Dec 2022 20:28:57 +0900 Subject: [PATCH 18/37] fix(tier4_control_launch): add parameter about nearest search (#2542) Signed-off-by: tomoya.kimura Signed-off-by: tomoya.kimura --- .../operation_mode_transition_manager.param.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml index a0ddef6e363cc..a86443f5cabdb 100644 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml @@ -3,6 +3,8 @@ transition_timeout: 10.0 frequency_hz: 10.0 check_engage_condition: false # set false if you do not want to care about the engage condition. + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index + nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index engage_acceptable_limits: allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. dist_threshold: 1.5 From 078857d5e3dda8028f7a0cddd08cb3613f83d644 Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Tue, 20 Dec 2022 22:49:53 +0900 Subject: [PATCH 19/37] feat(behavior_path_planner): rename drivable area name (#2535) * feat(behavior_path_planner): rename drivable area name Signed-off-by: yutaka * update document Signed-off-by: yutaka * update Signed-off-by: yutaka * update Signed-off-by: yutaka Signed-off-by: yutaka --- .../behavior_path_planner_avoidance-design.md | 22 ------------------ .../behavior_path_planner_drivable_area.md | 14 +++++++++++ .../config/behavior_path_planner.param.yaml | 2 +- ...drivable_area_boundary_marker_example1.png | Bin ...drivable_area_boundary_marker_example2.png | Bin .../behavior_path_planner_node.hpp | 2 +- .../behavior_path_planner/parameters.hpp | 4 ++-- .../behavior_path_planner/utilities.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 17 +++++++------- .../behavior_path_planner/src/utilities.cpp | 2 +- 10 files changed, 28 insertions(+), 37 deletions(-) rename planning/behavior_path_planner/image/{avoidance_design => drivable_area}/drivable_area_boundary_marker_example1.png (100%) rename planning/behavior_path_planner/image/{avoidance_design => drivable_area}/drivable_area_boundary_marker_example2.png (100%) diff --git a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md index 821190b62956b..3a2bc1a0cd1c0 100644 --- a/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md +++ b/planning/behavior_path_planner/behavior_path_planner_avoidance-design.md @@ -513,25 +513,3 @@ To print the debug message, just run the following ```bash ros2 topic echo /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/avoidance_debug_message_array ``` - -### Visualizing drivable area boundary - -Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. - -For example, in the same area, one can perform avoidance and another cannot. This might be related to the drivable area issues due to the non-compliance vector map design from the user. - -To debug the issue, the drivable area boundary can be visualized. - -![drivable_area_boundary_marker1](./image/avoidance_design/drivable_area_boundary_marker_example1.png) - -![drivable_area_boundary_marker2](./image/avoidance_design/drivable_area_boundary_marker_example2.png) - -The boundary can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_boundary` - -### Visualizing drivable area - -The drivable area can be visualize by adding the drivable area plugin - -![drivable_area_plugin](./image/drivable_area_plugin.png) - -and then add `/planning/scenario_planning/lane_driving/behavior_planning/path` as the topic. diff --git a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md index 88dcf30299a59..0d17c8c9fd509 100644 --- a/planning/behavior_path_planner/behavior_path_planner_drivable_area.md +++ b/planning/behavior_path_planner/behavior_path_planner_drivable_area.md @@ -76,3 +76,17 @@ std::vector right_bound; and each point of right bound and left bound has a position in the absolute coordinate system. ![drivable_lines](./image/drivable_area/drivable_lines.drawio.svg) + +### Visualizing maximum drivable area (Debug) + +Sometimes, the developers might get a different result between two maps that may look identical during visual inspection. + +For example, in the same area, one can perform avoidance and another cannot. This might be related to the maximum drivable area issues due to the non-compliance vector map design from the user. + +To debug the issue, the maximum drivable area boundary can be visualized. + +![drivable_area_boundary_marker1](./image/drivable_area/drivable_area_boundary_marker_example1.png) + +![drivable_area_boundary_marker2](./image/drivable_area/drivable_area_boundary_marker_example2.png) + +The maximum drivable area can be visualize by adding the marker from `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/maximum_drivable_area` diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml index da2342f6782d4..6dce3ff06206b 100644 --- a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml +++ b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml @@ -21,7 +21,7 @@ path_interval: 2.0 - visualize_drivable_area_for_shared_linestrings_lanelet: true + visualize_maximum_drivable_area: true lateral_distance_max_threshold: 2.0 longitudinal_distance_min_threshold: 3.0 diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example1.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example1.png diff --git a/planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png b/planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png similarity index 100% rename from planning/behavior_path_planner/image/avoidance_design/drivable_area_boundary_marker_example2.png rename to planning/behavior_path_planner/image/drivable_area/drivable_area_boundary_marker_example2.png diff --git a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp index 75d1f798d8b65..d4bc06a33d349 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/behavior_path_planner_node.hpp @@ -178,7 +178,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node const BehaviorModuleOutput & output); // debug - rclcpp::Publisher::SharedPtr debug_drivable_area_lanelets_publisher_; + rclcpp::Publisher::SharedPtr debug_maximum_drivable_area_publisher_; rclcpp::Publisher::SharedPtr debug_avoidance_msg_array_publisher_; rclcpp::Publisher::SharedPtr debug_lane_change_msg_array_publisher_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 8103092e23cdc..7caa25a9b4cf6 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -58,8 +58,8 @@ struct BehaviorPathPlannerParameters double base_link2front; double base_link2rear; - // drivable area visualization - bool visualize_drivable_area_for_shared_linestrings_lanelet; + // maximum drivable area visualization + bool visualize_maximum_drivable_area; // collision check double lateral_distance_max_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 6b684998141c5..2de16855391ff 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -296,7 +296,7 @@ void generateDrivableArea( PathWithLaneId & path, const std::vector & lanes, const double vehicle_length, const std::shared_ptr planner_data, const bool is_driving_forward = true); -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data); /** diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index f9d98b0241629..5f5de72b26160 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -69,9 +69,9 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod debug_lane_change_msg_array_publisher_ = create_publisher("~/debug/lane_change_debug_message_array", 1); - if (planner_data_->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - debug_drivable_area_lanelets_publisher_ = - create_publisher("~/drivable_area_boundary", 1); + if (planner_data_->parameters.visualize_maximum_drivable_area) { + debug_maximum_drivable_area_publisher_ = + create_publisher("~/maximum_drivable_area", 1); } bound_publisher_ = create_publisher("~/debug/bound", 1); @@ -232,8 +232,7 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true); p.path_interval = declare_parameter("path_interval"); - p.visualize_drivable_area_for_shared_linestrings_lanelet = - declare_parameter("visualize_drivable_area_for_shared_linestrings_lanelet", true); + p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -655,10 +654,10 @@ void BehaviorPathPlannerNode::run() publishSceneModuleDebugMsg(); - if (planner_data->parameters.visualize_drivable_area_for_shared_linestrings_lanelet) { - const auto drivable_area_lines = marker_utils::createFurthestLineStringMarkerArray( - util::getDrivableAreaForAllSharedLinestringLanelets(planner_data)); - debug_drivable_area_lanelets_publisher_->publish(drivable_area_lines); + if (planner_data->parameters.visualize_maximum_drivable_area) { + const auto maximum_drivable_area = + marker_utils::createFurthestLineStringMarkerArray(util::getMaximumDrivableArea(planner_data)); + debug_maximum_drivable_area_publisher_->publish(maximum_drivable_area); } mutex_bt_.unlock(); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index d335c6c13b582..c38b54086b3fd 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1665,7 +1665,7 @@ std::shared_ptr generateCenterLinePath( // TODO(Azu) Some parts of is the same with generateCenterLinePath. Therefore it might be better if // we can refactor some of the code for better readability -lanelet::ConstLineStrings3d getDrivableAreaForAllSharedLinestringLanelets( +lanelet::ConstLineStrings3d getMaximumDrivableArea( const std::shared_ptr & planner_data) { const auto & p = planner_data->parameters; From febbc135b8e09e993ed345ee6d3cd7e65b6c1d68 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 21 Dec 2022 10:03:24 +0900 Subject: [PATCH 20/37] refactor(obstacle_cruise_planner): clean up the code (#2526) * refactor obstacle_cruise_planner Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * add vel lpf and disable_target_acceleration Signed-off-by: Takayuki Murooka * update Signed-off-by: Takayuki Murooka * fix README.md Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- planning/obstacle_cruise_planner/README.md | 58 ++++-- .../config/obstacle_cruise_planner.param.yaml | 49 +++-- .../common_structs.hpp | 86 +++++--- .../include/obstacle_cruise_planner/node.hpp | 54 +---- .../optimization_based_planner.hpp | 17 +- .../pid_based_planner/debug_values.hpp | 79 ------- .../pid_based_planner/pid_based_planner.hpp | 2 + .../planner_interface.hpp | 68 ++---- .../obstacle_cruise_planner/polygon_utils.hpp | 35 ++-- .../obstacle_cruise_planner/type_alias.hpp | 61 ++++++ .../include/obstacle_cruise_planner/utils.hpp | 36 +--- planning/obstacle_cruise_planner/src/node.cpp | 194 +++++------------- .../optimization_based_planner.cpp | 36 ++-- .../pid_based_planner/pid_based_planner.cpp | 46 +++-- .../src/planner_interface.cpp | 32 ++- .../src/polygon_utils.cpp | 29 ++- .../obstacle_cruise_planner/src/utils.cpp | 42 +--- 17 files changed, 385 insertions(+), 539 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/README.md b/planning/obstacle_cruise_planner/README.md index 5f372a5baed34..e3ddbfb1b157a 100644 --- a/planning/obstacle_cruise_planner/README.md +++ b/planning/obstacle_cruise_planner/README.md @@ -4,30 +4,29 @@ The `obstacle_cruise_planner` package has following modules. -- obstacle stop planning - - inserting a stop point in the trajectory when there is a static obstacle on the trajectory. -- adaptive cruise planning - - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory +- Stop planning + - stop when there is a static obstacle near the trajectory. +- Cruise planning + - slow down when there is a dynamic obstacle to cruise near the trajectory ## Interfaces ### Input topics -| Name | Type | Description | -| ----------------------------- | ----------------------------------------------- | --------------------------------- | -| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | -| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory with smoothed velocity | -| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | -| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | +| Name | Type | Description | +| -------------------- | ----------------------------------------------- | ---------------- | +| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory | input trajectory | +| `~/input/objects` | autoware_auto_perception_msgs::PredictedObjects | dynamic objects | +| `~/input/odometry` | nav_msgs::msg::Odometry | ego odometry | ### Output topics -| Name | Type | Description | -| ------------------------------ | ---------------------------------------------- | ------------------------------------- | -| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | -| `~output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | -| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | -| `~output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | +| Name | Type | Description | +| ------------------------------- | ---------------------------------------------- | ------------------------------------- | +| `~/output/trajectory` | autoware_auto_planning_msgs::Trajectory | output trajectory | +| `~/output/velocity_limit` | tier4_planning_msgs::VelocityLimit | velocity limit for cruising | +| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit | +| `~/output/stop_reasons` | tier4_planning_msgs::StopReasonArray | reasons that make the vehicle to stop | ## Design @@ -176,16 +175,37 @@ This includes not only cruising a front vehicle, but also reacting a cut-in and The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation. $$ -d = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, +d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}}, $$ -assuming that $d$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. +assuming that $d_rss$ is the calculated safe distance, $t_{idling}$ is the idling time for the ego to detect the front vehicle's deceleration, $v_{ego}$ is the ego's current velocity, $v_{obstacle}$ is the front obstacle's current velocity, $a_{ego}$ is the ego's acceleration, and $a_{obstacle}$ is the obstacle's acceleration. These values are parameterized as follows. Other common values such as ego's minimum acceleration is defined in `common.param.yaml`. | Parameter | Type | Description | | --------------------------------- | ------ | ----------------------------------------------------------------------------- | | `common.idling_time` | double | idling time for the ego to detect the front vehicle starting deceleration [s] | -| `common.min_object_accel_for_rss` | double | front obstacle's acceleration [m/ss] | +| `common.min_ego_accel_for_rss` | double | ego's acceleration for RSS [m/ss] | +| `common.min_object_accel_for_rss` | double | front obstacle's acceleration for RSS [m/ss] | + +The detailed formulation is as follows. + +$$ +d_{error} = d - d_{rss} \\ +d_{normalized} = lpf(d_{error} / d_{obstacle}) \\ +d_{quad, normalized} = sign(d_{normalized}) *d_{normalized}*d_{normalized} \\ +v_{pid} = pid(d_{quad, normalized}) \\ +v_{add} = v_{pid} > 0 ? v_{pid}* w_{acc} : v_{pid} \\ +v_{target} = max(v_{ego} + v_{add}, v_{min, cruise}) +$$ + +| Variable | Description | +| ----------------- | --------------------------------------- | +| `d` | actual distane to obstacle | +| `d_{rss}` | ideal distance to obstacle based on RSS | +| `v_{min, cruise}` | `min_cruise_target_vel` | +| `w_{acc}` | `output_ratio_during_accel` | +| `lpf(val)` | apply low-pass filter to `val` | +| `pid(val)` | apply pid to `val` | ## Implementation diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 520637abed775..dfc2b2e6ddb44 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -3,15 +3,18 @@ common: planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - is_showing_debug_info: true + is_showing_debug_info: false + disable_stop_planning: false # true # longitudinal info - idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] + idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + lpf_gain_for_accel: 0.2 # gain of low pass filter for ego acceleration [-] + nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] @@ -25,7 +28,7 @@ bus: true trailer: true motorcycle: true - bicycle: false + bicycle: true pedestrian: false stop_obstacle_type: @@ -62,7 +65,7 @@ stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle ignored_outside_obstacle_type: - unknown: false + unknown: true car: false truck: false bus: false @@ -72,21 +75,41 @@ pedestrian: true pid_based_planner: - # use_predicted_obstacle_pose: false + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic - # PID gains to keep safe distance with the front vehicle - kp: 2.5 - ki: 0.0 - kd: 2.3 + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 - min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false - output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + disable_target_acceleration: true + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 - lpf_cruise_gain: 0.2 + lpf_normalized_error_cruise_dist_gain: 0.2 optimization_based_planner: dense_resampling_time_interval: 0.2 diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp index 43949c55b21a5..3c35abe7fa14c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp @@ -15,37 +15,16 @@ #ifndef OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ #define OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_perception_msgs::msg::Shape; - -namespace -{ -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} -} // namespace - struct TargetObstacle { TargetObstacle( @@ -59,7 +38,7 @@ struct TargetObstacle velocity_reliable = true; velocity = aligned_velocity; classification = object.classification.at(0); - uuid = toHexString(object.object_id); + uuid = tier4_autoware_utils::toHexString(object.object_id); predicted_paths.clear(); for (const auto & path : object.kinematics.predicted_paths) { @@ -85,7 +64,7 @@ struct TargetObstacle struct ObstacleCruisePlannerData { rclcpp::Time current_time; - autoware_auto_planning_msgs::msg::Trajectory traj; + Trajectory traj; geometry_msgs::msg::Pose current_pose; double current_vel; double current_acc; @@ -95,6 +74,50 @@ struct ObstacleCruisePlannerData struct LongitudinalInfo { + explicit LongitudinalInfo(rclcpp::Node & node) + { + max_accel = node.declare_parameter("normal.max_acc"); + min_accel = node.declare_parameter("normal.min_acc"); + max_jerk = node.declare_parameter("normal.max_jerk"); + min_jerk = node.declare_parameter("normal.min_jerk"); + limit_max_accel = node.declare_parameter("limit.max_acc"); + limit_min_accel = node.declare_parameter("limit.min_acc"); + limit_max_jerk = node.declare_parameter("limit.max_jerk"); + limit_min_jerk = node.declare_parameter("limit.min_jerk"); + + idling_time = node.declare_parameter("common.idling_time"); + min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); + min_object_accel_for_rss = node.declare_parameter("common.min_object_accel_for_rss"); + + safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); + terminal_safe_distance_margin = + node.declare_parameter("common.terminal_safe_distance_margin"); + } + + void onParam(const std::vector & parameters) + { + tier4_autoware_utils::updateParam(parameters, "normal.max_accel", max_accel); + tier4_autoware_utils::updateParam(parameters, "normal.min_accel", min_accel); + tier4_autoware_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + tier4_autoware_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + tier4_autoware_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + + tier4_autoware_utils::updateParam(parameters, "common.idling_time", idling_time); + tier4_autoware_utils::updateParam( + parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); + tier4_autoware_utils::updateParam( + parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); + + tier4_autoware_utils::updateParam( + parameters, "common.safe_distance_margin", safe_distance_margin); + tier4_autoware_utils::updateParam( + parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + } + + // common parameter double max_accel; double min_accel; double max_jerk; @@ -103,9 +126,13 @@ struct LongitudinalInfo double limit_min_accel; double limit_max_jerk; double limit_min_jerk; + + // rss parameter double idling_time; double min_ego_accel_for_rss; double min_object_accel_for_rss; + + // distance margin double safe_distance_margin; double terminal_safe_distance_margin; }; @@ -115,18 +142,21 @@ struct DebugData std::vector intentionally_ignored_obstacles; std::vector obstacles_to_stop; std::vector obstacles_to_cruise; - visualization_msgs::msg::MarkerArray stop_wall_marker; - visualization_msgs::msg::MarkerArray cruise_wall_marker; + MarkerArray stop_wall_marker; + MarkerArray cruise_wall_marker; std::vector detection_polygons; std::vector collision_points; }; struct EgoNearestParam { - EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold) - : dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold) + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node & node) { + dist_threshold = node.declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node.declare_parameter("ego_nearest_yaw_threshold"); } + double dist_threshold; double yaw_threshold; }; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 8ce9ee3d7e925..e52c2671fea5e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -18,25 +18,12 @@ #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" #include "obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "signal_processing/lowpass_filter_1d.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/accel_stamped.hpp" -#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/point_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include "tier4_debug_msgs/msg/float32_stamped.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -44,21 +31,6 @@ #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedObject; -using autoware_auto_perception_msgs::msg::PredictedObjects; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using geometry_msgs::msg::AccelStamped; -using geometry_msgs::msg::AccelWithCovarianceStamped; -using nav_msgs::msg::Odometry; -using tier4_debug_msgs::msg::Float32Stamped; -using tier4_planning_msgs::msg::StopReasonArray; -using tier4_planning_msgs::msg::VelocityLimit; -using tier4_planning_msgs::msg::VelocityLimitClearCommand; -using vehicle_info_util::VehicleInfo; - namespace motion_planning { class ObstacleCruisePlannerNode : public rclcpp::Node @@ -70,9 +42,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // callback functions rcl_interfaces::msg::SetParametersResult onParam( const std::vector & parameters); - void onObjects(const PredictedObjects::ConstSharedPtr msg); - void onOdometry(const Odometry::ConstSharedPtr); - void onAccel(const AccelWithCovarianceStamped::ConstSharedPtr); void onTrajectory(const Trajectory::ConstSharedPtr msg); void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); @@ -111,8 +80,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool is_showing_debug_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; double obstacle_velocity_threshold_from_cruise_to_stop_; double obstacle_velocity_threshold_from_stop_to_cruise_; @@ -126,27 +93,23 @@ class ObstacleCruisePlannerNode : public rclcpp::Node rclcpp::Publisher::SharedPtr trajectory_pub_; rclcpp::Publisher::SharedPtr vel_limit_pub_; rclcpp::Publisher::SharedPtr clear_vel_limit_pub_; - rclcpp::Publisher::SharedPtr debug_marker_pub_; - rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; - rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber - rclcpp::Subscription::SharedPtr trajectory_sub_; - rclcpp::Subscription::SharedPtr smoothed_trajectory_sub_; + rclcpp::Subscription::SharedPtr traj_sub_; rclcpp::Subscription::SharedPtr objects_sub_; rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr acc_sub_; - // self pose listener - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - // data for callback functions PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr}; - geometry_msgs::msg::TwistStamped::SharedPtr current_twist_ptr_{nullptr}; - geometry_msgs::msg::AccelStamped::SharedPtr current_accel_ptr_{nullptr}; + Odometry::ConstSharedPtr current_odom_ptr_{nullptr}; + AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_{nullptr}; // Vehicle Parameters VehicleInfo vehicle_info_; @@ -195,13 +158,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleFilteringParam obstacle_filtering_param_; bool need_to_clear_vel_limit_{false}; + EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; bool disable_stop_planning_{false}; std::vector prev_target_obstacles_; - - std::shared_ptr lpf_acc_ptr_; }; } // namespace motion_planning diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index 609a09e00d219..bdd1bb5ee276e 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -18,6 +18,7 @@ #include "obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" #include "obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" #include "obstacle_cruise_planner/planner_interface.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" @@ -25,20 +26,12 @@ #include #include -#include -#include - #include #include #include #include -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_perception_msgs::msg::PredictedPath; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::Float32Stamped; - class OptimizationBasedPlanner : public PlannerInterface { public: @@ -77,8 +70,7 @@ class OptimizationBasedPlanner : public PlannerInterface const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::PointStamped & point); boost::optional calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose); + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); geometry_msgs::msg::Pose transformBaseLink2Center( const geometry_msgs::msg::Pose & pose_base_link); @@ -102,6 +94,11 @@ class OptimizationBasedPlanner : public PlannerInterface rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + // Resampling Parameter double dense_resampling_time_interval_; double sparse_resampling_time_interval_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp deleted file mode 100644 index ae8a909d3bb51..0000000000000 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/debug_values.hpp +++ /dev/null @@ -1,79 +0,0 @@ -// Copyright 2022 TIER IV, Inc. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -#ifndef OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ -#define OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ - -#include - -class DebugValues -{ -public: - enum class TYPE { - // current - CURRENT_VELOCITY = 0, - CURRENT_ACCELERATION, - CURRENT_JERK, // ignored - // stop - STOP_CURRENT_OBJECT_DISTANCE = 3, - STOP_CURRENT_OBJECT_VELOCITY, - STOP_TARGET_OBJECT_DISTANCE, - STOP_TARGET_VELOCITY, // ignored - STOP_TARGET_ACCELERATION, - STOP_TARGET_JERK, // ignored - STOP_ERROR_OBJECT_DISTANCE, - // cruise - CRUISE_CURRENT_OBJECT_DISTANCE = 10, - CRUISE_CURRENT_OBJECT_VELOCITY, - CRUISE_TARGET_OBJECT_DISTANCE, - CRUISE_TARGET_VELOCITY, - CRUISE_TARGET_ACCELERATION, - CRUISE_TARGET_JERK, - CRUISE_ERROR_OBJECT_DISTANCE, - - SIZE - }; - - /** - * @brief get the index corresponding to the given value TYPE - * @param [in] type the TYPE enum for which to get the index - * @return index of the type - */ - int getValuesIdx(const TYPE type) const { return static_cast(type); } - /** - * @brief get all the debug values as an std::array - * @return array of all debug values - */ - std::array(TYPE::SIZE)> getValues() const { return values_; } - /** - * @brief set the given type to the given value - * @param [in] type TYPE of the value - * @param [in] value value to set - */ - void setValues(const TYPE type, const double val) { values_.at(static_cast(type)) = val; } - /** - * @brief set the given type to the given value - * @param [in] type index of the type - * @param [in] value value to set - */ - void setValues(const int type, const double val) { values_.at(type) = val; } - - void resetValues() { values_.fill(0.0); } - -private: - static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); - std::array(TYPE::SIZE)> values_; -}; - -#endif // OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__DEBUG_VALUES_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index c2308adc1ca17..f94516407e3c5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -102,6 +102,7 @@ class PIDBasedPlanner : public PlannerInterface double output_ratio_during_accel; double vel_to_acc_weight; bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; }; VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; @@ -131,6 +132,7 @@ class PIDBasedPlanner : public PlannerInterface std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; // lpf for output + std::shared_ptr lpf_output_vel_ptr_; std::shared_ptr lpf_output_acc_ptr_; std::shared_ptr lpf_output_jerk_ptr_; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index a01d3589245b5..06d7710290081 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -18,31 +18,15 @@ #include "motion_utils/motion_utils.hpp" #include "obstacle_cruise_planner/common_structs.hpp" #include "obstacle_cruise_planner/stop_planning_debug_info.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "obstacle_cruise_planner/utils.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" -#include "tier4_planning_msgs/msg/velocity_limit.hpp" -#include "visualization_msgs/msg/marker_array.hpp" #include #include #include -using autoware_adapi_v1_msgs::msg::VelocityFactor; -using autoware_adapi_v1_msgs::msg::VelocityFactorArray; -using autoware_auto_perception_msgs::msg::ObjectClassification; -using autoware_auto_planning_msgs::msg::Trajectory; -using autoware_auto_planning_msgs::msg::TrajectoryPoint; -using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_planning_msgs::msg::StopSpeedExceeded; -using tier4_planning_msgs::msg::VelocityLimit; - class PlannerInterface { public: @@ -53,8 +37,7 @@ class PlannerInterface vehicle_info_(vehicle_info), ego_nearest_param_(ego_nearest_param) { - stop_reasons_pub_ = - node.create_publisher("~/output/stop_reasons", 1); + stop_reasons_pub_ = node.create_publisher("~/output/stop_reasons", 1); velocity_factors_pub_ = node.create_publisher("/planning/velocity_factors/obstacle_cruise", 1); stop_speed_exceeded_pub_ = @@ -63,14 +46,10 @@ class PlannerInterface PlannerInterface() = default; - void setParams( - const bool is_showing_debug_info, const double min_behavior_stop_margin, - const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold) + void setParam(const bool is_showing_debug_info, const double min_behavior_stop_margin) { is_showing_debug_info_ = is_showing_debug_info; min_behavior_stop_margin_ = min_behavior_stop_margin; - nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold; - nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold; } Trajectory generateStopTrajectory( @@ -80,31 +59,10 @@ class PlannerInterface const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, DebugData & debug_data) = 0; - void updateCommonParam(const std::vector & parameters) - { - auto & i = longitudinal_info_; - - tier4_autoware_utils::updateParam(parameters, "common.max_accel", i.max_accel); - tier4_autoware_utils::updateParam(parameters, "common.min_accel", i.min_accel); - tier4_autoware_utils::updateParam(parameters, "common.max_jerk", i.max_jerk); - tier4_autoware_utils::updateParam(parameters, "common.min_jerk", i.min_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.max_accel", i.limit_max_accel); - tier4_autoware_utils::updateParam(parameters, "limit.min_accel", i.limit_min_accel); - tier4_autoware_utils::updateParam(parameters, "limit.max_jerk", i.limit_max_jerk); - tier4_autoware_utils::updateParam(parameters, "limit.min_jerk", i.limit_min_jerk); - tier4_autoware_utils::updateParam( - parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss); - tier4_autoware_utils::updateParam( - parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss); - tier4_autoware_utils::updateParam(parameters, "common.idling_time", i.idling_time); - } - - virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} - - // TODO(shimizu) remove this function - void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj) + void onParam(const std::vector & parameters) { - smoothed_trajectory_ptr_ = traj; + updateCommonParam(parameters); + updateParam(parameters); } Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const @@ -122,11 +80,9 @@ class PlannerInterface bool is_showing_debug_info_{false}; LongitudinalInfo longitudinal_info_; double min_behavior_stop_margin_; - double nearest_dist_deviation_threshold_; - double nearest_yaw_deviation_threshold_; // Publishers - rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr stop_reasons_pub_; rclcpp::Publisher::SharedPtr velocity_factors_pub_; rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; @@ -138,9 +94,6 @@ class PlannerInterface // debug info StopPlanningDebugInfo stop_planning_debug_info_; - // TODO(shimizu) remove these parameters - Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; - double calcDistanceToCollisionPoint( const ObstacleCruisePlannerData & planner_data, const geometry_msgs::msg::Point & collision_point); @@ -155,6 +108,13 @@ class PlannerInterface return rss_dist_with_margin; } + void updateCommonParam(const std::vector & parameters) + { + longitudinal_info_.onParam(parameters); + } + + virtual void updateParam([[maybe_unused]] const std::vector & parameters) {} + size_t findEgoIndex(const Trajectory & traj, const geometry_msgs::msg::Pose & ego_pose) const { const auto traj_points = motion_utils::convertToTrajectoryPointArray(traj); diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp index 651c63d4593a5..6e468bd82defa 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/polygon_utils.hpp @@ -15,14 +15,10 @@ #ifndef OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include "vehicle_info_util/vehicle_info_util.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/point32.hpp" -#include "geometry_msgs/msg/pose.hpp" - #include #include @@ -36,39 +32,36 @@ using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_points, const double max_dist = std::numeric_limits::max()); std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist = std::numeric_limits::max(), const double max_prediction_time_for_collision_check = std::numeric_limits::max()); std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward); std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width); + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width); geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward); + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward); } // namespace polygon_utils #endif // OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp new file mode 100644 index 0000000000000..7f42512646f3c --- /dev/null +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/type_alias.hpp @@ -0,0 +1,61 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#define OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ + +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" +#include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_planning_msgs/msg/stop_factor.hpp" +#include "tier4_planning_msgs/msg/stop_reason_array.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +using autoware_adapi_v1_msgs::msg::VelocityFactor; +using autoware_adapi_v1_msgs::msg::VelocityFactorArray; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using nav_msgs::msg::Odometry; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_planning_msgs::msg::StopFactor; +using tier4_planning_msgs::msg::StopReason; +using tier4_planning_msgs::msg::StopReasonArray; +using tier4_planning_msgs::msg::StopSpeedExceeded; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using vehicle_info_util::VehicleInfo; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; + +#endif // OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 553f4ecfcfe9b..b509192d34371 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -17,18 +17,11 @@ #include "common_structs.hpp" #include "motion_utils/motion_utils.hpp" +#include "obstacle_cruise_planner/type_alias.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" #include -#include "autoware_auto_perception_msgs/msg/object_classification.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_object.hpp" -#include "autoware_auto_perception_msgs/msg/predicted_path.hpp" -#include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "visualization_msgs/msg/marker_array.hpp" - #include #include @@ -37,36 +30,27 @@ namespace obstacle_cruise_utils { -using autoware_auto_perception_msgs::msg::ObjectClassification; - -bool isVehicle(const uint8_t label); - -visualization_msgs::msg::Marker getObjectMarker( +Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b); boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length); + const Trajectory & traj, const size_t start_idx, const double target_length); boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time); + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time); geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction); + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction); boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles); - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id); + const Trajectory & traj, const std::vector & target_obstacles); template size_t getIndexWithLongitudinalOffset( diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 71ec3e5ef200c..8b344affd9c7f 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -29,7 +29,7 @@ namespace { -VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time & current_time) +VelocityLimitClearCommand createVelocityLimitClearCommandMessage(const rclcpp::Time & current_time) { VelocityLimitClearCommand msg; msg.stamp = current_time; @@ -38,23 +38,11 @@ VelocityLimitClearCommand createVelocityLimitClearCommandMsg(const rclcpp::Time return msg; } -// TODO(murooka) make this function common -size_t findExtendedNearestIndex( - const Trajectory traj, const geometry_msgs::msg::Pose & pose, const double max_dist, - const double max_yaw) -{ - const auto nearest_idx = motion_utils::findNearestIndex(traj.points, pose, max_dist, max_yaw); - if (nearest_idx) { - return nearest_idx.get(); - } - return motion_utils::findNearestIndex(traj.points, pose.position); -} - -Trajectory trimTrajectoryFrom(const Trajectory & input, const double nearest_idx) +Trajectory trimTrajectoryFrom(const Trajectory & input, const double start_idx) { Trajectory output{}; - for (size_t i = nearest_idx; i < input.points.size(); ++i) { + for (size_t i = start_idx; i < input.points.size(); ++i) { output.points.push_back(input.points.at(i)); } @@ -192,32 +180,25 @@ Trajectory extendTrajectory( namespace motion_planning { -using visualization_msgs::msg::Marker; -using visualization_msgs::msg::MarkerArray; - ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) : Node("obstacle_cruise_planner", node_options), - self_pose_listener_(this), - in_objects_ptr_(nullptr), vehicle_info_(vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo()) { using std::placeholders::_1; // subscriber - trajectory_sub_ = create_subscription( + traj_sub_ = create_subscription( "~/input/trajectory", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); - smoothed_trajectory_sub_ = create_subscription( - "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onSmoothedTrajectory, this, _1)); objects_sub_ = create_subscription( - "~/input/objects", rclcpp::QoS{1}, std::bind(&ObstacleCruisePlannerNode::onObjects, this, _1)); + "~/input/objects", rclcpp::QoS{1}, + [this](const PredictedObjects::ConstSharedPtr msg) { in_objects_ptr_ = msg; }); odom_sub_ = create_subscription( "~/input/odometry", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onOdometry, this, std::placeholders::_1)); + [this](const Odometry::ConstSharedPtr msg) { current_odom_ptr_ = msg; }); acc_sub_ = create_subscription( "~/input/acceleration", rclcpp::QoS{1}, - std::bind(&ObstacleCruisePlannerNode::onAccel, this, std::placeholders::_1)); + [this](const AccelWithCovarianceStamped::ConstSharedPtr msg) { current_accel_ptr_ = msg; }); // publisher trajectory_pub_ = create_publisher("~/output/trajectory", 1); @@ -225,6 +206,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); clear_vel_limit_pub_ = create_publisher( "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); + + // debug publisher debug_calculation_time_pub_ = create_publisher("~/debug/calculation_time", 1); debug_cruise_wall_marker_pub_ = create_publisher("~/debug/cruise/virtual_wall", 1); debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall", 1); @@ -234,50 +217,9 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & debug_cruise_planning_info_pub_ = create_publisher("~/debug/cruise_planning_info", 1); - // longitudinal_info - const auto longitudinal_info = [&]() { - const double max_accel = declare_parameter("normal.max_acc"); - const double min_accel = declare_parameter("normal.min_acc"); - const double max_jerk = declare_parameter("normal.max_jerk"); - const double min_jerk = declare_parameter("normal.min_jerk"); - const double limit_max_accel = declare_parameter("limit.max_acc"); - const double limit_min_accel = declare_parameter("limit.min_acc"); - const double limit_max_jerk = declare_parameter("limit.max_jerk"); - const double limit_min_jerk = declare_parameter("limit.min_jerk"); - - const double min_ego_accel_for_rss = declare_parameter("common.min_ego_accel_for_rss"); - const double min_object_accel_for_rss = - declare_parameter("common.min_object_accel_for_rss"); - const double idling_time = declare_parameter("common.idling_time"); - const double safe_distance_margin = declare_parameter("common.safe_distance_margin"); - const double terminal_safe_distance_margin = - declare_parameter("common.terminal_safe_distance_margin"); - - lpf_acc_ptr_ = std::make_shared(0.2); - - return LongitudinalInfo{ - max_accel, - min_accel, - max_jerk, - min_jerk, - limit_max_accel, - limit_min_accel, - limit_max_jerk, - limit_min_jerk, - idling_time, - min_ego_accel_for_rss, - min_object_accel_for_rss, - safe_distance_margin, - terminal_safe_distance_margin}; - }(); - - const auto ego_nearest_param = [&]() { - const double ego_nearest_dist_threshold = - declare_parameter("ego_nearest_dist_threshold"); - const double ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); + const auto longitudinal_info = LongitudinalInfo(*this); - return EgoNearestParam(ego_nearest_dist_threshold, ego_nearest_yaw_threshold); - }(); + ego_nearest_param_ = EgoNearestParam(*this); is_showing_debug_info_ = declare_parameter("common.is_showing_debug_info"); disable_stop_planning_ = declare_parameter("common.disable_stop_planning"); @@ -357,26 +299,20 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) { planner_ptr_ = std::make_unique( - *this, longitudinal_info, vehicle_info_, ego_nearest_param); + *this, longitudinal_info, vehicle_info_, ego_nearest_param_); } else { std::logic_error("Designated algorithm is not supported."); } min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); - nearest_dist_deviation_threshold_ = - declare_parameter("common.nearest_dist_deviation_threshold"); - nearest_yaw_deviation_threshold_ = - declare_parameter("common.nearest_yaw_deviation_threshold"); obstacle_velocity_threshold_from_cruise_to_stop_ = declare_parameter("common.obstacle_velocity_threshold_from_cruise_to_stop"); obstacle_velocity_threshold_from_stop_to_cruise_ = declare_parameter("common.obstacle_velocity_threshold_from_stop_to_cruise"); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); } { // cruise obstacle type @@ -432,8 +368,6 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); } } - // wait for first self pose - self_pose_listener_.waitForFirstPose(); // set parameter callback set_param_res_ = this->add_on_set_parameters_callback( @@ -454,14 +388,11 @@ ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlann rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( const std::vector & parameters) { - planner_ptr_->updateCommonParam(parameters); - planner_ptr_->updateParam(parameters); + planner_ptr_->onParam(parameters); tier4_autoware_utils::updateParam( parameters, "common.is_showing_debug_info", is_showing_debug_info_); - planner_ptr_->setParams( - is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_ptr_->setParam(is_showing_debug_info_, min_behavior_stop_margin_); tier4_autoware_utils::updateParam( parameters, "common.disable_stop_planning", disable_stop_planning_); @@ -516,59 +447,34 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( return result; } -void ObstacleCruisePlannerNode::onObjects(const PredictedObjects::ConstSharedPtr msg) -{ - in_objects_ptr_ = msg; -} - -void ObstacleCruisePlannerNode::onOdometry(const Odometry::ConstSharedPtr msg) -{ - current_twist_ptr_ = std::make_unique(); - current_twist_ptr_->header = msg->header; - current_twist_ptr_->twist = msg->twist.twist; -} - -void ObstacleCruisePlannerNode::onAccel(const AccelWithCovarianceStamped::ConstSharedPtr msg) -{ - current_accel_ptr_ = std::make_unique(); - current_accel_ptr_->header = msg->header; - current_accel_ptr_->accel = msg->accel.accel; -} - -void ObstacleCruisePlannerNode::onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg) -{ - planner_ptr_->setSmoothedTrajectory(msg); -} - void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) { - const auto current_pose_ptr = self_pose_listener_.getCurrentPose(); - // check if subscribed variables are ready - if (msg->points.empty() || !current_twist_ptr_ || !in_objects_ptr_ || !current_pose_ptr) { + if (msg->points.empty() || !current_odom_ptr_ || !in_objects_ptr_) { return; } stop_watch_.tic(__func__); + const auto current_pose = current_odom_ptr_->pose.pose; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + // Get Target Obstacles DebugData debug_data; const auto is_driving_forward = motion_utils::isDrivingForwardWithTwist(msg->points); is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_; - const auto target_obstacles = getTargetObstacles( - *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, is_driving_forward_, - debug_data); + const auto target_obstacles = + getTargetObstacles(*msg, current_pose, current_vel, is_driving_forward_, debug_data); // create data for stop - const auto stop_data = - createStopData(*msg, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + const auto stop_data = createStopData(*msg, current_pose, target_obstacles, is_driving_forward_); // stop planning const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data); // create data for cruise const auto cruise_data = - createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles, is_driving_forward_); + createCruiseData(stop_traj, current_pose, target_obstacles, is_driving_forward_); // cruise planning boost::optional vel_limit; @@ -610,8 +516,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -651,8 +557,8 @@ ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( const std::vector & obstacles, const bool is_driving_forward) { const auto current_time = now(); - const double current_vel = current_twist_ptr_->twist.linear.x; - const double current_accel = current_accel_ptr_->accel.linear.x; + const double current_vel = current_odom_ptr_->twist.twist.linear.x; + const double current_accel = current_accel_ptr_->accel.accel.linear.x; // create planner_stop data ObstacleCruisePlannerData planner_data; @@ -697,8 +603,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto current_time = now(); const auto time_stamp = rclcpp::Time(predicted_objects.header.stamp); - const size_t ego_idx = findExtendedNearestIndex( - traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + const size_t ego_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); // calculate decimated trajectory const auto trimmed_traj = trimTrajectoryFrom(traj, ego_idx); @@ -718,7 +624,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( std::vector target_obstacles; for (const auto & predicted_object : predicted_objects.objects) { - const auto object_id = toHexString(predicted_object.object_id).substr(0, 4); + const auto object_id = + tier4_autoware_utils::toHexString(predicted_object.object_id).substr(0, 4); // filter object whose label is not cruised or stopped const bool is_target_obstacle = isStopObstacle(predicted_object.classification.front().label) || @@ -943,7 +850,7 @@ void ObstacleCruisePlannerNode::checkConsistency( const auto predicted_object_itr = std::find_if( predicted_objects.objects.begin(), predicted_objects.objects.end(), [&](PredictedObject predicted_object) { - return obstacle_cruise_utils::toHexString(predicted_object.object_id) == + return tier4_autoware_utils::toHexString(predicted_object.object_id) == prev_closest_obstacle_ptr_->uuid; }); @@ -1025,23 +932,28 @@ void ObstacleCruisePlannerNode::publishVelocityLimit( const boost::optional & vel_limit) { if (vel_limit) { + // publish velocity limit vel_limit_pub_->publish(vel_limit.get()); need_to_clear_vel_limit_ = true; - } else { - if (need_to_clear_vel_limit_) { - const auto clear_vel_limit_msg = createVelocityLimitClearCommandMsg(now()); - clear_vel_limit_pub_->publish(clear_vel_limit_msg); - need_to_clear_vel_limit_ = false; - } + return; } + + if (!need_to_clear_vel_limit_) { + return; + } + + // clear velocity limit + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now()); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_ = false; } void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) const { stop_watch_.tic(__func__); + // 1. publish debug marker MarkerArray debug_marker; - const auto current_time = now(); // obstacles to cruise for (size_t i = 0; i < debug_data.obstacles_to_cruise.size(); ++i) { @@ -1067,7 +979,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) { // footprint polygons auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "detection_polygons", 0, Marker::LINE_LIST, + "map", now(), "detection_polygons", 0, Marker::LINE_LIST, tier4_autoware_utils::createMarkerScale(0.01, 0.0, 0.0), tier4_autoware_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); @@ -1089,7 +1001,7 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) { // collision points for (size_t i = 0; i < debug_data.collision_points.size(); ++i) { auto marker = tier4_autoware_utils::createDefaultMarker( - "map", current_time, "collision_points", i, Marker::SPHERE, + "map", now(), "collision_points", i, Marker::SPHERE, tier4_autoware_utils::createMarkerScale(0.25, 0.25, 0.25), tier4_autoware_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); marker.pose.position = debug_data.collision_points.at(i); @@ -1099,11 +1011,11 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) debug_marker_pub_->publish(debug_marker); - // virtual wall for cruise and stop + // 2. publish virtual wall for cruise and stop debug_cruise_wall_marker_pub_->publish(debug_data.cruise_wall_marker); debug_stop_wall_marker_pub_->publish(debug_data.stop_wall_marker); - // print calculation time + // 3. print calculation time const double calculation_time = stop_watch_.toc(__func__); RCLCPP_INFO_EXPRESSION( rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", @@ -1112,14 +1024,12 @@ void ObstacleCruisePlannerNode::publishDebugMarker(const DebugData & debug_data) void ObstacleCruisePlannerNode::publishDebugInfo() const { - const auto current_time = now(); - // stop - const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(current_time); + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); debug_stop_planning_info_pub_->publish(stop_debug_msg); // cruise - const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(current_time); + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); debug_cruise_planning_info_pub_->publish(cruise_debug_msg); } diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index 8948cd2de8eb3..262e2b619d36e 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -39,6 +39,10 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( const vehicle_info_util::VehicleInfo & vehicle_info, const EgoNearestParam & ego_nearest_param) : PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param) { + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + // parameter dense_resampling_time_interval_ = node.declare_parameter("optimization_based_planner.dense_resampling_time_interval"); @@ -85,8 +89,7 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); boundary_pub_ = node.create_publisher("~/boundary", 1); - debug_wall_marker_pub_ = - node.create_publisher("~/debug/wall_marker", 1); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); } Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( @@ -284,8 +287,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const auto & input_traj = planner_data.traj; const double vehicle_speed{std::abs(current_vel)}; const auto current_closest_point = motion_utils::calcInterpolatedPoint( - input_traj, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; double initial_vel{}; @@ -301,11 +304,11 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - *smoothed_trajectory_ptr_, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + *smoothed_trajectory_ptr_, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); } else { prev_output_closest_point = motion_utils::calcInterpolatedPoint( - prev_traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + prev_traj, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); } // when velocity tracking deviation is large @@ -328,8 +331,8 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { const auto stop_dist = motion_utils::calcDistanceToForwardStopPoint( - input_traj.points, current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + input_traj.points, current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { initial_vel = engage_velocity_; initial_acc = engage_acceleration_; @@ -363,8 +366,8 @@ bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerDa { // If goal is close and current velocity is low, we don't optimize trajectory const auto closest_stop_dist = motion_utils::calcDistanceToForwardStopPoint( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) { return true; } @@ -437,7 +440,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( planner_data.traj.points, planner_data.current_pose.position, min_slow_down_point_length); if (marker_pose) { - visualization_msgs::msg::MarkerArray wall_msg; + MarkerArray wall_msg; if (obj.has_stopped) { const auto markers = motion_utils::createStopVirtualWallMarker( @@ -590,19 +593,18 @@ bool OptimizationBasedPlanner::checkOnTrajectory( } boost::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Pose & current_pose) + const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) { const auto traj_length = motion_utils::calcSignedArcLength( - traj.points, current_pose, traj.points.size() - 1, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + traj.points, current_pose, traj.points.size() - 1, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!traj_length) { return {}; } const auto dist_to_closest_stop_point = motion_utils::calcDistanceToForwardStopPoint( - traj.points, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + traj.points, current_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_closest_stop_point) { return std::min(traj_length.get(), dist_to_closest_stop_point.get()); } diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index cfd9dd24c543b..8925759ba7e47 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -82,6 +82,9 @@ PIDBasedPlanner::PIDBasedPlanner( velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = node.declare_parameter( "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); } { // velocity insertion based planner @@ -139,6 +142,7 @@ PIDBasedPlanner::PIDBasedPlanner( lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); lpf_obstacle_vel_ptr_ = std::make_shared(0.5); lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); lpf_output_acc_ptr_ = std::make_shared(0.5); lpf_output_jerk_ptr_ = std::make_shared(0.5); } @@ -300,10 +304,12 @@ Trajectory PIDBasedPlanner::planCruise( : std::abs(vehicle_info_.min_longitudinal_offset_m); const double dist_to_rss_wall = std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, dist_to_rss_wall, ego_idx); const auto markers = motion_utils::createSlowDownVirtualWallMarker( - planner_data.traj.points.at(ego_idx).pose, "obstacle cruise", planner_data.current_time, 0, - dist_to_rss_wall); + planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, + 0); tier4_autoware_utils::appendMarkerArray(markers, &debug_data.cruise_wall_marker); // cruise obstalce @@ -324,6 +330,7 @@ Trajectory PIDBasedPlanner::planCruise( prev_target_acc_ = {}; lpf_normalized_error_cruise_dist_ptr_->reset(); lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); lpf_output_jerk_ptr_->reset(); // delete marker @@ -356,11 +363,15 @@ VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( return pid_output_vel; }(); - const double positive_target_vel = - std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel); + const double positive_target_vel = lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.current_vel + additional_vel)); // calculate target acceleration const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + double target_acc = p.vel_to_acc_weight * additional_vel; // apply acc limit @@ -450,11 +461,6 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory( return target_jerk_ratio; }(); - /* - const double target_acc = vel_to_acc_weight_ * additional_vel; - const double target_acc_with_acc_limit = - std::clamp(target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); - */ cruise_planning_debug_info_.set( CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); cruise_planning_debug_info_.set( @@ -462,14 +468,14 @@ Trajectory PIDBasedPlanner::doCruiseWithTrajectory( // set target longitudinal motion const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { - if (smoothed_trajectory_ptr_) { - return motion_utils::calcInterpolatedPoint( - *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); - } + // if (smoothed_trajectory_ptr_) { + // return motion_utils::calcInterpolatedPoint( + // *smoothed_trajectory_ptr_, planner_data.current_pose, nearest_dist_deviation_threshold_, + // nearest_yaw_deviation_threshold_); + // } return motion_utils::calcInterpolatedPoint( - prev_traj_, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + prev_traj_, planner_data.current_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); }(); const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; const double a0 = prev_traj_closest_point.acceleration_mps2; @@ -580,9 +586,7 @@ Trajectory PIDBasedPlanner::getAccelerationLimitedTrajectory( } auto acc_limited_traj = traj; - const size_t ego_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - acc_limited_traj.points, start_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + const size_t ego_seg_idx = findEgoIndex(acc_limited_traj, start_pose); double sum_dist = 0.0; for (size_t i = ego_seg_idx; i < acc_limited_traj.points.size(); ++i) { if (i != ego_seg_idx) { @@ -636,6 +640,10 @@ void PIDBasedPlanner::updateParam(const std::vector & paramet tier4_autoware_utils::updateParam( parameters, "pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", p.enable_jerk_limit_to_output_acc); + + tier4_autoware_utils::updateParam( + parameters, "pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", + p.disable_target_acceleration); } { // velocity insertion based planner diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp index adee76f67c065..420b72d2bf0aa 100644 --- a/planning/obstacle_cruise_planner/src/planner_interface.cpp +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -35,26 +35,25 @@ tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( header.stamp = current_time; // create stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; + StopFactor stop_factor; stop_factor.stop_pose = stop_pose; geometry_msgs::msg::Point stop_factor_point = stop_obstacle.collision_points.front().point; stop_factor_point.z = stop_pose.position.z; stop_factor.stop_factor_points.emplace_back(stop_factor_point); // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; stop_reason_msg.stop_factors.emplace_back(stop_factor); // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; } -tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( - const rclcpp::Time & current_time) +StopReasonArray makeEmptyStopReasonArray(const rclcpp::Time & current_time) { // create header std_msgs::msg::Header header; @@ -62,11 +61,11 @@ tier4_planning_msgs::msg::StopReasonArray makeEmptyStopReasonArray( header.stamp = current_time; // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + StopReason stop_reason_msg; + stop_reason_msg.reason = StopReason::OBSTACLE_STOP; // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + StopReasonArray stop_reason_array; stop_reason_array.header = header; stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); return stop_reason_array; @@ -145,8 +144,8 @@ Trajectory PlannerInterface::generateStopTrajectory( planner_data.traj.points, 0, closest_stop_obstacle->collision_points.front().point); const auto negative_dist_to_ego = motion_utils::calcSignedArcLength( - planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + planner_data.traj.points, planner_data.current_pose, 0, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); if (!negative_dist_to_ego) { // delete marker const auto markers = @@ -161,9 +160,7 @@ Trajectory PlannerInterface::generateStopTrajectory( // we set closest_obstacle_stop_distance to closest_behavior_stop_distance const double margin_from_obstacle = [&]() { const size_t nearest_segment_idx = - motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_); + findEgoSegmentIndex(planner_data.traj, planner_data.current_pose); const auto closest_behavior_stop_idx = motion_utils::searchZeroVelocityIndex(planner_data.traj.points, nearest_segment_idx + 1); @@ -242,8 +239,9 @@ Trajectory PlannerInterface::generateStopTrajectory( stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); } - // stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, // - // TODO(murooka) + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + closest_obstacle_dist - abs_ego_offset); // TODO(murooka) stop_planning_debug_info_.set( StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, closest_stop_obstacle->velocity); @@ -264,7 +262,7 @@ double PlannerInterface::calcDistanceToCollisionPoint( const auto dist_to_collision_point = motion_utils::calcSignedArcLength( planner_data.traj.points, planner_data.current_pose, collision_point, - nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); + ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); if (dist_to_collision_point) { return dist_to_collision_point.get() - offset; diff --git a/planning/obstacle_cruise_planner/src/polygon_utils.cpp b/planning/obstacle_cruise_planner/src/polygon_utils.cpp index d0b635951e074..28fa21646062a 100644 --- a/planning/obstacle_cruise_planner/src/polygon_utils.cpp +++ b/planning/obstacle_cruise_planner/src/polygon_utils.cpp @@ -88,9 +88,8 @@ Polygon2d createOneStepPolygon( namespace polygon_utils { boost::optional getCollisionIndex( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const geometry_msgs::msg::PoseStamped & obj_pose, - const autoware_auto_perception_msgs::msg::Shape & shape, + const Trajectory & traj, const std::vector & traj_polygons, + const geometry_msgs::msg::PoseStamped & obj_pose, const Shape & shape, std::vector & collision_geom_points, const double max_dist) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose.pose, shape); @@ -128,10 +127,9 @@ boost::optional getCollisionIndex( } std::vector getCollisionPoints( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double vehicle_max_longitudinal_offset, const bool is_driving_forward, std::vector & collision_index, const double max_dist, const double max_prediction_time_for_collision_check) @@ -172,11 +170,10 @@ std::vector getCollisionPoints( } std::vector willCollideWithSurroundObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & traj_polygons, const std_msgs::msg::Header & obj_header, - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const autoware_auto_perception_msgs::msg::Shape & shape, const rclcpp::Time & current_time, - const double max_dist, const double ego_obstacle_overlap_time_threshold, + const Trajectory & traj, const std::vector & traj_polygons, + const std_msgs::msg::Header & obj_header, const PredictedPath & predicted_path, + const Shape & shape, const rclcpp::Time & current_time, const double max_dist, + const double ego_obstacle_overlap_time_threshold, const double max_prediction_time_for_collision_check, std::vector & collision_index, const double vehicle_max_longitudinal_offset, const bool is_driving_forward) { @@ -200,8 +197,8 @@ std::vector willCollideWithSurroundObstacle( } std::vector createOneStepPolygons( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const vehicle_info_util::VehicleInfo & vehicle_info, const double expand_width) + const Trajectory & traj, const vehicle_info_util::VehicleInfo & vehicle_info, + const double expand_width) { std::vector polygons; @@ -223,8 +220,8 @@ std::vector createOneStepPolygons( geometry_msgs::msg::PointStamped calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, - const autoware_auto_planning_msgs::msg::Trajectory & decimated_traj, - const double vehicle_max_longitudinal_offset, const bool is_driving_forward) + const Trajectory & decimated_traj, const double vehicle_max_longitudinal_offset, + const bool is_driving_forward) { std::vector segment_points(2); if (first_within_idx == 0) { diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 8aecbf393bcc0..45cc0a2e6c89d 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -18,12 +18,6 @@ namespace obstacle_cruise_utils { -bool isVehicle(const uint8_t label) -{ - return label == ObjectClassification::CAR || label == ObjectClassification::TRUCK || - label == ObjectClassification::BUS || label == ObjectClassification::MOTORCYCLE; -} - visualization_msgs::msg::Marker getObjectMarker( const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns, const double r, const double g, const double b) @@ -41,8 +35,7 @@ visualization_msgs::msg::Marker getObjectMarker( } boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, - const double target_length) + const Trajectory & traj, const size_t start_idx, const double target_length) { if (traj.points.empty()) { return {}; @@ -77,8 +70,8 @@ boost::optional calcForwardPose( } boost::optional getCurrentObjectPoseFromPredictedPath( - const autoware_auto_perception_msgs::msg::PredictedPath & predicted_path, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { const double rel_time = (current_time - obj_base_time).seconds(); if (rel_time < 0.0) { @@ -89,8 +82,8 @@ boost::optional getCurrentObjectPoseFromPredictedPath( } boost::optional getCurrentObjectPoseFromPredictedPaths( - const std::vector & predicted_paths, - const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time) + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) { if (predicted_paths.empty()) { return boost::none; @@ -98,19 +91,14 @@ boost::optional getCurrentObjectPoseFromPredictedPaths // Get the most reliable path const auto predicted_path = std::max_element( predicted_paths.begin(), predicted_paths.end(), - []( - const autoware_auto_perception_msgs::msg::PredictedPath & a, - const autoware_auto_perception_msgs::msg::PredictedPath & b) { - return a.confidence < b.confidence; - }); + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); } geometry_msgs::msg::PoseStamped getCurrentObjectPose( - const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, - const std_msgs::msg::Header & obj_header, const rclcpp::Time & current_time, - const bool use_prediction) + const PredictedObject & predicted_object, const std_msgs::msg::Header & obj_header, + const rclcpp::Time & current_time, const bool use_prediction) { if (!use_prediction) { geometry_msgs::msg::PoseStamped current_pose; @@ -119,7 +107,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( return current_pose; } - std::vector predicted_paths; + std::vector predicted_paths; for (const auto & path : predicted_object.kinematics.predicted_paths) { predicted_paths.push_back(path); } @@ -143,8 +131,7 @@ geometry_msgs::msg::PoseStamped getCurrentObjectPose( } boost::optional getClosestStopObstacle( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const std::vector & target_obstacles) + const Trajectory & traj, const std::vector & target_obstacles) { if (target_obstacles.empty()) { return boost::none; @@ -167,13 +154,4 @@ boost::optional getClosestStopObstacle( } return closest_stop_obstacle; } - -std::string toHexString(const unique_identifier_msgs::msg::UUID & id) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +id.uuid[i]; - } - return ss.str(); -} } // namespace obstacle_cruise_utils From f33905a118afe1b732240d69974955745fa7f19b Mon Sep 17 00:00:00 2001 From: Kenzo Lobos Tsunekawa Date: Wed, 21 Dec 2022 13:51:39 +0900 Subject: [PATCH 21/37] chore(lidar_centerpoint): update maintainer (#2549) * feat(lidar_centerpoint): update codeowners Signed-off-by: Kenzo Lobos-Tsunekawa * Moved the inclusion to the corresponding package.xml Signed-off-by: Kenzo Lobos-Tsunekawa Signed-off-by: Kenzo Lobos-Tsunekawa --- perception/lidar_centerpoint/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 14cfaa7a72e3b..602b81ae5b946 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -5,6 +5,7 @@ 1.0.0 The lidar_centerpoint package Yusuke Muramatsu + Kenzo Lobos-Tsunekawa Apache License 2.0 ament_cmake_auto From 3efb2ce6fca5fadb471af418041f0f2ab25903c6 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Wed, 21 Dec 2022 16:26:35 +0900 Subject: [PATCH 22/37] fix(trajectory_follower_node): remove unused include line (#2550) Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../include/trajectory_follower_nodes/controller_node.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index f8493c7a1cb28..67f901ec1f8f5 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -33,7 +33,6 @@ #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" #include "autoware_auto_control_msgs/msg/longitudinal_command.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_system_msgs/msg/float32_multi_array_diagnostic.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_odometry.hpp" #include "geometry_msgs/msg/accel_stamped.hpp" #include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" From 0b0b2952254ea46dffb10b059a0764a8fdd9f0f6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Wed, 21 Dec 2022 16:37:43 +0900 Subject: [PATCH 23/37] fix(component_state_monitor): fix lanelet route package (#2552) Signed-off-by: Takagi, Isamu Signed-off-by: Takagi, Isamu --- system/component_state_monitor/config/topics.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 9f56c8bffec96..5a5f2e880bcae 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -82,7 +82,7 @@ args: node_name_suffix: mission_planning_route topic: /planning/mission_planning/route - topic_type: autoware_auto_planning_msgs/msg/LaneletRoute + topic_type: autoware_planning_msgs/msg/LaneletRoute best_effort: false transient_local: true warn_rate: 0.0 From 35fee809ebd861f8c2a86da74070f027cf96b7b0 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 21 Dec 2022 17:02:07 +0900 Subject: [PATCH 24/37] feat(tier4_system_launch): remove configs and move to autoware_launch (#2540) * feat(tier4_system_launch): remove configs and move to autoware_launch Signed-off-by: kminoda * update readme Signed-off-by: kminoda * fix readme * remove config Signed-off-by: kminoda * minor fix Signed-off-by: kminoda * fix readme Signed-off-by: kminoda * fix mistake Signed-off-by: kminoda * fix typo Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_system_launch/CMakeLists.txt | 1 - launch/tier4_system_launch/README.md | 7 + .../component_state_monitor/topics.yaml | 182 ------------------ .../emergency_handler.param.yaml | 14 -- .../mrm_comfortable_stop_operator.param.yaml | 6 - .../mrm_emergency_stop_operator.param.yaml | 5 - .../diagnostic_aggregator/vehicle.param.yaml | 11 -- .../system_error_monitor.param.yaml | 52 ----- ...ror_monitor.planning_simulation.param.yaml | 52 ----- .../system_monitor/cpu_monitor.param.yaml | 8 - .../system_monitor/gpu_monitor.param.yaml | 8 - .../system_monitor/hdd_monitor.param.yaml | 23 --- .../system_monitor/mem_monitor.param.yaml | 3 - .../system_monitor/net_monitor.param.yaml | 9 - .../system_monitor/ntp_monitor.param.yaml | 5 - .../system_monitor/process_monitor.param.yaml | 3 - .../system_monitor/voltage_monitor.param.yaml | 5 - .../launch/system.launch.xml | 54 +++--- 18 files changed, 39 insertions(+), 409 deletions(-) delete mode 100644 launch/tier4_system_launch/config/component_state_monitor/topics.yaml delete mode 100644 launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml delete mode 100644 launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml delete mode 100644 launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml delete mode 100644 launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml diff --git a/launch/tier4_system_launch/CMakeLists.txt b/launch/tier4_system_launch/CMakeLists.txt index 89170940dbab1..cde16af48f523 100644 --- a/launch/tier4_system_launch/CMakeLists.txt +++ b/launch/tier4_system_launch/CMakeLists.txt @@ -6,5 +6,4 @@ autoware_package() ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_system_launch/README.md b/launch/tier4_system_launch/README.md index 252701df8eaa3..76cea5a575c5d 100644 --- a/launch/tier4_system_launch/README.md +++ b/launch/tier4_system_launch/README.md @@ -10,10 +10,17 @@ Please see `` in `package.xml`. ## Usage +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `system.launch.xml`. + ```xml + + + + + ... ``` diff --git a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml b/launch/tier4_system_launch/config/component_state_monitor/topics.yaml deleted file mode 100644 index 5a5f2e880bcae..0000000000000 --- a/launch/tier4_system_launch/config/component_state_monitor/topics.yaml +++ /dev/null @@ -1,182 +0,0 @@ -- module: map - mode: [online, planning_simulation] - type: launch - args: - node_name_suffix: vector_map - topic: /map/vector_map - topic_type: autoware_auto_mapping_msgs/msg/HADMapBin - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: map - mode: [online] - type: launch - args: - node_name_suffix: pointcloud_map - topic: /map/pointcloud_map - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: initialpose3d - topic: /initialpose3d - topic_type: geometry_msgs/msg/PoseWithCovarianceStamped - best_effort: false - transient_local: false - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: localization - mode: [online] - type: autonomous - args: - node_name_suffix: pose_twist_fusion_filter_pose - topic: /localization/pose_twist_fusion_filter/pose - topic_type: geometry_msgs/msg/PoseStamped - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: perception - mode: [online] - type: launch - args: - node_name_suffix: obstacle_segmentation_pointcloud - topic: /perception/obstacle_segmentation/pointcloud - topic_type: sensor_msgs/msg/PointCloud2 - best_effort: true - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: perception - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: object_recognition_objects - topic: /perception/object_recognition/objects - topic_type: autoware_auto_perception_msgs/msg/PredictedObjects - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: planning - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: mission_planning_route - topic: /planning/mission_planning/route - topic_type: autoware_planning_msgs/msg/LaneletRoute - best_effort: false - transient_local: true - warn_rate: 0.0 - error_rate: 0.0 - timeout: 0.0 - -- module: planning - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: scenario_planning_trajectory - topic: /planning/scenario_planning/trajectory - topic_type: autoware_auto_planning_msgs/msg/Trajectory - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: trajectory_follower_control_cmd - topic: /control/trajectory_follower/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: control - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: control_command_control_cmd - topic: /control/command/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: vehicle_status_velocity_status - topic: /vehicle/status/velocity_status - topic_type: autoware_auto_vehicle_msgs/msg/VelocityReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: vehicle - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: vehicle_status_steering_status - topic: /vehicle/status/steering_status - topic_type: autoware_auto_vehicle_msgs/msg/SteeringReport - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: system - mode: [online, planning_simulation] - type: launch - args: - node_name_suffix: system_emergency_control_cmd - topic: /system/emergency/control_cmd - topic_type: autoware_auto_control_msgs/msg/AckermannControlCommand - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 - -- module: localization - mode: [online, planning_simulation] - type: autonomous - args: - node_name_suffix: transform_map_to_base_link - topic: /tf - frame_id: map - child_frame_id: base_link - best_effort: false - transient_local: false - warn_rate: 5.0 - error_rate: 1.0 - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml b/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml deleted file mode 100644 index 652a984ce539a..0000000000000 --- a/launch/tier4_system_launch/config/emergency_handler/emergency_handler.param.yaml +++ /dev/null @@ -1,14 +0,0 @@ -# Default configuration for emergency handler ---- -/**: - ros__parameters: - update_rate: 10 - timeout_hazard_status: 0.5 - timeout_takeover_request: 10.0 - use_takeover_request: false - use_parking_after_stopped: false - use_comfortable_stop: false - - # setting whether to turn hazard lamp on for each situation - turning_hazard_on: - emergency: true diff --git a/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml b/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml deleted file mode 100644 index 81bc9b9c0b5d0..0000000000000 --- a/launch/tier4_system_launch/config/mrm_comfortable_stop_operator/mrm_comfortable_stop_operator.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - update_rate: 10 - min_acceleration: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 diff --git a/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml b/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml deleted file mode 100644 index 1ee2699a23a82..0000000000000 --- a/launch/tier4_system_launch/config/mrm_emergency_stop_operator/mrm_emergency_stop_operator.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - update_rate: 30 - target_acceleration: -2.5 - target_jerk: -1.5 diff --git a/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml deleted file mode 100644 index e96e3b3b05934..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/diagnostic_aggregator/vehicle.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - vehicle: - type: diagnostic_aggregator/AnalyzerGroup - path: vehicle - analyzers: - vehicle_errors: - type: diagnostic_aggregator/GenericAnalyzer - path: vehicle_errors - contains: [": vehicle_errors"] - timeout: 1.0 diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml deleted file mode 100644 index 71dc2ac600685..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.param.yaml +++ /dev/null @@ -1,52 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - - /autoware/localization/node_alive_monitoring: default - /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/localization/performance_monitoring/localization_accuracy: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml b/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml deleted file mode 100644 index 9708456df4fe7..0000000000000 --- a/launch/tier4_system_launch/config/system_error_monitor/system_error_monitor.planning_simulation.param.yaml +++ /dev/null @@ -1,52 +0,0 @@ -# Description: -# name: diag name -# sf_at: diag level where it becomes Safe Fault -# lf_at: diag level where it becomes Latent Fault -# spf_at: diag level where it becomes Single Point Fault -# auto_recovery: Determines whether the system will automatically recover when it recovers from an error. -# -# Note: -# empty-value for sf_at, lf_at and spf_at is "none" -# default values are: -# sf_at: "none" -# lf_at: "warn" -# spf_at: "error" -# auto_recovery: "true" ---- -/**: - ros__parameters: - required_modules: - autonomous_driving: - /autoware/control/autonomous_driving/node_alive_monitoring: default - /autoware/control/autonomous_driving/performance_monitoring/lane_departure: default - /autoware/control/control_command_gate/node_alive_monitoring: default - - /autoware/localization/node_alive_monitoring: default - # /autoware/localization/performance_monitoring/matching_score: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/localization/performance_monitoring/localization_accuracy: default - - /autoware/map/node_alive_monitoring: default - - /autoware/perception/node_alive_monitoring: default - - /autoware/planning/node_alive_monitoring: default - /autoware/planning/performance_monitoring/trajectory_validation: default - - # /autoware/sensing/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - # /autoware/system/resource_monitoring: { sf_at: "warn", lf_at: "error", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default - - external_control: - /autoware/control/control_command_gate/node_alive_monitoring: default - /autoware/control/external_control/external_command_selector/node_alive_monitoring: default - - /autoware/system/node_alive_monitoring: default - /autoware/system/emergency_stop_operation: default - /autoware/system/service_log_checker: { sf_at: "warn", lf_at: "none", spf_at: "none" } - - /autoware/vehicle/node_alive_monitoring: default diff --git a/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml deleted file mode 100644 index da4d74e53d475..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/cpu_monitor.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - usage_warn: 0.96 - usage_error: 0.96 - usage_warn_count: 1 - usage_error_count: 2 - usage_avg: true - msr_reader_port: 7634 diff --git a/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml deleted file mode 100644 index d96b9f24640b2..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/gpu_monitor.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - temp_warn: 90.0 - temp_error: 95.0 - gpu_usage_warn: 0.90 - gpu_usage_error: 1.00 - memory_usage_warn: 0.95 - memory_usage_error: 0.99 diff --git a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml deleted file mode 100644 index d818d848be801..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/hdd_monitor.param.yaml +++ /dev/null @@ -1,23 +0,0 @@ -/**: - ros__parameters: - hdd_reader_port: 7635 - num_disks: 1 - disks: # Until multi type lists are allowed, name N the disks as disk0...disk{N-1} - disk0: - name: / - temp_attribute_id: 0xC2 - temp_warn: 55.0 - temp_error: 70.0 - power_on_hours_attribute_id: 0x09 - power_on_hours_warn: 3000000 - total_data_written_attribute_id: 0xF1 - total_data_written_warn: 4915200 # =150TB (1unit=32MB) - total_data_written_safety_factor: 0.05 - recovered_error_attribute_id: 0xC3 - recovered_error_warn: 1 - free_warn: 5120 # MB(8hour) - free_error: 100 # MB(last 1 minute) - read_data_rate_warn: 360.0 # MB/s - write_data_rate_warn: 103.5 # MB/s - read_iops_warn: 63360.0 # IOPS - write_iops_warn: 24120.0 # IOPS diff --git a/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml deleted file mode 100644 index f2da3972be91a..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/mem_monitor.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - available_size: 1024 # MB diff --git a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml deleted file mode 100644 index d72b8d1334c17..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/net_monitor.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - devices: ["*"] - traffic_reader_port: 7636 - monitor_program: "greengrass" - crc_error_check_duration: 1 - crc_error_count_threshold: 1 - reassembles_failed_check_duration: 1 - reassembles_failed_check_count: 1 diff --git a/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml deleted file mode 100644 index db54f70d1ce59..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/ntp_monitor.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - server: ntp.nict.jp - offset_warn: 0.1 - offset_error: 5.0 diff --git a/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml deleted file mode 100644 index 3d6d82fae5ce2..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/process_monitor.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - num_of_procs: 5 diff --git a/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml b/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml deleted file mode 100644 index e55410be39250..0000000000000 --- a/launch/tier4_system_launch/config/system_monitor/voltage_monitor.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - cmos_battery_warn: 2.90 - cmos_battery_error: 2.70 - cmos_battery_label: "" diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 2400b8b449a6a..ae64cac3ad2f4 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -1,9 +1,25 @@ + + + + + + + + + + + + + + + + + - @@ -14,14 +30,14 @@ - - - - - - - - + + + + + + + + @@ -34,22 +50,18 @@ - + - - + + - + @@ -57,21 +69,19 @@ - + - - + - - + From 429f1ed85ab080cdbe4f5e49fd9b9a66af255d6e Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 21 Dec 2022 17:02:29 +0900 Subject: [PATCH 25/37] feat(tier4_planning_launch): remove configs and move to autoware_launch (#2543) * feat(tier4_planning_launch): remove configs and move to autoware_launch Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * remove config Signed-off-by: Takayuki Murooka * add rtc Signed-off-by: Takayuki Murooka * Update launch/tier4_planning_launch/README.md Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Signed-off-by: Takayuki Murooka Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> --- launch/tier4_planning_launch/CMakeLists.txt | 1 - launch/tier4_planning_launch/README.md | 6 + .../common/common.param.yaml | 15 - .../Analytical.param.yaml | 25 -- .../JerkFiltered.param.yaml | 7 - .../motion_velocity_smoother/L2.param.yaml | 5 - .../motion_velocity_smoother/Linf.param.yaml | 5 - .../motion_velocity_smoother.param.yaml | 58 ---- .../common/nearest_search.param.yaml | 5 - .../avoidance/avoidance.param.yaml | 77 ----- .../behavior_path_planner.param.yaml | 17 -- .../drivable_area_expansion.param.yaml | 20 -- .../lane_change/lane_change.param.yaml | 19 -- .../lane_following/lane_following.param.yaml | 4 - .../pull_out/pull_out.param.yaml | 30 -- .../pull_over/pull_over.param.yaml | 61 ---- .../side_shift/side_shift.param.yaml | 8 - .../behavior_velocity_planner.param.yaml | 19 -- .../blind_spot.param.yaml | 8 - .../crosswalk.param.yaml | 44 --- .../detection_area.param.yaml | 8 - .../no_stopping_area.param.yaml | 10 - .../occlusion_spot.param.yaml | 34 --- .../run_out.param.yaml | 45 --- .../stop_line.param.yaml | 6 - .../traffic_light.param.yaml | 8 - .../virtual_traffic_light.param.yaml | 8 - .../lane_change_planner.param.yaml | 22 -- .../rtc_auto_mode_manager.param.yaml | 29 -- .../obstacle_avoidance_planner.param.yaml | 159 ---------- .../obstacle_cruise_planner.param.yaml | 138 --------- .../adaptive_cruise_control.param.yaml | 38 --- .../obstacle_stop_planner.param.yaml | 49 ---- .../obstacle_velocity_limiter.param.yaml | 33 --- .../surround_obstacle_checker.param.yaml | 15 - .../freespace_planner.param.yaml | 39 --- .../launch/planning.launch.xml | 57 +++- .../scenario_planning/lane_driving.launch.xml | 19 +- .../behavior_planning.launch.py | 275 +++--------------- .../behavior_planning.launch.xml | 6 +- .../motion_planning/motion_planning.launch.py | 92 ++---- .../motion_planning.launch.xml | 8 +- .../scenario_planning/parking.launch.py | 15 +- .../scenario_planning/parking.launch.xml | 6 +- .../scenario_planning.launch.xml | 23 +- 45 files changed, 129 insertions(+), 1447 deletions(-) delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml delete mode 100644 launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml diff --git a/launch/tier4_planning_launch/CMakeLists.txt b/launch/tier4_planning_launch/CMakeLists.txt index 306b557745756..040cc5a1ad0a6 100644 --- a/launch/tier4_planning_launch/CMakeLists.txt +++ b/launch/tier4_planning_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_planning_launch/README.md b/launch/tier4_planning_launch/README.md index 2b5790536827c..392e324d71eb6 100644 --- a/launch/tier4_planning_launch/README.md +++ b/launch/tier4_planning_launch/README.md @@ -10,7 +10,13 @@ Please see `` in `package.xml`. ## Usage +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`. + ```xml + + + + ... ``` diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml deleted file mode 100644 index a23570a5fc791..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/common.param.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/**: - ros__parameters: - # constraints param for normal driving - normal: - min_acc: -0.5 # min deceleration [m/ss] - max_acc: 1.0 # max acceleration [m/ss] - min_jerk: -0.5 # min jerk [m/sss] - max_jerk: 1.0 # max jerk [m/sss] - - # constraints to be observed - limit: - min_acc: -2.5 # min deceleration limit [m/ss] - max_acc: 1.0 # max acceleration limit [m/ss] - min_jerk: -1.5 # min jerk limit [m/sss] - max_jerk: 1.5 # max jerk limit [m/sss] diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml deleted file mode 100644 index 329714e3d371e..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Analytical.param.yaml +++ /dev/null @@ -1,25 +0,0 @@ -/**: - ros__parameters: - resample: - ds_resample: 0.1 - num_resample: 1 - delta_yaw_threshold: 0.785 - - latacc: - enable_constant_velocity_while_turning: false - constant_velocity_dist_threshold: 2.0 - - forward: - max_acc: 1.0 - min_acc: -1.0 - max_jerk: 0.3 - min_jerk: -0.3 - kp: 0.3 - - backward: - start_jerk: -0.1 - min_jerk_mild_stop: -0.3 - min_jerk: -1.5 - min_acc_mild_stop: -1.0 - min_acc: -2.5 - span_jerk: -0.01 diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml deleted file mode 100644 index a6906b7117fd5..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/JerkFiltered.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - jerk_weight: 0.1 # weight for "smoothness" cost for jerk - over_v_weight: 10000.0 # weight for "over speed limit" cost - over_a_weight: 500.0 # weight for "over accel limit" cost - over_j_weight: 200.0 # weight for "over jerk limit" cost - jerk_filter_ds: 0.1 # resampling ds for jerk filter diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml deleted file mode 100644 index c993978204f10..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/L2.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - pseudo_jerk_weight: 100.0 # weight for "smoothness" cost - over_v_weight: 100000.0 # weight for "over speed limit" cost - over_a_weight: 1000.0 # weight for "over accel limit" cost diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml deleted file mode 100644 index ec38010621cc9..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/Linf.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - pseudo_jerk_weight: 200.0 # weight for "smoothness" cost - over_v_weight: 100000.0 # weight for "over speed limit" cost - over_a_weight: 5000.0 # weight for "over accel limit" cost diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml deleted file mode 100644 index 64b2d95c6fd01..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/motion_velocity_smoother/motion_velocity_smoother.param.yaml +++ /dev/null @@ -1,58 +0,0 @@ -/**: - ros__parameters: - # motion state constraints - max_velocity: 20.0 # max velocity limit [m/s] - stop_decel: 0.0 # deceleration at a stop point[m/ss] - - # external velocity limit parameter - margin_to_insert_external_velocity_limit: 0.3 # margin distance to insert external velocity limit [m] - - # curve parameters - max_lateral_accel: 1.0 # max lateral acceleration limit [m/ss] - min_curve_velocity: 0.5 # min velocity at lateral acceleration limit and steering angle rate limit [m/s] - decel_distance_before_curve: 3.5 # slow speed distance before a curve for lateral acceleration limit - decel_distance_after_curve: 2.0 # slow speed distance after a curve for lateral acceleration limit - min_decel_for_lateral_acc_lim_filter: -2.5 # deceleration limit applied in the lateral acceleration filter to avoid sudden braking [m/ss] - - # engage & replan parameters - replan_vel_deviation: 5.53 # velocity deviation to replan initial velocity [m/s] - engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) - engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) - engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. - stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] - - # stop velocity - stopping_velocity: 2.778 # change target velocity to this value before v=0 point [m/s] - stopping_distance: 0.0 # distance for the stopping_velocity [m]. 0 means the stopping velocity is not applied. - - # path extraction parameters - extract_ahead_dist: 200.0 # forward trajectory distance used for planning [m] - extract_behind_dist: 5.0 # backward trajectory distance used for planning [m] - delta_yaw_threshold: 1.0472 # Allowed delta yaw between ego pose and trajectory pose [radian] - - # resampling parameters for optimization - max_trajectory_length: 200.0 # max trajectory length for resampling [m] - min_trajectory_length: 150.0 # min trajectory length for resampling [m] - resample_time: 2.0 # resample total time for dense sampling [s] - dense_resample_dt: 0.2 # resample time interval for dense sampling [s] - dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] - sparse_resample_dt: 0.5 # resample time interval for sparse sampling [s] - sparse_min_interval_distance: 4.0 # minimum points-interval length for sparse sampling [m] - - # resampling parameters for post process - post_max_trajectory_length: 300.0 # max trajectory length for resampling [m] - post_min_trajectory_length: 30.0 # min trajectory length for resampling [m] - post_resample_time: 10.0 # resample total time for dense sampling [s] - post_dense_resample_dt: 0.1 # resample time interval for dense sampling [s] - post_dense_min_interval_distance: 0.1 # minimum points-interval length for dense sampling [m] - post_sparse_resample_dt: 0.1 # resample time interval for sparse sampling [s] - post_sparse_min_interval_distance: 1.0 # minimum points-interval length for sparse sampling [m] - - # steering angle rate limit parameters - max_steering_angle_rate: 40.0 # maximum steering angle rate [degree/s] - resample_ds: 0.1 # distance between trajectory points [m] - curvature_threshold: 0.02 # if curvature > curvature_threshold, steeringRateLimit is triggered [1/m] - curvature_calculation_distance: 1.0 # distance of points while curvature is calculating [m] - - # system - over_stop_velocity_warn_thr: 1.389 # used to check if the optimization exceeds the input velocity on the stop point diff --git a/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/common/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml deleted file mode 100644 index b4e992ab56916..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ /dev/null @@ -1,77 +0,0 @@ -# see AvoidanceParameters description in avoidance_module_data.hpp for description. -/**: - ros__parameters: - avoidance: - resample_interval_for_planning: 0.3 - resample_interval_for_output: 4.0 - detection_area_right_expand_dist: 0.0 - detection_area_left_expand_dist: 1.0 - - enable_avoidance_over_same_direction: true - enable_avoidance_over_opposite_direction: true - enable_update_path_when_object_is_gone: false - - # For target object filtering - threshold_speed_object_is_stopped: 1.0 # [m/s] - threshold_time_object_is_moving: 1.0 # [s] - - object_check_forward_distance: 150.0 # [m] - object_check_backward_distance: 2.0 # [m] - - threshold_distance_object_is_on_center: 1.0 # [m] - - object_check_shiftable_ratio: 0.6 # [-] - object_check_min_road_shoulder_width: 0.5 # [m] - - # For lateral margin - object_envelope_buffer: 0.3 # [m] - lateral_collision_margin: 1.0 # [m] - lateral_collision_safety_buffer: 0.7 # [m] - - prepare_time: 2.0 # [s] - min_prepare_distance: 1.0 # [m] - min_avoidance_distance: 10.0 # [m] - - min_nominal_avoidance_speed: 7.0 # [m/s] - min_sharp_avoidance_speed: 1.0 # [m/s] - - road_shoulder_safety_margin: 0.5 # [m] - - max_right_shift_length: 5.0 - max_left_shift_length: 5.0 - - nominal_lateral_jerk: 0.2 # [m/s3] - max_lateral_jerk: 1.0 # [m/s3] - - # For detection loss compensation - object_last_seen_threshold: 2.0 # [s] - - # For prevention of large acceleration while avoidance - min_avoidance_speed_for_acc_prevention: 3.0 # [m/s] - max_avoidance_acceleration: 0.5 # [m/ss] - - # bound clipping for objects - enable_bound_clipping: false - - # for debug - publish_debug_marker: false - print_debug_info: false - - # not enabled yet - longitudinal_collision_margin_min_distance: 0.0 # [m] - longitudinal_collision_margin_time: 0.0 - - - # avoidance is performed for the object type with true - target_object: - car: true - truck: true - bus: true - trailer: true - unknown: false - bicycle: false - motorcycle: false - pedestrian: false - - # ---------- advanced parameters ---------- - avoidance_execution_lateral_threshold: 0.499 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml deleted file mode 100644 index 5edecdde26a11..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/behavior_path_planner.param.yaml +++ /dev/null @@ -1,17 +0,0 @@ -/**: - ros__parameters: - backward_path_length: 5.0 - forward_path_length: 300.0 - backward_length_buffer_for_end_of_lane: 5.0 - backward_length_buffer_for_end_of_pull_over: 5.0 - backward_length_buffer_for_end_of_pull_out: 5.0 - minimum_lane_change_length: 12.0 - minimum_pull_over_length: 16.0 - refine_goal_search_radius_range: 7.5 - turn_signal_intersection_search_distance: 30.0 - turn_signal_intersection_angle_threshold_deg: 15.0 - turn_signal_minimum_search_distance: 10.0 - turn_signal_search_time: 3.0 - turn_signal_shift_length_threshold: 0.3 - turn_signal_on_swerving: true - path_interval: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml deleted file mode 100644 index 660a4d2af0e84..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/drivable_area_expansion.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - avoidance: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - lane_change: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - lane_following: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - pull_out: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - pull_over: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 - side_shift: - drivable_area_right_bound_offset: 0.0 - drivable_area_left_bound_offset: 0.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml deleted file mode 100644 index 1a66bc09d040c..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_change/lane_change.param.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - lane_change: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - enable_abort_lane_change: true - enable_collision_check_at_prepare_phase: true - use_predicted_path_outside_lanelet: true - use_all_predicted_path: true - enable_blocked_by_obstacle: false diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml deleted file mode 100644 index b6a9574bb4800..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/lane_following/lane_following.param.yaml +++ /dev/null @@ -1,4 +0,0 @@ -/**: - ros__parameters: - lane_following: - lane_change_prepare_duration: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml deleted file mode 100644 index 7a9a63f805d67..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_out/pull_out.param.yaml +++ /dev/null @@ -1,30 +0,0 @@ -/**: - ros__parameters: - pull_out: - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 1.0 - collision_check_margin: 1.0 - pull_out_finish_judge_buffer: 1.0 - # shift pull out - enable_shift_pull_out: true - shift_pull_out_velocity: 2.0 - pull_out_sampling_num: 4 - before_pull_out_straight_distance: 0.0 - minimum_shift_pull_out_distance: 20.0 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - # geometric pull out - enable_geometric_pull_out: true - geometric_pull_out_velocity: 1.0 - arc_path_interval: 1.0 - lane_departure_margin: 0.2 - backward_velocity: -1.0 - pull_out_max_steer_angle: 0.26 # 15deg - # search start pose backward - enable_back: true - search_priority: "efficient_path" # "efficient_path" or "short_back_distance" - max_back_distance: 15.0 - backward_search_resolution: 2.0 - backward_path_update_duration: 3.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml deleted file mode 100644 index 44d859a477767..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/pull_over/pull_over.param.yaml +++ /dev/null @@ -1,61 +0,0 @@ -/**: - ros__parameters: - pull_over: - request_length: 200.0 - th_arrived_distance: 1.0 - th_stopped_velocity: 0.01 - th_stopped_time: 2.0 # It must be greater than the state_machine's. - pull_over_velocity: 3.0 - pull_over_minimum_velocity: 1.38 - margin_from_boundary: 0.5 - decide_path_distance: 10.0 - maximum_deceleration: 1.0 - # goal research - enable_goal_research: true - search_priority: "efficient_path" # "efficient_path" or "close_goal" - forward_goal_search_length: 20.0 - backward_goal_search_length: 20.0 - goal_search_interval: 2.0 - longitudinal_margin: 3.0 - # occupancy grid map - use_occupancy_grid: true - use_occupancy_grid_for_longitudinal_margin: false - occupancy_grid_collision_check_margin: 0.0 - theta_size: 360 - obstacle_threshold: 60 - # object recognition - use_object_recognition: true - object_recognition_collision_check_margin: 1.0 - # shift path - enable_shift_parking: true - pull_over_sampling_num: 4 - maximum_lateral_jerk: 2.0 - minimum_lateral_jerk: 0.5 - deceleration_interval: 15.0 - after_pull_over_straight_distance: 5.0 - before_pull_over_straight_distance: 5.0 - # parallel parking path - enable_arc_forward_parking: true - enable_arc_backward_parking: true - after_forward_parking_straight_distance: 2.0 - after_backward_parking_straight_distance: 2.0 - forward_parking_velocity: 1.38 - backward_parking_velocity: -1.38 - forward_parking_lane_departure_margin: 0.0 - backward_parking_lane_departure_margin: 0.0 - arc_path_interval: 1.0 - pull_over_max_steer_angle: 0.35 # 20deg - # hazard on when parked - hazard_on_threshold_distance: 1.0 - hazard_on_threshold_velocity: 0.5 - # check safety with dynamic objects. Not used now. - pull_over_duration: 2.0 - pull_over_prepare_duration: 4.0 - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - enable_collision_check_at_prepare_phase: false - use_predicted_path_outside_lanelet: false - use_all_predicted_path: false - # debug - print_debug_info: false diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml deleted file mode 100644 index 79044041b4889..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/side_shift/side_shift.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - side_shift: - min_distance_to_start_shifting: 5.0 - time_to_start_shifting: 1.0 - shifting_lateral_jerk: 0.2 - min_shifting_distance: 5.0 - min_shifting_speed: 5.56 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml deleted file mode 100644 index 1e0777ecf994a..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ /dev/null @@ -1,19 +0,0 @@ -/**: - ros__parameters: - launch_stop_line: true - launch_crosswalk: true - launch_traffic_light: true - launch_intersection: true - launch_blind_spot: true - launch_detection_area: true - launch_virtual_traffic_light: true - launch_occlusion_spot: true - launch_no_stopping_area: true - launch_run_out: false - forward_path_length: 1000.0 - backward_path_length: 5.0 - max_accel: -2.8 - max_jerk: -5.0 - system_delay: 0.5 - delay_response_time: 0.5 - is_publish_debug_path: false # publish all debug path with lane id in each module diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml deleted file mode 100644 index 31f75d7f7c5df..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/blind_spot.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - blind_spot: - use_pass_judge_line: true - stop_line_margin: 1.0 # [m] - backward_length: 15.0 # [m] - ignore_width_from_center_line: 0.7 # [m] - max_future_movement_time: 10.0 # [second] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml deleted file mode 100644 index 2bbc5d31fcaff..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/crosswalk.param.yaml +++ /dev/null @@ -1,44 +0,0 @@ -/**: - ros__parameters: - crosswalk: - show_processing_time: false # [-] whether to show processing time - - # param for stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists - stop_line_margin: 10.0 # [m] if objects cross X meters behind the stop line, the stop position is determined according to the object position (stop_margin meters before the object) - stop_margin: 2.0 # [m] the vehicle decelerates to be able to stop in front of object with margin - stop_position_threshold: 1.0 # [m] threshold for check whether the vehicle stop in front of crosswalk - - # param for ego velocity - min_slow_down_velocity: 2.78 # [m/s] target vehicle velocity when module receive slow down command from FOA (2.78 m/s = 10.0 kmph) - max_slow_down_jerk: -1.5 # [m/sss] minimum jerk deceleration for safe brake - max_slow_down_accel: -2.5 # [m/ss] minimum accel deceleration for safe brake - no_relax_velocity: 2.78 # [m/s] if the current velocity is less than X m/s, ego always stops at the stop position(not relax deceleration constraints 2.78 m/s = 10 kmph) - - # param for stuck vehicle - stuck_vehicle_velocity: 1.0 # [m/s] maximum velocity threshold whether the vehicle is stuck - max_lateral_offset: 2.0 # [m] maximum lateral offset for stuck vehicle position should be looked - stuck_vehicle_attention_range: 10.0 # [m] the detection area is defined as X meters behind the crosswalk - - # param for pass judge logic - ego_pass_first_margin: 6.0 # [s] time margin for ego pass first situation (the module judges that ego don't have to stop at TTC + MARGIN < TTV condition) - ego_pass_later_margin: 10.0 # [s] time margin for object pass first situation (the module judges that ego don't have to stop at TTV + MARGIN < TTC condition) - stop_object_velocity_threshold: 0.28 # [m/s] velocity threshold for the module to judge whether the objects is stopped (0.28 m/s = 1.0 kmph) - min_object_velocity: 1.39 # [m/s] minimum object velocity (compare the estimated velocity by perception module with this parameter and adopt the larger one to calculate TTV. 1.39 m/s = 5.0 kmph) - max_yield_timeout: 3.0 # [s] if the pedestrian does not move for X seconds after stopping before the crosswalk, the module judge that ego is able to pass first. - ego_yield_query_stop_duration: 0.1 # [s] the amount of time which ego should be stopping to query whether it yields or not - - # param for input data - tl_state_timeout: 3.0 # [s] timeout threshold for traffic light signal - - # param for target area & object - crosswalk_attention_range: 1.0 # [m] the detection area is defined as -X meters before the crosswalk to +X meters behind the crosswalk - target_object: - unknown: true # [-] whether to look and stop by UNKNOWN objects - bicycle: true # [-] whether to look and stop by BICYCLE objects - motorcycle: true # [-] whether to look and stop by MOTORCYCLE objects (tmp: currently it is difficult for perception modules to detect bicycles and motorcycles separately.) - pedestrian: true # [-] whether to look and stop by PEDESTRIAN objects - - walkway: - stop_duration_sec: 1.0 # [s] stop time at stop position - stop_line_distance: 3.5 # [m] make stop line away from crosswalk when no explicit stop line exists diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml deleted file mode 100644 index 9174045bf0150..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/detection_area.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - detection_area: - stop_margin: 0.0 - use_dead_line: false - dead_line_margin: 5.0 - use_pass_judge_line: false - state_clear_time: 2.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml deleted file mode 100644 index 32cd05a9cc153..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/no_stopping_area.param.yaml +++ /dev/null @@ -1,10 +0,0 @@ -/**: - ros__parameters: - no_stopping_area: - state_clear_time: 2.0 # [s] time to clear stop state - stuck_vehicle_vel_thr: 0.833 # 0.833m/s = 3.0km/h - stop_margin: 0.0 # [m] margin to stop line at no stopping area - dead_line_margin: 1.0 # [m] dead line offset go at no stopping area - stop_line_margin: 1.0 # [m] margin to auto-gen stop line at no stopping area - detection_area_length: 200.0 # [m] used to create detection area polygon - stuck_vehicle_front_margin: 6.0 # [m] obstacle stop max distance(5.0m) diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml deleted file mode 100644 index 957f7988a621b..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/occlusion_spot.param.yaml +++ /dev/null @@ -1,34 +0,0 @@ -/**: - ros__parameters: - occlusion_spot: - detection_method: "occupancy_grid" # [-] candidate is "occupancy_grid" or "predicted_object" - pass_judge: "smooth_velocity" # [-] candidate is "smooth_velocity" or "current_velocity" - use_object_info: true # [-] whether to reflect object info to occupancy grid map or not - use_moving_object_ray_cast: true # [-] whether to reflect moving object ray_cast to occupancy grid map or not - use_partition_lanelet: true # [-] whether to use partition lanelet map data - pedestrian_vel: 1.5 # [m/s] assume pedestrian is dashing from occlusion at this velocity - pedestrian_radius: 0.3 # [m] assume pedestrian width(0.2m) + margin(0.1m) - debug: # !Note: default should be false for performance - is_show_occlusion: false # [-] whether to show occlusion point markers. - is_show_cv_window: false # [-] whether to show open_cv debug window - is_show_processing_time: false # [-] whether to show processing time - threshold: - detection_area_length: 100.0 # [m] the length of path to consider perception range - stuck_vehicle_vel: 1.0 # [m/s] velocity below this value is assumed to stop - lateral_distance: 1.5 # [m] maximum lateral distance to consider hidden collision - motion: - safety_ratio: 0.8 # [-] jerk/acceleration ratio for safety - max_slow_down_jerk: -0.5 # [m/s^3] minimum jerk deceleration for safe brake. - max_slow_down_accel: -1.8 # [m/s^2] minimum accel deceleration for safe brake. - non_effective_jerk: -0.3 # [m/s^3] weak jerk for velocity planning. - non_effective_acceleration: -1.0 # [m/s^2] weak deceleration for velocity planning. - min_allowed_velocity: 1.0 # [m/s] minimum velocity allowed - safe_margin: 2.0 # [m] margin for detection failure(0.5m) + pedestrian radius(0.5m) + safe margin(1.0m) - detection_area: - min_occlusion_spot_size: 1.0 # [m] occupancy grid must contain an UNKNOWN area of at least size NxN to be considered a hidden obstacle. - slice_length: 10.0 # [m] size of slices in both length and distance relative to the ego path. - min_longitudinal_offset: 1.0 # [m] detection area safety buffer from front bumper. - max_lateral_distance: 6.0 # [m] buffer around the ego path used to build the detection area. - grid: - free_space_max: 43 # [-] maximum value of a free space cell in the occupancy grid - occupied_min: 58 # [-] minimum value of an occupied cell in the occupancy grid diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml deleted file mode 100644 index ccf90a0b29794..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml +++ /dev/null @@ -1,45 +0,0 @@ -/**: - ros__parameters: - run_out: - detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points - use_partition_lanelet: true # [-] whether to use partition lanelet map data - specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates - stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin - passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin - deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles - detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles - detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time - min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision - - detection_area: - margin_behind: 0.5 # [m] ahead margin for detection area length - margin_ahead: 1.0 # [m] behind margin for detection area length - - # parameter to create abstracted dynamic obstacles - dynamic_obstacle: - min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles - max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles - diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points - height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points - max_prediction_time: 10.0 # [sec] create predicted path until this time - time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path - points_interval: 0.1 # [m] divide obstacle points into groups with this interval, and detect only lateral nearest point. used only for Points method - - # approach if ego has stopped in the front of the obstacle for a certain amount of time - approaching: - enable: false - margin: 0.0 # [m] distance on how close ego approaches the obstacle - limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping - - # parameters for the change of state. used only when approaching is enabled - state: - stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping - stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state - disable_approach_dist: 3.0 # [m] end the approaching state if distance to the obstacle is longer than this value - keep_approach_duration: 1.0 # [sec] keep approach state for this duration to avoid chattering of state transition - - # parameter to avoid sudden stopping - slow_down_limit: - enable: true - max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. - max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml deleted file mode 100644 index a2b5ac5d5fcd1..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/stop_line.param.yaml +++ /dev/null @@ -1,6 +0,0 @@ -/**: - ros__parameters: - stop_line: - stop_margin: 0.0 - stop_check_dist: 2.0 - stop_duration_sec: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml deleted file mode 100644 index f5db276c9ead8..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/traffic_light.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - traffic_light: - stop_margin: 0.0 - tl_state_timeout: 1.0 - external_tl_state_timeout: 1.0 - yellow_lamp_period: 2.75 - enable_pass_judge: true diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml deleted file mode 100644 index 267dde50c7970..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/virtual_traffic_light.param.yaml +++ /dev/null @@ -1,8 +0,0 @@ -/**: - ros__parameters: - virtual_traffic_light: - max_delay_sec: 3.0 - near_line_distance: 1.0 - dead_line_margin: 1.0 - max_yaw_deviation_deg: 90.0 - check_timeout_after_stop_line: true diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml deleted file mode 100644 index 0bf42387a6621..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/lane_change_planner/lane_change_planner.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - min_stop_distance: 5.0 - stop_time: 2.0 - hysteresis_buffer_distance: 2.0 - backward_path_length: 5.0 - forward_path_length: 300.0 - max_accel: -5.0 - lane_change_prepare_duration: 4.0 - lane_changing_duration: 8.0 - backward_length_buffer_for_end_of_lane: 5.0 - lane_change_finish_judge_buffer: 3.0 - minimum_lane_change_length: 12.0 - minimum_lane_change_velocity: 5.6 - prediction_duration: 8.0 - prediction_time_resolution: 0.5 - drivable_area_resolution: 0.1 - drivable_area_width: 100.0 - drivable_area_height: 50.0 - static_obstacle_velocity_thresh: 1.5 - maximum_deceleration: 1.0 - refine_goal_search_radius_range: 7.5 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml deleted file mode 100644 index 8ba0a402ee861..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/rtc_auto_mode_manager/rtc_auto_mode_manager.param.yaml +++ /dev/null @@ -1,29 +0,0 @@ -/**: - ros__parameters: - module_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "pull_over" - - "pull_out" - - default_enable_list: - - "blind_spot" - - "crosswalk" - - "detection_area" - - "intersection" - - "no_stopping_area" - - "traffic_light" - - "lane_change_left" - - "lane_change_right" - - "avoidance_left" - - "avoidance_right" - - "pull_over" - - "pull_out" diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml deleted file mode 100644 index a18e531806171..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ /dev/null @@ -1,159 +0,0 @@ -/**: - ros__parameters: - option: - # publish - is_publishing_debug_visualization_marker: true - is_publishing_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_object_clearance_map: false # publish clearance map as nav_msgs::OccupancyGrid - is_publishing_area_with_objects: false # publish occupancy map as nav_msgs::OccupancyGrid - - is_stopping_if_outside_drivable_area: true # stop if the ego's footprint will be outside the drivable area - - # show - is_showing_debug_info: false - is_showing_calculation_time: false - - # other - enable_avoidance: false # enable avoidance function - enable_pre_smoothing: true # enable EB - skip_optimization: false # skip MPT and EB - reset_prev_optimization: false - - common: - # sampling - num_sampling_points: 100 # number of optimizing points - - # trajectory total/fixing length - trajectory_length: 300.0 # total trajectory length[m] - - forward_fixing_min_distance: 1.0 # number of fixing points around ego vehicle [m] - forward_fixing_min_time: 0.5 # forward fixing time with current velocity [s] - - backward_fixing_distance: 5.0 # backward fixing length from base_link [m] - delta_arc_length_for_trajectory: 0.1 # delta arc length for trajectory [m] - - delta_dist_threshold_for_closest_point: 3.0 # delta dist thres for closest point [m] - delta_yaw_threshold_for_closest_point: 1.046 #M_PI/3.0, delta yaw thres for closest point - delta_yaw_threshold_for_straight: 0.02 # delta dist thres for straight point - - num_fix_points_for_extending: 50 # number of fixing points when extending - max_dist_for_extending_end_point: 0.0001 # minimum delta dist thres for extending last point [m] - - enable_clipping_fixed_traj: false - non_fixed_trajectory_length: 5.0 # length of the trajectory merging optimized mpt trajectory to original(not optimized) trajectory - - object: # avoiding object - max_avoiding_objects_velocity_ms: 0.5 # maximum velocity for avoiding objects [m/s] - max_avoiding_ego_velocity_ms: 6.0 # maximum ego velocity when avoiding objects [m/s] - - avoiding_object_type: - unknown: true - car: true - truck: true - bus: true - bicycle: true - motorbike: true - pedestrian: true - animal: true - - # mpt param - mpt: - option: - steer_limit_constraint: true - fix_points_around_ego: true - plan_from_ego: true - max_plan_from_ego_length: 10.0 - visualize_sampling_num: 1 - enable_manual_warm_start: true - enable_warm_start: true # false - is_fixed_point_single: false - - common: - num_curvature_sampling_points: 5 # number of sampling points when calculating curvature - delta_arc_length_for_mpt_points: 0.5 # delta arc length when generating mpt [m] - - # kinematics: - # If this parameter is commented out, the parameter is set as below by default. - # The logic could be `optimization_center_offset = vehicle_info.wheel_base * 0.8` - # The 0.8 scale is adopted as it performed the best. - # optimization_center_offset: 2.3 # optimization center offset from base link - - # replanning & trimming trajectory param outside algorithm - replan: - max_path_shape_change_dist: 0.3 # threshold of path shape change from behavior path [m] - max_ego_moving_dist_for_replan: 3.0 # threshold of ego's moving distance for replan [m] - max_delta_time_sec_for_replan: 1.0 # threshold of delta time for replan [second] - - # advanced parameters to improve performance as much as possible - advanced: - eb: - common: - num_joint_buffer_points: 3 # number of joint buffer points - num_offset_for_begin_idx: 2 # number of shifting points for beginning non-fix idx - delta_arc_length_for_eb: 0.6 # 1.0 # delta arc length when optimizing[m] When the value is around 1.0, overshoot at the corner happens. - num_sampling_points_for_eb: 95 # number of optimizing points # must be longer than mpt points - - clearance: - clearance_for_straight_line: 0.05 # minimum optimizing range around straight points - clearance_for_joint: 0.1 # minimum optimizing range around joint points - clearance_for_only_smoothing: 0.1 # minimum optimizing range when applying only smoothing - - qp: - max_iteration: 10000 # max iteration when solving QP - eps_abs: 1.0e-8 # eps abs when solving OSQP - eps_rel: 1.0e-10 # eps rel when solving OSQP - - mpt: - bounds_search_widths: [0.45, 0.15, 0.05, 0.01] - - clearance: # clearance(distance) between vehicle and roads/objects when generating trajectory - hard_clearance_from_road: 0.0 # clearance from road boundary[m] - soft_clearance_from_road: 0.1 # clearance from road boundary[m] - soft_second_clearance_from_road: 1.0 # clearance from road boundary[m] - clearance_from_object: 1.0 # clearance from object[m] - extra_desired_clearance_from_road: 0.0 # extra desired clearance from road - - weight: - soft_avoidance_weight: 1000.0 # slack weight for lateral error around the middle point - soft_second_avoidance_weight: 100.0 # slack weight for lateral error around the middle point - - lat_error_weight: 100.0 # weight for lateral error - yaw_error_weight: 0.0 # weight for yaw error - yaw_error_rate_weight: 0.0 # weight for yaw error rate - steer_input_weight: 10.0 # weight for steering input - steer_rate_weight: 10.0 # weight for steering rate - - obstacle_avoid_lat_error_weight: 3.0 # weight for lateral error - obstacle_avoid_yaw_error_weight: 0.0 # weight for yaw error - obstacle_avoid_steer_input_weight: 1000.0 # weight for yaw error - near_objects_length: 30.0 # weight for yaw error - - terminal_lat_error_weight: 100.0 # weight for lateral error at terminal point - terminal_yaw_error_weight: 100.0 # weight for yaw error at terminal point - terminal_path_lat_error_weight: 1000.0 # weight for lateral error at path end point - terminal_path_yaw_error_weight: 1000.0 # weight for yaw error at path end point - - # check if planned trajectory is outside drivable area - collision_free_constraints: - option: - l_inf_norm: true - soft_constraint: true - hard_constraint: false - # two_step_soft_constraint: false - - vehicle_circles: - method: "rear_drive" - - uniform_circle: - num: 3 - radius_ratio: 0.8 - - rear_drive: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 - - bicycle_model: - num_for_calculation: 3 - front_radius_ratio: 1.0 - rear_radius_ratio: 1.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml deleted file mode 100644 index dfc2b2e6ddb44..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ /dev/null @@ -1,138 +0,0 @@ -/**: - ros__parameters: - common: - planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" - - is_showing_debug_info: false - disable_stop_planning: false # true - - # longitudinal info - 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] - - cruise_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: false - - stop_obstacle_type: - unknown: true - car: true - truck: true - bus: true - trailer: true - motorcycle: true - bicycle: true - pedestrian: true - - obstacle_filtering: - rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] - detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion - decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking - - # if crossing vehicle is decided as target obstacles or not - crossing_obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] - crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop - collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] - - outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] - outside_obstacle_min_velocity_threshold : 3.0 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] - ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] - max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego - - prediction_resampling_time_interval: 0.1 - prediction_resampling_time_horizon: 10.0 - - goal_extension_length: 5.0 # extension length for collision check around the goal - goal_extension_interval: 1.0 # extension interval for collision check around the goal - - stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle - - ignored_outside_obstacle_type: - unknown: true - car: false - truck: false - bus: false - trailer: false - motorcycle: false - bicycle: true - pedestrian: true - - pid_based_planner: - use_velocity_limit_based_planner: true - error_function_type: quadratic # choose from linear, quadratic - - velocity_limit_based_planner: - # PID gains to keep safe distance with the front vehicle - kp: 10.0 - ki: 0.0 - kd: 2.0 - - 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 - - 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_normalized_error_cruise_dist_gain: 0.2 - - optimization_based_planner: - dense_resampling_time_interval: 0.2 - sparse_resampling_time_interval: 2.0 - dense_time_horizon: 5.0 - max_time_horizon: 25.0 - velocity_margin: 0.2 #[m/s] - - # Parameters for safe distance - t_dangerous: 0.5 - - # For initial Motion - replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] - engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) - engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) - engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. - stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] - - # Weights for optimization - max_s_weight: 100.0 - max_v_weight: 1.0 - over_s_safety_weight: 1000000.0 - over_s_ideal_weight: 50.0 - over_v_weight: 500000.0 - over_a_weight: 5000.0 - over_j_weight: 10000.0 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml deleted file mode 100644 index dd76f2c4e540d..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml +++ /dev/null @@ -1,38 +0,0 @@ -/**: - ros__parameters: - adaptive_cruise_control: - # Adaptive Cruise Control (ACC) config - use_object_to_estimate_vel: true # use tracking objects for estimating object velocity or not - use_pcl_to_estimate_vel: true # use pcl for estimating object velocity or not - consider_obj_velocity: true # consider forward vehicle velocity to ACC or not - - # general parameter for ACC - obstacle_velocity_thresh_to_start_acc: 1.5 # start adaptive cruise control when the velocity of the forward obstacle exceeds this value [m/s] - obstacle_velocity_thresh_to_stop_acc: 1.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s] - emergency_stop_acceleration: -5.0 # supposed minimum acceleration (deceleration) in emergency stop [m/ss] - emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s] - min_dist_stop: 4.0 # minimum distance of emergency stop [m] - max_standard_acceleration: 0.5 # supposed maximum acceleration in active cruise control [m/ss] - min_standard_acceleration: -1.0 # supposed minimum acceleration (deceleration) in active cruise control - standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s] - min_dist_standard: 4.0 # minimum distance in active cruise control [m] - obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss] - margin_rate_to_change_vel: 0.3 # margin to insert upper velocity [-] - - # pid parameter for ACC - p_coefficient_positive: 0.1 # coefficient P in PID control (used when target dist -current_dist >=0) [-] - p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-] - d_coefficient_positive: 0.0 # coefficient D in PID control (used when delta_dist >=0) [-] - d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-] - - # parameter for object velocity estimation - object_polygon_length_margin: 2.0 # The distance to extend the polygon length the object in pointcloud-object matching [m] - object_polygon_width_margin: 0.5 # The distance to extend the polygon width the object in pointcloud-object matching [m] - valid_estimated_vel_diff_time: 1.0 # Maximum time difference treated as continuous points in speed estimation using a point cloud [s] - valid_vel_que_time: 0.5 # Time width of information used for speed estimation in speed estimation using a point cloud [s] - valid_estimated_vel_max: 20.0 # Maximum value of valid speed estimation results in speed estimation using a point cloud [m/s] - valid_estimated_vel_min: -20.0 # Minimum value of valid speed estimation results in speed estimation using a point cloud [m/s] - thresh_vel_to_stop: 1.5 # Embed a stop line if the maximum speed calculated by ACC is lower than this speed [m/s] - lowpass_gain_of_upper_velocity: 0.75 # Lowpass-gain of upper velocity - use_rough_velocity_estimation: false # Use rough estimated velocity if the velocity estimation is failed (#### If this parameter is true, the vehicle may collide with the front car. Be careful. ####) - rough_velocity_rate: 0.9 # In the rough velocity estimation, the velocity of front car is estimated as self current velocity * this value diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml deleted file mode 100644 index 55a21d6cd7b20..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/obstacle_stop_planner.param.yaml +++ /dev/null @@ -1,49 +0,0 @@ -/**: - ros__parameters: - hunting_threshold: 0.5 # even if the obstacle disappears, the stop judgment continues for hunting_threshold [s] - lowpass_gain: 0.9 # gain parameter for low pass filter [-] - max_velocity: 20.0 # max velocity [m/s] - enable_slow_down: False # whether to use slow down planner [-] - - stop_planner: - # params for stop position - stop_position: - max_longitudinal_margin: 5.0 # stop margin distance from obstacle on the path [m] - min_longitudinal_margin: 5.0 # stop margin distance when any other stop point is inserted in stop margin [m] - hold_stop_margin_distance: 0.0 # the ego keeps stopping if the ego is in this margin [m] - - # params for detection area - detection_area: - lateral_margin: 0.0 # margin of vehicle footprint [m] - step_length: 1.0 # step length for pointcloud search range [m] - extend_distance: 0.0 # extend trajectory to consider after goal obstacle in the extend_distance [m] - - - slow_down_planner: - # params for slow down section - slow_down_section: - longitudinal_forward_margin: 5.0 # margin distance from slow down point to vehicle front [m] - longitudinal_backward_margin: 0.0 # margin distance from slow down point to vehicle rear [m] - longitudinal_margin_span: -0.1 # fineness param for relaxing slow down margin (use this param if consider_constraints is True) [m/s] - min_longitudinal_forward_margin: 1.0 # min margin for relaxing slow down margin (use this param if consider_constraints is True) [m/s] - - # params for detection area - detection_area: - lateral_margin: 1.0 # offset from vehicle side edge for expanding the search area of the surrounding point cloud [m] - - # params for velocity - target_velocity: - max_slow_down_velocity: 1.38 # max slow down velocity (use this param if consider_constraints is False)[m/s] - min_slow_down_velocity: 0.28 # min slow down velocity (use this param if consider_constraints is False)[m/s] - slow_down_velocity: 1.38 # target slow down velocity (use this param if consider_constraints is True)[m/s] - - # params for deceleration constraints (use this param if consider_constraints is True) - constraints: - jerk_min_slow_down: -0.6 # min slow down jerk constraint [m/sss] - jerk_span: -0.01 # fineness param for planning deceleration jerk [m/sss] - jerk_start: -0.1 # init jerk used for deceleration planning [m/sss] - - # others - consider_constraints: False # set "True", if no decel plan found under jerk/dec constrains, relax target slow down vel [-] - velocity_threshold_decel_complete: 0.2 # use for judge whether the ego velocity converges the target slow down velocity [m/s] - acceleration_threshold_decel_complete: 0.1 # use for judge whether the ego velocity converges the target slow down velocity [m/ss] diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml deleted file mode 100644 index c6a0c275db0b4..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_velocity_limiter/obstacle_velocity_limiter.param.yaml +++ /dev/null @@ -1,33 +0,0 @@ -/**: - ros__parameters: - min_ttc: 1.0 # [s] minimum time without collision when doing projection at each trajectory point - distance_buffer: 0.0 # [m] extra distance to add to a projection (in addition to the vehicle overhang) - min_adjusted_velocity: 2.5 # [m/s] minimum velocity that the module can set - max_deceleration: 2.0 # [m/s²] maximum deceleration caused by the adjusted velocity - - trajectory_preprocessing: - start_distance: 0.0 # [m] distance ahead of ego from which to start modifying the trajectory - max_length: 100.0 # [m] maximum distance (from the start point) where the velocity is adjusted - max_duration: 10.0 # [s] maximum duration (from the start point) where the velocity is adjusted - downsample_factor: 10 # factor by which to downsample the input trajectory for calculation - calculate_steering_angles: false # if true, the steering angle at each trajectory point is calculated from the change in heading - - simulation: - model: particle # model to use for forward projection at each trajectory point. Either "particle" or "bicycle" - distance_method: exact # distance calculation method. Either "exact" or "approximation". - # parameters used only with the bicycle model - steering_offset: 0.01 # [rad] steering angle offset used to model uncertainty in the forward projection - nb_points: 5 # number of points representing the curved projections - - obstacles: - dynamic_source: static_only # source of dynamic obstacles. Must be 'pointcloud', 'occupancy_grid', or 'static_only'. - ignore_obstacles_on_path: false # if true, obstacles on the ego paths are ignored - ignore_extra_distance: 1.0 # [m] extra lateral distance where obstacles along the path are ignored - occupancy_grid_threshold: 60 # occupancy grid values higher than this are considered to be obstacles - dynamic_obstacles_buffer: 1.5 # [m] extra distance around dynamic obstacles used to mask the occupancy grid and avoid false obstacle detection - dynamic_obstacles_min_vel: 0.5 # [m/s] velocity above which a dynamic obstacle is ignored by the module - static_map_tags: # linestring tags in the lanelet maps that will be used as static obstacles - - guard_rail - filter_envelope : false # whether to calculate the apparent safety envelope and use it to filter obstacles - rtree_min_points: 500 # from this number of obstacle points, a rtree is used for collision detection - rtree_min_segments: 1600 # from this number of obstacle segments, a rtree is used for collision detection diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml deleted file mode 100644 index 6aa4e71774742..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/surround_obstacle_checker/surround_obstacle_checker.param.yaml +++ /dev/null @@ -1,15 +0,0 @@ -/**: - ros__parameters: - - # obstacle check - use_pointcloud: true # use pointcloud as obstacle check - use_dynamic_object: true # use dynamic object as obstacle check - surround_check_distance: 0.5 # if objects exist in this distance, transit to "exist-surrounding-obstacle" status [m] - surround_check_recover_distance: 0.8 # if no object exists in this distance, transit to "non-surrounding-obstacle" status [m] - state_clear_time: 2.0 - - # ego stop state - stop_state_ego_speed: 0.1 #[m/s] - - # debug - publish_debug_footprints: true # publish vehicle footprint & footprints with surround_check_distance and surround_check_recover_distance offsets diff --git a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml deleted file mode 100644 index d6152c4f56243..0000000000000 --- a/launch/tier4_planning_launch/config/scenario_planning/parking/freespace_planner/freespace_planner.param.yaml +++ /dev/null @@ -1,39 +0,0 @@ -/**: - ros__parameters: - # -- Node Configurations -- - planning_algorithm: "astar" - waypoints_velocity: 5.0 - update_rate: 10.0 - th_arrived_distance_m: 1.0 - th_stopped_time_sec: 1.0 - th_stopped_velocity_mps: 0.01 - th_course_out_distance_m: 1.0 - vehicle_shape_margin_m: 1.0 - replan_when_obstacle_found: true - replan_when_course_out: true - - # -- Configurations common to the all planners -- - # base configs - time_limit: 30000.0 - # robot configs # TODO replace by vehicle_info - robot_length: 4.5 - robot_width: 1.75 - robot_base2back: 1.0 - minimum_turning_radius: 9.0 - maximum_turning_radius: 9.0 - turning_radius_size: 1 - # search configs - theta_size: 144 - angle_goal_range: 6.0 - curve_weight: 1.2 - reverse_weight: 2.0 - lateral_goal_range: 0.5 - longitudinal_goal_range: 2.0 - # costmap configs - obstacle_threshold: 100 - - # -- A* search Configurations -- - astar: - only_behind_solutions: false - use_back: true - distance_heuristic_weight: 1.0 diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index e179d410e345e..e628c120a0e36 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -1,14 +1,54 @@ - + + + + + + + + - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -22,10 +62,9 @@ - + - diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 7cd07bdc47c1d..ffe3a32b6a1af 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -1,13 +1,4 @@ - - - - - - - - - @@ -16,18 +7,13 @@ - + - - @@ -38,8 +24,7 @@ - - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 25c23f9189087..1eccb82e4ae82 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -34,113 +32,30 @@ def launch_setup(context, *args, **kwargs): # vehicle information parameter - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: - vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + vehicle_param_path = LaunchConfiguration("vehicle_param_file").perform(context) + with open(vehicle_param_path, "r") as f: + vehicle_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] # behavior path planner - side_shift_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "side_shift", - "side_shift.param.yaml", - ) - with open(side_shift_param_path, "r") as f: + with open(LaunchConfiguration("side_shift_param_path").perform(context), "r") as f: side_shift_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - avoidance_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "avoidance", - "avoidance.param.yaml", - ) - with open(avoidance_param_path, "r") as f: + with open(LaunchConfiguration("avoidance_param_path").perform(context), "r") as f: avoidance_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_change_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "lane_change", - "lane_change.param.yaml", - ) - with open(lane_change_param_path, "r") as f: + with open(LaunchConfiguration("lane_change_param_path").perform(context), "r") as f: lane_change_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_following_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "lane_following", - "lane_following.param.yaml", - ) - with open(lane_following_param_path, "r") as f: + with open(LaunchConfiguration("lane_following_param_path").perform(context), "r") as f: lane_following_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - pull_over_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "pull_over", - "pull_over.param.yaml", - ) - with open(pull_over_param_path, "r") as f: + with open(LaunchConfiguration("pull_over_param_path").perform(context), "r") as f: pull_over_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - pull_out_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "pull_out", - "pull_out.param.yaml", - ) - with open(pull_out_param_path, "r") as f: + with open(LaunchConfiguration("pull_out_param_path").perform(context), "r") as f: pull_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - drivable_area_expansion_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "drivable_area_expansion.param.yaml", - ) - with open(drivable_area_expansion_param_path, "r") as f: + with open(LaunchConfiguration("drivable_area_expansion_param_path").perform(context), "r") as f: drivable_area_expansion_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - behavior_path_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_path_planner", - "behavior_path_planner.param.yaml", - ) - with open(behavior_path_planner_param_path, "r") as f: + with open(LaunchConfiguration("behavior_path_planner_param_path").perform(context), "r") as f: behavior_path_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] behavior_path_planner_component = ComposableNode( @@ -169,7 +84,7 @@ def launch_setup(context, *args, **kwargs): pull_out_param, drivable_area_expansion_param, behavior_path_planner_param, - vehicle_info_param, + vehicle_param, { "bt_tree_config_path": [ FindPackageShare("behavior_path_planner"), @@ -182,155 +97,41 @@ def launch_setup(context, *args, **kwargs): ) # smoother param - common_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "common.param.yaml", - ) - with open(common_param_path, "r") as f: + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: common_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - motion_velocity_smoother_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "motion_velocity_smoother", - "motion_velocity_smoother.param.yaml", - ) - with open(motion_velocity_smoother_param_path, "r") as f: + with open( + LaunchConfiguration("motion_velocity_smoother_param_path").perform(context), "r" + ) as f: motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - smoother_type_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "motion_velocity_smoother", - "Analytical.param.yaml", - ) - with open(smoother_type_param_path, "r") as f: - smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open( + LaunchConfiguration("behavior_velocity_smoother_type_param_path").perform(context), "r" + ) as f: + behavior_velocity_smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] # behavior velocity planner - blind_spot_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "blind_spot.param.yaml", - ) - with open(blind_spot_param_path, "r") as f: + with open(LaunchConfiguration("blind_spot_param_path").perform(context), "r") as f: blind_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - crosswalk_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "crosswalk.param.yaml", - ) - with open(crosswalk_param_path, "r") as f: + with open(LaunchConfiguration("crosswalk_param_path").perform(context), "r") as f: crosswalk_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - detection_area_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "detection_area.param.yaml", - ) - with open(detection_area_param_path, "r") as f: + with open(LaunchConfiguration("detection_area_param_path").perform(context), "r") as f: detection_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - intersection_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "intersection.param.yaml", - ) - with open(intersection_param_path, "r") as f: + with open(LaunchConfiguration("intersection_param_path").perform(context), "r") as f: intersection_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - stop_line_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "stop_line.param.yaml", - ) - with open(stop_line_param_path, "r") as f: + with open(LaunchConfiguration("stop_line_param_path").perform(context), "r") as f: stop_line_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - traffic_light_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "traffic_light.param.yaml", - ) - with open(traffic_light_param_path, "r") as f: + with open(LaunchConfiguration("traffic_light_param_path").perform(context), "r") as f: traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - virtual_traffic_light_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "virtual_traffic_light.param.yaml", - ) - with open(virtual_traffic_light_param_path, "r") as f: + with open(LaunchConfiguration("virtual_traffic_light_param_path").perform(context), "r") as f: virtual_traffic_light_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - occlusion_spot_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "occlusion_spot.param.yaml", - ) - with open(occlusion_spot_param_path, "r") as f: + with open(LaunchConfiguration("occlusion_spot_param_path").perform(context), "r") as f: occlusion_spot_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - no_stopping_area_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "no_stopping_area.param.yaml", - ) - with open(no_stopping_area_param_path, "r") as f: + with open(LaunchConfiguration("no_stopping_area_param_path").perform(context), "r") as f: no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - run_out_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "run_out.param.yaml", - ) - with open(run_out_param_path, "r") as f: + with open(LaunchConfiguration("run_out_param_path").perform(context), "r") as f: run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - behavior_velocity_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "behavior_planning", - "behavior_velocity_planner", - "behavior_velocity_planner.param.yaml", - ) - with open(behavior_velocity_planner_param_path, "r") as f: + with open( + LaunchConfiguration("behavior_velocity_planner_param_path").perform(context), "r" + ) as f: behavior_velocity_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] behavior_velocity_planner_component = ComposableNode( @@ -389,11 +190,11 @@ def launch_setup(context, *args, **kwargs): virtual_traffic_light_param, occlusion_spot_param, no_stopping_area_param, - vehicle_info_param, + vehicle_param, run_out_param, common_param, motion_velocity_smoother_param, - smoother_type_param, + behavior_velocity_smoother_type_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -477,7 +278,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", @@ -490,8 +291,6 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg("map_topic_name", "/map/vector_map", "input topic of map") - add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") - # component add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index c4c7b3bab7597..92795bd2a9533 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml @@ -1,7 +1,7 @@ - + @@ -17,13 +17,13 @@ - + - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py index 9ea5200a1cbd2..e7e1292f1d2c1 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -32,40 +30,22 @@ def launch_setup(context, *args, **kwargs): # vehicle information param path - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] # planning common param path - common_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "common.param.yaml", - ) - with open(common_param_path, "r") as f: + with open(LaunchConfiguration("common_param_path").perform(context), "r") as f: common_param = yaml.safe_load(f)["/**"]["ros__parameters"] # nearest search parameter - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] # obstacle avoidance planner - obstacle_avoidance_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_avoidance_planner", - "obstacle_avoidance_planner.param.yaml", - ) - with open(obstacle_avoidance_planner_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_avoidance_planner_param_path").perform(context), "r" + ) as f: obstacle_avoidance_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_avoidance_planner_component = ComposableNode( package="obstacle_avoidance_planner", @@ -88,15 +68,9 @@ def launch_setup(context, *args, **kwargs): ) # obstacle velocity limiter - obstacle_velocity_limiter_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_velocity_limiter", - "obstacle_velocity_limiter.param.yaml", - ) - with open(obstacle_velocity_limiter_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r" + ) as f: obstacle_velocity_limiter_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_velocity_limiter_component = ComposableNode( package="obstacle_velocity_limiter", @@ -122,15 +96,9 @@ def launch_setup(context, *args, **kwargs): ) # surround obstacle checker - surround_obstacle_checker_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "surround_obstacle_checker", - "surround_obstacle_checker.param.yaml", - ) - with open(surround_obstacle_checker_param_path, "r") as f: + with open( + LaunchConfiguration("surround_obstacle_checker_param_path").perform(context), "r" + ) as f: surround_obstacle_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] surround_obstacle_checker_component = ComposableNode( package="surround_obstacle_checker", @@ -160,25 +128,11 @@ def launch_setup(context, *args, **kwargs): ) # obstacle stop planner - obstacle_stop_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_stop_planner", - "obstacle_stop_planner.param.yaml", - ) - obstacle_stop_planner_acc_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_stop_planner", - "adaptive_cruise_control.param.yaml", - ) - with open(obstacle_stop_planner_param_path, "r") as f: + with open(LaunchConfiguration("obstacle_stop_planner_param_path").perform(context), "r") as f: obstacle_stop_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] - with open(obstacle_stop_planner_acc_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_stop_planner_acc_param_path").perform(context), "r" + ) as f: obstacle_stop_planner_acc_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_stop_planner_component = ComposableNode( package="obstacle_stop_planner", @@ -214,15 +168,7 @@ def launch_setup(context, *args, **kwargs): ) # obstacle cruise planner - obstacle_cruise_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "lane_driving", - "motion_planning", - "obstacle_cruise_planner", - "obstacle_cruise_planner.param.yaml", - ) - with open(obstacle_cruise_planner_param_path, "r") as f: + with open(LaunchConfiguration("obstacle_cruise_planner_param_path").perform(context), "r") as f: obstacle_cruise_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] obstacle_cruise_planner_component = ComposableNode( package="obstacle_cruise_planner", @@ -317,7 +263,7 @@ def add_launch_arg(name: str, default_value=None, description=None): # vehicle information parameter file add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", @@ -338,8 +284,6 @@ def add_launch_arg(name: str, default_value=None, description=None): "cruise_planner", "obstacle_stop_planner", "cruise planner type" ) # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none" - add_launch_arg("tier4_planning_launch_param_path", None, "tier4_planning_launch parameter path") - add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml index 3d195185d9bd6..3f7650486e69d 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.xml @@ -1,7 +1,7 @@ - + @@ -12,7 +12,7 @@ - + @@ -20,7 +20,7 @@ - + @@ -29,7 +29,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py index 81758eec72485..b6400b568837a 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -31,18 +29,11 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) + vehicle_info_param_path = LaunchConfiguration("vehicle_param_file").perform(context) with open(vehicle_info_param_path, "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - freespace_planner_param_path = os.path.join( - LaunchConfiguration("tier4_planning_launch_param_path").perform(context), - "scenario_planning", - "parking", - "freespace_planner", - "freespace_planner.param.yaml", - ) - with open(freespace_planner_param_path, "r") as f: + with open(LaunchConfiguration("freespace_planner_param_path").perform(context), "r") as f: freespace_planner_param = yaml.safe_load(f)["/**"]["ros__parameters"] container = ComposableNodeContainer( @@ -136,7 +127,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ) add_launch_arg( - "vehicle_info_param_file", + "vehicle_param_file", [ FindPackageShare("vehicle_info_util"), "/config/vehicle_info.param.yaml", diff --git a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml index 6e3a183220d0a..741df95937c42 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/parking.launch.xml @@ -1,5 +1,5 @@ - + @@ -12,7 +12,7 @@ - + @@ -25,7 +25,7 @@ - + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 1bcc85f6cf33b..103c2172ec7f0 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -1,14 +1,4 @@ - - - - - - - - - - @@ -25,25 +15,23 @@ - - + - - - + + @@ -54,16 +42,15 @@ - + - - + From 5f042baf1241201bcd33311b9f7a11b65791d1f0 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 21 Dec 2022 17:02:41 +0900 Subject: [PATCH 26/37] feat(tier4_perception_launch): remove configs and move to autoware_launch (#2539) * feat(tier4_perception_launch): remove configs and move to autoware_launch Signed-off-by: kminoda * update readme Signed-off-by: kminoda * remove config Signed-off-by: kminoda * update readme Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_perception_launch/CMakeLists.txt | 1 - launch/tier4_perception_launch/README.md | 7 ++ .../object_lanelet_filter.param.yaml | 11 ---- .../object_position_filter.param.yaml | 16 ----- .../pointcloud_map_filter.param.yaml | 5 -- .../data_association_matrix.param.yaml | 66 ------------------- .../elevation_map_parameters.yaml | 30 --------- .../ground_segmentation.param.yaml | 31 --------- ...ra_lidar_fusion_based_detection.launch.xml | 4 +- .../detection/detection.launch.xml | 3 - .../lidar_based_detection.launch.xml | 4 +- .../detection/pointcloud_map_filter.launch.py | 6 +- .../tracking/tracking.launch.xml | 3 +- .../ground_segmentation.launch.py | 15 ++--- .../launch/perception.launch.xml | 15 +++-- 15 files changed, 27 insertions(+), 190 deletions(-) delete mode 100644 launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml delete mode 100644 launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml delete mode 100644 launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml delete mode 100644 launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml delete mode 100644 launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml delete mode 100644 launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml diff --git a/launch/tier4_perception_launch/CMakeLists.txt b/launch/tier4_perception_launch/CMakeLists.txt index 4962a2932201b..2474b2f4dd3af 100644 --- a/launch/tier4_perception_launch/CMakeLists.txt +++ b/launch/tier4_perception_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_perception_launch/README.md b/launch/tier4_perception_launch/README.md index a81c43591483b..6877e66bef7f5 100644 --- a/launch/tier4_perception_launch/README.md +++ b/launch/tier4_perception_launch/README.md @@ -12,9 +12,16 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `perception.launch.xml`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `perception.launch.xml`. + ```xml + + + + + ... ``` diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml deleted file mode 100644 index dfdee95642fed..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/object_lanelet_filter.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - filter_target_label: - UNKNOWN : true - CAR : false - TRUCK : false - BUS : false - TRAILER : false - MOTORCYCLE : false - BICYCLE : false - PEDESTRIAN : false diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml deleted file mode 100644 index 70afd9d31be94..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/object_position_filter.param.yaml +++ /dev/null @@ -1,16 +0,0 @@ -/**: - ros__parameters: - filter_target_label: - UNKNOWN : true - CAR : false - TRUCK : false - BUS : false - TRAILER : false - MOTORCYCLE : false - BICYCLE : false - PEDESTRIAN : false - - upper_bound_x: 100.0 - lower_bound_x: 0.0 - upper_bound_y: 10.0 - lower_bound_y: -10.0 diff --git a/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml b/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml deleted file mode 100644 index a07a9416c2a3e..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/detection/pointcloud_map_filter.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - use_down_sample_filter: False - down_sample_voxel_size: 0.1 - distance_threshold: 0.5 diff --git a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml b/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml deleted file mode 100644 index 2541ab0367cad..0000000000000 --- a/launch/tier4_perception_launch/config/object_recognition/tracking/multi_object_tracker/data_association_matrix.param.yaml +++ /dev/null @@ -1,66 +0,0 @@ -/**: - ros__parameters: - can_assign_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE,PEDESTRIAN <-Measurement - [1, 0, 0, 0, 0, 0, 0, 0, #UNKNOWN <-Tracker - 0, 1, 1, 1, 1, 0, 0, 0, #CAR - 0, 1, 1, 1, 1, 0, 0, 0, #TRUCK - 0, 1, 1, 1, 1, 0, 0, 0, #BUS - 0, 1, 1, 1, 1, 0, 0, 0, #TRAILER - 0, 0, 0, 0, 0, 1, 1, 1, #MOTORBIKE - 0, 0, 0, 0, 0, 1, 1, 1, #BICYCLE - 0, 0, 0, 0, 0, 1, 1, 1] #PEDESTRIAN - - max_dist_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [4.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, #UNKNOWN - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #CAR - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRUCK - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #BUS - 4.0, 2.0, 5.0, 5.0, 5.0, 1.0, 1.0, 1.0, #TRAILER - 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #MOTORBIKE - 3.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0, #BICYCLE - 2.0, 1.0, 1.0, 1.0, 1.0, 3.0, 3.0, 2.0] #PEDESTRIAN - max_area_matrix: - # NOTE: The size of truck is 12 m length x 3 m width. - # NOTE: The size of trailer is 20 m length x 3 m width. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, 100.00, #UNKNOWN - 12.10, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #CAR - 36.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRUCK - 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #BUS - 60.00, 12.10, 36.00, 60.00, 60.00, 10000.00, 10000.00, 10000.00, #TRAILER - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #MOTORBIKE - 2.50, 10000.00, 10000.00, 10000.00, 10000.00, 2.50, 2.50, 1.00, #BICYCLE - 2.00, 10000.00, 10000.00, 10000.00, 10000.00, 1.50, 1.50, 1.00] #PEDESTRIAN - min_area_matrix: - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [ 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, #UNKNOWN - 3.600, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #CAR - 6.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRUCK - 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #BUS - 10.000, 3.600, 6.000, 10.000, 10.000, 0.000, 0.000, 0.000, #TRAILER - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #MOTORBIKE - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100, #BICYCLE - 0.001, 0.000, 0.000, 0.000, 0.000, 0.100, 0.100, 0.100] #PEDESTRIAN - max_rad_matrix: # If value is greater than pi, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER MOTORBIKE, BICYCLE, PEDESTRIAN - [3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #UNKNOWN - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #CAR - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRUCK - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #BUS - 3.150, 1.047, 1.047, 1.047, 1.047, 3.150, 3.150, 3.150, #TRAILER - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #MOTORBIKE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, #BICYCLE - 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150, 3.150] #PEDESTRIAN - - min_iou_matrix: # If value is negative, it will be ignored. - #UNKNOWN, CAR, TRUCK, BUS, TRAILER, MOTORBIKE, BICYCLE, PEDESTRIAN - [0.0001, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #UNKNOWN - 0.1, 0.1, 0.2, 0.2, 0.2, 0.1, 0.1, 0.1, #CAR - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRUCK - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #BUS - 0.1, 0.2, 0.3, 0.3, 0.3, 0.1, 0.1, 0.1, #TRAILER - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #MOTORBIKE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, #BICYCLE - 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.0001] #PEDESTRIAN diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml deleted file mode 100644 index 781ccb806b25d..0000000000000 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/elevation_map_parameters.yaml +++ /dev/null @@ -1,30 +0,0 @@ -pcl_grid_map_extraction: - num_processing_threads: 12 - cloud_transform: - translation: - x: 0.0 - y: 0.0 - z: 0.0 - rotation: # intrinsic rotation X-Y-Z (r-p-y)sequence - r: 0.0 - p: 0.0 - y: 0.0 - cluster_extraction: - cluster_tolerance: 0.2 - min_num_points: 3 - max_num_points: 1000000 - outlier_removal: - is_remove_outliers: false - mean_K: 10 - stddev_threshold: 1.0 - downsampling: - is_downsample_cloud: false - voxel_size: - x: 0.02 - y: 0.02 - z: 0.02 - grid_map: - min_num_points_per_cell: 3 - resolution: 0.3 - height_type: 1 - height_thresh: 1.0 diff --git a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml b/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml deleted file mode 100644 index 8d56e12244716..0000000000000 --- a/launch/tier4_perception_launch/config/obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml +++ /dev/null @@ -1,31 +0,0 @@ -/**: - ros__parameters: - additional_lidars: [] - ransac_input_topics: [] - use_single_frame_filter: False - use_time_series_filter: True - - common_crop_box_filter: - parameters: - min_x: -50.0 - max_x: 100.0 - min_y: -50.0 - max_y: 50.0 - max_z: 2.5 # recommended 2.5 for non elevation_grid_mode - min_z: -2.5 # recommended 0.0 for non elevation_grid_mode - negative: False - - common_ground_filter: - plugin: "ground_segmentation::ScanGroundFilterComponent" - parameters: - global_slope_max_angle_deg: 10.0 - local_slope_max_angle_deg: 13.0 # recommended 30.0 for non elevation_grid_mode - split_points_distance_tolerance: 0.2 - use_virtual_ground_point: True - split_height_distance: 0.2 - non_ground_height_threshold: 0.20 - grid_size_m: 0.1 - grid_mode_switch_radius: 20.0 - gnd_grid_buffer_size: 4 - detection_range_z_max: 2.5 - elevation_grid_mode: true diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml index 87a39cfb62c1a..74d4cbe98de44 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml @@ -1,6 +1,5 @@ - @@ -48,7 +47,6 @@ - @@ -274,7 +272,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml index 40e9c407d38a2..9c0e2b1ac6d02 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/detection.launch.xml @@ -1,6 +1,5 @@ - @@ -64,7 +63,6 @@ - @@ -106,7 +104,6 @@ - diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml index 1fb33b2be9014..552b605a90db1 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/lidar_based_detection.launch.xml @@ -1,6 +1,5 @@ - @@ -18,7 +17,6 @@ - @@ -143,7 +141,7 @@ - + diff --git a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py index f6a88bb63f639..bd636b289ca93 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py +++ b/launch/tier4_perception_launch/launch/object_recognition/detection/pointcloud_map_filter.launch.py @@ -29,8 +29,9 @@ class PointcloudMapFilterPipeline: def __init__(self, context): pointcloud_map_filter_param_path = os.path.join( - LaunchConfiguration("tier4_perception_launch_param_path").perform(context), - "object_recognition/detection/pointcloud_map_filter.param.yaml", + LaunchConfiguration( + "object_recognition_detection_pointcloud_map_filter_param_path" + ).perform(context), ) with open(pointcloud_map_filter_param_path, "r") as f: self.pointcloud_map_filter_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -150,7 +151,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "pointcloud_map_filter_pipeline_container") - add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml index 4ab68d11ea7c9..0d17e1d7338e5 100644 --- a/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml +++ b/launch/tier4_perception_launch/launch/object_recognition/tracking/tracking.launch.xml @@ -2,11 +2,10 @@ - - + diff --git a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py index b68275bdfe462..afc55582853f9 100644 --- a/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py +++ b/launch/tier4_perception_launch/launch/obstacle_segmentation/ground_segmentation/ground_segmentation.launch.py @@ -33,8 +33,9 @@ def __init__(self, context): self.context = context self.vehicle_info = self.get_vehicle_info() ground_segmentation_param_path = os.path.join( - LaunchConfiguration("tier4_perception_launch_param_path").perform(context), - "obstacle_segmentation/ground_segmentation/ground_segmentation.param.yaml", + LaunchConfiguration("obstacle_segmentation_ground_segmentation_param_path").perform( + context + ), ) with open(ground_segmentation_param_path, "r") as f: self.ground_segmentation_param = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -329,12 +330,9 @@ def create_single_frame_outlier_filter_components(input_topic, output_topic, con "inpaint_radius": 1.0, "param_file_path": PathJoinSubstitution( [ - LaunchConfiguration("tier4_perception_launch_param_path").perform( - context - ), - "obstacle_segmentation", - "ground_segmentation", - "elevation_map_parameters.yaml", + LaunchConfiguration( + "obstacle_segmentation_ground_segmentation_elevation_map_param_path" + ).perform(context), ] ), "elevation_map_directory": PathJoinSubstitution( @@ -518,7 +516,6 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("use_intra_process", "True") add_launch_arg("use_pointcloud_container", "False") add_launch_arg("container_name", "perception_pipeline_container") - add_launch_arg("tier4_perception_launch_param_path", "tier4_perception_launch parameter path") add_launch_arg("input/pointcloud", "/sensing/lidar/concatenated/pointcloud") set_container_executable = SetLaunchConfiguration( diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index b9a94a04e46da..8fb906fadab60 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -1,7 +1,12 @@ - - + + + + + + + @@ -45,7 +50,6 @@ - @@ -76,7 +80,6 @@ - @@ -106,9 +109,7 @@ - - - + From 55f94d052d70809efb91a9e4adbd42d65c847842 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 21 Dec 2022 17:03:50 +0900 Subject: [PATCH 27/37] feat(tier4_localization_launch): remove configs and move to autoware_launch (#2537) * feat(tier4_localization_launch): remove configs and move to autoware_launch Signed-off-by: kminoda * update readme Signed-off-by: kminoda * Update launch/tier4_localization_launch/README.md Co-authored-by: Yamato Ando * fix order Signed-off-by: kminoda * remove config Signed-off-by: kminoda * update readme Signed-off-by: kminoda * pre-commit Signed-off-by: kminoda Signed-off-by: kminoda Co-authored-by: Yamato Ando --- .../tier4_localization_launch/CMakeLists.txt | 1 - launch/tier4_localization_launch/README.md | 16 ++--- ...op_box_filter_measurement_range.param.yaml | 11 --- .../localization_error_monitor.param.yaml | 7 -- .../config/ndt_scan_matcher.param.yaml | 67 ------------------- .../random_downsample_filter.param.yaml | 3 - .../config/voxel_grid_filter.param.yaml | 5 -- .../launch/localization.launch.xml | 18 ++--- .../localization_error_monitor.launch.xml | 2 +- .../pose_estimator/pose_estimator.launch.xml | 3 +- .../launch/util/util.launch.py | 15 +---- .../launch/util/util.launch.xml | 2 - 12 files changed, 21 insertions(+), 129 deletions(-) delete mode 100644 launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml delete mode 100644 launch/tier4_localization_launch/config/localization_error_monitor.param.yaml delete mode 100644 launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml delete mode 100644 launch/tier4_localization_launch/config/random_downsample_filter.param.yaml delete mode 100644 launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml diff --git a/launch/tier4_localization_launch/CMakeLists.txt b/launch/tier4_localization_launch/CMakeLists.txt index 056fe87ca3ab8..e327b2e955acd 100644 --- a/launch/tier4_localization_launch/CMakeLists.txt +++ b/launch/tier4_localization_launch/CMakeLists.txt @@ -6,5 +6,4 @@ autoware_package() ament_auto_package(INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_localization_launch/README.md b/launch/tier4_localization_launch/README.md index ee7d6cab09364..a95980d193e93 100644 --- a/launch/tier4_localization_launch/README.md +++ b/launch/tier4_localization_launch/README.md @@ -10,17 +10,15 @@ Please see `` in `package.xml`. ## Usage -You can include as follows in `*.launch.xml` to use `localization.launch.xml`. +Include `localization.launch.xml` in other launch files as follows. + +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `localization.launch.xml`. ```xml + + + + ... ``` - -## Notes - -There are some `param.yaml` files in `config` directory. - -```bash -ndt_scan_matcher.param.yaml -``` diff --git a/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml b/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml deleted file mode 100644 index ad5542315410e..0000000000000 --- a/launch/tier4_localization_launch/config/crop_box_filter_measurement_range.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - input_frame: "base_link" - output_frame: "base_link" - min_x: -60.0 - max_x: 60.0 - min_y: -60.0 - max_y: 60.0 - min_z: -30.0 - max_z: 50.0 - negative: False diff --git a/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml b/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml deleted file mode 100644 index 026daf0532d33..0000000000000 --- a/launch/tier4_localization_launch/config/localization_error_monitor.param.yaml +++ /dev/null @@ -1,7 +0,0 @@ -/**: - ros__parameters: - scale: 3.0 - error_ellipse_size: 1.0 - warn_ellipse_size: 0.8 - error_ellipse_size_lateral_direction: 0.3 - warn_ellipse_size_lateral_direction: 0.2 diff --git a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml b/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml deleted file mode 100644 index 3b7f113c43a94..0000000000000 --- a/launch/tier4_localization_launch/config/ndt_scan_matcher.param.yaml +++ /dev/null @@ -1,67 +0,0 @@ -/**: - ros__parameters: - - # Vehicle reference frame - base_frame: "base_link" - - # Subscriber queue size - input_sensor_points_queue_size: 1 - - # The maximum difference between two consecutive - # transformations in order to consider convergence - trans_epsilon: 0.01 - - # The newton line search maximum step length - step_size: 0.1 - - # The ND voxel grid resolution - resolution: 2.0 - - # The number of iterations required to calculate alignment - max_iterations: 30 - - # Converged param type - # 0=TRANSFORM_PROBABILITY, 1=NEAREST_VOXEL_TRANSFORMATION_LIKELIHOOD - converged_param_type: 1 - - # If converged_param_type is 0 - # Threshold for deciding whether to trust the estimation result - converged_param_transform_probability: 3.0 - - # If converged_param_type is 1 - # Threshold for deciding whether to trust the estimation result - converged_param_nearest_voxel_transformation_likelihood: 2.3 - - # The number of particles to estimate initial pose - initial_estimate_particles_num: 100 - - # Tolerance of timestamp difference between initial_pose and sensor pointcloud. [sec] - initial_pose_timeout_sec: 1.0 - - # Tolerance of distance difference between two initial poses used for linear interpolation. [m] - initial_pose_distance_tolerance_m: 10.0 - - # neighborhood search method - # 0=KDTREE, 1=DIRECT26, 2=DIRECT7, 3=DIRECT1 - neighborhood_search_method: 0 - - # Number of threads used for parallel computing - num_threads: 4 - - # The covariance of output pose - # Do note that this covariance matrix is empirically derived - output_pose_covariance: - [ - 0.0225, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0225, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0225, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.000625, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.000625, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.000625, - ] - - # Regularization switch - regularization_enabled: false - - # regularization scale factor - regularization_scale_factor: 0.01 diff --git a/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml b/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml deleted file mode 100644 index 53be849e0af22..0000000000000 --- a/launch/tier4_localization_launch/config/random_downsample_filter.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - sample_num: 1500 diff --git a/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml b/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml deleted file mode 100644 index 51a7ee9d89b6b..0000000000000 --- a/launch/tier4_localization_launch/config/voxel_grid_filter.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - voxel_size_x: 3.0 - voxel_size_y: 3.0 - voxel_size_z: 3.0 diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 2883a21877888..14fe9c55f7e5d 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -1,11 +1,16 @@ + + + + + + + - - @@ -16,15 +21,12 @@ - - - - + @@ -38,9 +40,7 @@ - - - + diff --git a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml index 43cfc7403184d..4caefed02584a 100644 --- a/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml +++ b/launch/tier4_localization_launch/launch/localization_error_monitor/localization_error_monitor.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml index 750adca4c2fa0..e4b04d3d4a32e 100644 --- a/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml +++ b/launch/tier4_localization_launch/launch/pose_estimator/pose_estimator.launch.xml @@ -1,6 +1,5 @@ - @@ -10,7 +9,7 @@ - + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.py b/launch/tier4_localization_launch/launch/util/util.launch.py index 44186b9986618..dd58311afe2df 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.py +++ b/launch/tier4_localization_launch/launch/util/util.launch.py @@ -20,7 +20,6 @@ from launch.substitutions import LaunchConfiguration from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode -from launch_ros.substitutions import FindPackageShare import yaml @@ -94,32 +93,24 @@ def add_launch_arg(name: str, default_value=None, description=None): arg = DeclareLaunchArgument(name, default_value=default_value, description=description) launch_arguments.append(arg) - add_launch_arg( - "tier4_localization_launch_param_path", - [FindPackageShare("tier4_localization_launch"), "/config"], - "tier4_localization_launch param path", - ) add_launch_arg( "crop_box_filter_measurement_range_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/crop_box_filter_measurement_range.param.yaml", + LaunchConfiguration("crop_box_filter_measurement_range_param_path"), ], "path to the parameter file of crop_box_filter_measurement_range", ) add_launch_arg( "voxel_grid_downsample_filter_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/voxel_grid_filter.param.yaml", + LaunchConfiguration("voxel_grid_downsample_filter_param_path"), ], "path to the parameter file of voxel_grid_downsample_filter", ) add_launch_arg( "random_downsample_filter_param_path", [ - LaunchConfiguration("tier4_localization_launch_param_path"), - "/random_downsample_filter.param.yaml", + LaunchConfiguration("random_downsample_filter_param_path"), ], "path to the parameter file of random_downsample_filter", ) diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index 3070bc80bbc54..1dfe2e3d5f109 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -2,7 +2,6 @@ - @@ -29,7 +28,6 @@ - From 618746c0b0f324655741f6c9e874743817e7c0a2 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 21 Dec 2022 17:04:13 +0900 Subject: [PATCH 28/37] feat(tier4_map_launch): remove configs and move to autoware_launch (#2538) * feat(tier4_map_launch): remove configs and move to autoware_launch Signed-off-by: kminoda * update readme Signed-off-by: kminoda * fix readme Signed-off-by: kminoda * remove config Signed-off-by: kminoda * update readme Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_map_launch/CMakeLists.txt | 1 - launch/tier4_map_launch/README.md | 7 +++++++ .../config/pointcloud_map_loader.param.yaml | 9 --------- launch/tier4_map_launch/launch/map.launch.py | 5 +---- launch/tier4_map_launch/launch/map.launch.xml | 4 +++- 5 files changed, 11 insertions(+), 15 deletions(-) delete mode 100644 launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index b0e770d7c7619..98f12b64f3ca4 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -7,5 +7,4 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch - config ) diff --git a/launch/tier4_map_launch/README.md b/launch/tier4_map_launch/README.md index 344a56a0f9947..5e98593a9bec0 100644 --- a/launch/tier4_map_launch/README.md +++ b/launch/tier4_map_launch/README.md @@ -12,6 +12,8 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `map.launch.py`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `map.launch.xml`. + ```xml @@ -20,6 +22,11 @@ You can include as follows in `*.launch.xml` to use `map.launch.py`. + + + + + ... ``` diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml deleted file mode 100644 index 8f3ccbff00360..0000000000000 --- a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml +++ /dev/null @@ -1,9 +0,0 @@ -/**: - ros__parameters: - enable_whole_load: true - enable_downsampled_whole_load: false - enable_partial_load: false - enable_differential_load: false - - # only used when downsample_whole_load enabled - leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index bf396921fcb7a..231a805603a44 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -161,10 +161,7 @@ def add_launch_arg(name: str, default_value=None, description=None): ), add_launch_arg( "pointcloud_map_loader_param_path", - [ - FindPackageShare("tier4_map_launch"), - "/config/pointcloud_map_loader.param.yaml", - ], + "", "path to pointcloud_map_loader param file", ), add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 8fe3f141a0dbd..7ecc61605eeac 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,8 +1,10 @@ + + + - From db178a92e5a1a23ad7dba1b061368169e34f7f1a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Wed, 21 Dec 2022 17:08:40 +0900 Subject: [PATCH 29/37] feat(tier4_control_launch): remove configs and move to autoware_launch (#2544) * feat(tier4_control_launch): remove configs and move to autoware_launch Signed-off-by: Takayuki Murooka * remove config Signed-off-by: Takayuki Murooka * Update launch/tier4_control_launch/README.md Signed-off-by: Takayuki Murooka Signed-off-by: kminoda Co-authored-by: kminoda <44218668+kminoda@users.noreply.github.com> Co-authored-by: kminoda --- launch/tier4_control_launch/CMakeLists.txt | 1 - launch/tier4_control_launch/README.md | 6 + .../config/common/nearest_search.param.yaml | 5 - .../obstacle_collision_checker.param.yaml | 11 -- ...eration_mode_transition_manager.param.yaml | 22 --- .../shift_decider/shift_decider.param.yaml | 3 - .../lateral_controller.param.yaml | 77 ---------- .../longitudinal_controller.param.yaml | 86 ------------ .../vehicle_cmd_gate.param.yaml | 20 --- .../launch/control.launch.py | 132 +++++------------- 10 files changed, 39 insertions(+), 324 deletions(-) delete mode 100644 launch/tier4_control_launch/config/common/nearest_search.param.yaml delete mode 100644 launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml delete mode 100644 launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml delete mode 100644 launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml delete mode 100644 launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml delete mode 100644 launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml delete mode 100644 launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml diff --git a/launch/tier4_control_launch/CMakeLists.txt b/launch/tier4_control_launch/CMakeLists.txt index 06862431da1fd..1c4bccd83f619 100644 --- a/launch/tier4_control_launch/CMakeLists.txt +++ b/launch/tier4_control_launch/CMakeLists.txt @@ -6,6 +6,5 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE - config launch ) diff --git a/launch/tier4_control_launch/README.md b/launch/tier4_control_launch/README.md index 919258cfc60dc..2d2a6487986bd 100644 --- a/launch/tier4_control_launch/README.md +++ b/launch/tier4_control_launch/README.md @@ -12,9 +12,15 @@ Please see `` in `package.xml`. You can include as follows in `*.launch.xml` to use `control.launch.py`. +Note that you should provide parameter paths as `PACKAGE_param_path`. The list of parameter paths you should provide is written at the top of `planning.launch.xml`. + ```xml + + + + ... ``` diff --git a/launch/tier4_control_launch/config/common/nearest_search.param.yaml b/launch/tier4_control_launch/config/common/nearest_search.param.yaml deleted file mode 100644 index eb6709764ce3e..0000000000000 --- a/launch/tier4_control_launch/config/common/nearest_search.param.yaml +++ /dev/null @@ -1,5 +0,0 @@ -/**: - ros__parameters: - # ego - ego_nearest_dist_threshold: 3.0 # [m] - ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg] diff --git a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml b/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml deleted file mode 100644 index e3c78c90e2ed1..0000000000000 --- a/launch/tier4_control_launch/config/obstacle_collision_checker/obstacle_collision_checker.param.yaml +++ /dev/null @@ -1,11 +0,0 @@ -/**: - ros__parameters: - # Node - update_rate: 10.0 - - # Core - delay_time: 0.03 # delay time of vehicle [s] - footprint_margin: 0.0 # margin for footprint [m] - max_deceleration: 1.5 # max deceleration [m/ss] - resample_interval: 0.3 # interval distance to resample point cloud [m] - search_radius: 5.0 # search distance from trajectory to point cloud [m] diff --git a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml b/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml deleted file mode 100644 index a86443f5cabdb..0000000000000 --- a/launch/tier4_control_launch/config/operation_mode_transition_manager/operation_mode_transition_manager.param.yaml +++ /dev/null @@ -1,22 +0,0 @@ -/**: - ros__parameters: - transition_timeout: 10.0 - frequency_hz: 10.0 - check_engage_condition: false # set false if you do not want to care about the engage condition. - nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index - nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index - engage_acceptable_limits: - allow_autonomous_in_stopped: true # no check if the velocity is zero, always allowed. - dist_threshold: 1.5 - yaw_threshold: 0.524 - speed_upper_threshold: 10.0 - speed_lower_threshold: -10.0 - acc_threshold: 1.5 - lateral_acc_threshold: 1.0 - lateral_acc_diff_threshold: 0.5 - stable_check: - duration: 0.1 - dist_threshold: 1.5 - speed_upper_threshold: 2.0 - speed_lower_threshold: -2.0 - yaw_threshold: 0.262 diff --git a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml b/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml deleted file mode 100644 index 4c45b36223401..0000000000000 --- a/launch/tier4_control_launch/config/shift_decider/shift_decider.param.yaml +++ /dev/null @@ -1,3 +0,0 @@ -/**: - ros__parameters: - park_on_goal: true diff --git a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml deleted file mode 100644 index f3819f155edfe..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml +++ /dev/null @@ -1,77 +0,0 @@ -/**: - ros__parameters: - - # -- system -- - traj_resample_dist: 0.1 # path resampling interval [m] - use_steer_prediction: false # flag for using steer prediction (do not use steer measurement) - admissible_position_error: 5.0 # stop mpc calculation when error is larger than the following value - admissible_yaw_error_rad: 1.57 # stop mpc calculation when error is larger than the following value - - # -- path smoothing -- - enable_path_smoothing: false # flag for path smoothing - path_filter_moving_ave_num: 25 # param of moving average filter for path smoothing - curvature_smoothing_num_traj: 15 # point-to-point index distance used in curvature calculation (for trajectory): curvature is calculated from three points p(i-num), p(i), p(i+num) - curvature_smoothing_num_ref_steer: 15 # point-to-point index distance used in curvature calculation (for steer command reference): curvature is calculated from three points p(i-num), p(i), p(i+num) - - # -- trajectory extending -- - extend_trajectory_for_end_yaw_control: true # flag of trajectory extending for terminal yaw control - - # -- mpc optimization -- - qp_solver_type: "osqp" # optimization solver option (unconstraint_fast or osqp) - mpc_prediction_horizon: 50 # prediction horizon step - mpc_prediction_dt: 0.1 # prediction horizon period [s] - mpc_weight_lat_error: 0.1 # lateral error weight in matrix Q - mpc_weight_heading_error: 0.0 # heading error weight in matrix Q - mpc_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q - mpc_weight_steering_input: 1.0 # steering error weight in matrix R - mpc_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R - mpc_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R - mpc_weight_steer_rate: 0.0 # steering rate weight in matrix R - mpc_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R - mpc_low_curvature_weight_lat_error: 0.1 # lateral error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error: 0.0 # heading error weight in matrix Q in low curvature point - mpc_low_curvature_weight_heading_error_squared_vel: 0.3 # heading error * velocity weight in matrix Q in low curvature point - mpc_low_curvature_weight_steering_input: 1.0 # steering error weight in matrix R in low curvature point - mpc_low_curvature_weight_steering_input_squared_vel: 0.25 # steering error * velocity weight in matrix R in low curvature point - mpc_low_curvature_weight_lat_jerk: 0.0 # lateral jerk weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_rate: 0.0 # steering rate weight in matrix R in low curvature point - mpc_low_curvature_weight_steer_acc: 0.000001 # steering angular acceleration weight in matrix R in low curvature point - mpc_low_curvature_thresh_curvature: 0.0 # threshold of curvature to use "low_curvature" parameter (recommend: 0.01~0.03) - mpc_weight_terminal_lat_error: 1.0 # terminal lateral error weight in matrix Q to improve mpc stability - mpc_weight_terminal_heading_error: 0.1 # terminal heading error weight in matrix Q to improve mpc stability - mpc_zero_ff_steer_deg: 0.5 # threshold that feed-forward angle becomes zero - mpc_acceleration_limit: 2.0 # limit on the vehicle's acceleration - mpc_velocity_time_constant: 0.3 # time constant used for velocity smoothing - mpc_min_prediction_length: 5.0 # minimum prediction length - - # -- vehicle model -- - vehicle_model_type: "kinematics" # vehicle model type for mpc prediction. option is kinematics, kinematics_no_delay, and dynamics - input_delay: 0.24 # steering input delay time for delay compensation - vehicle_model_steer_tau: 0.3 # steering dynamics time constant (1d approximation) [s] - steer_rate_lim_dps: 40.0 # steering angle rate limit [deg/s] - acceleration_limit: 2.0 # acceleration limit for trajectory velocity modification [m/ss] - velocity_time_constant: 0.3 # velocity dynamics time constant for trajectory velocity modification [s] - - # -- lowpass filter for noise reduction -- - steering_lpf_cutoff_hz: 3.0 # cutoff frequency of lowpass filter for steering command [Hz] - error_deriv_lpf_cutoff_hz: 5.0 - - # stop state: steering command is kept in the previous value in the stop state. - stop_state_entry_ego_speed: 0.001 - stop_state_entry_target_speed: 0.001 - converged_steer_rad: 0.1 - keep_steer_control_until_converged: true - new_traj_duration_time: 1.0 - new_traj_end_dist: 0.3 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml b/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml deleted file mode 100644 index b6e1c3a38c799..0000000000000 --- a/launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml +++ /dev/null @@ -1,86 +0,0 @@ -/**: - ros__parameters: - delay_compensation_time: 0.17 - - enable_smooth_stop: true - enable_overshoot_emergency: true - enable_large_tracking_error_emergency: true - enable_slope_compensation: false - enable_keep_stopped_until_steer_convergence: true - - # state transition - drive_state_stop_dist: 0.5 - drive_state_offset_stop_dist: 1.0 - stopping_state_stop_dist: 0.49 - stopped_state_entry_duration_time: 0.1 - stopped_state_entry_vel: 0.1 - stopped_state_entry_acc: 0.1 - emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7 - - # drive state - kp: 1.0 - ki: 0.1 - kd: 0.0 - max_out: 1.0 - min_out: -1.0 - max_p_effort: 1.0 - min_p_effort: -1.0 - max_i_effort: 0.3 - min_i_effort: -0.3 - max_d_effort: 0.0 - min_d_effort: 0.0 - lpf_vel_error_gain: 0.9 - current_vel_threshold_pid_integration: 0.5 - enable_brake_keeping_before_stop: false - brake_keeping_acc: -0.2 - - # smooth stop state - smooth_stop_max_strong_acc: -0.5 - smooth_stop_min_strong_acc: -1.0 - smooth_stop_weak_acc: -0.3 - smooth_stop_weak_stop_acc: -0.8 - smooth_stop_strong_stop_acc: -3.4 - smooth_stop_max_fast_vel: 0.5 - smooth_stop_min_running_vel: 0.01 - smooth_stop_min_running_acc: 0.01 - smooth_stop_weak_stop_time: 0.8 - smooth_stop_weak_stop_dist: -0.3 - smooth_stop_strong_stop_dist: -0.5 - - # stopped state - stopped_vel: 0.0 - stopped_acc: -3.4 - stopped_jerk: -5.0 - - # emergency state - emergency_vel: 0.0 - emergency_acc: -5.0 - emergency_jerk: -3.0 - - # acceleration limit - max_acc: 3.0 - min_acc: -5.0 - - # jerk limit - max_jerk: 2.0 - min_jerk: -5.0 - - # pitch - use_trajectory_for_pitch_calculation: false - lpf_pitch_gain: 0.95 - max_pitch_rad: 0.1 - min_pitch_rad: -0.1 - - # vehicle parameters - vehicle: - cg_to_front_m: 1.228 - cg_to_rear_m: 1.5618 - mass_kg: 2400.0 - mass_fl: 600.0 - mass_fr: 600.0 - mass_rl: 600.0 - mass_rr: 600.0 - cf: 155494.663 - cr: 155494.663 diff --git a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml b/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml deleted file mode 100644 index 4876699351ffd..0000000000000 --- a/launch/tier4_control_launch/config/vehicle_cmd_gate/vehicle_cmd_gate.param.yaml +++ /dev/null @@ -1,20 +0,0 @@ -/**: - ros__parameters: - update_rate: 10.0 - system_emergency_heartbeat_timeout: 0.5 - external_emergency_stop_heartbeat_timeout: 0.0 - stop_hold_acceleration: -1.5 - emergency_acceleration: -2.4 - stopped_state_entry_duration_time: 0.1 - nominal: - vel_lim: 25.0 - lon_acc_lim: 5.0 - lon_jerk_lim: 5.0 - lat_acc_lim: 5.0 - lat_jerk_lim: 5.0 - on_transition: - vel_lim: 50.0 - lon_acc_lim: 1.0 - lon_jerk_lim: 0.5 - lat_acc_lim: 1.2 - lat_jerk_lim: 0.75 diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index 4cb4c54dd5ddb..d5e7323ba6d9a 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/launch/tier4_control_launch/launch/control.launch.py @@ -12,8 +12,6 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import launch from launch.actions import DeclareLaunchArgument from launch.actions import GroupAction @@ -33,71 +31,29 @@ def launch_setup(context, *args, **kwargs): - vehicle_info_param_path = LaunchConfiguration("vehicle_info_param_file").perform(context) - with open(vehicle_info_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f: vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lat_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "lateral_controller.param.yaml", - ) - with open(lat_controller_param_path, "r") as f: - lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - nearest_search_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "common", - "nearest_search.param.yaml", - ) - with open(nearest_search_param_path, "r") as f: + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] - lon_controller_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "trajectory_follower", - "longitudinal_controller.param.yaml", - ) - with open(lon_controller_param_path, "r") as f: + with open(LaunchConfiguration("lat_controller_param_path").perform(context), "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lon_controller_param_path").perform(context), "r") as f: lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - vehicle_cmd_gate_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "vehicle_cmd_gate", - "vehicle_cmd_gate.param.yaml", - ) - with open(vehicle_cmd_gate_param_path, "r") as f: + with open(LaunchConfiguration("vehicle_cmd_gate_param_path").perform(context), "r") as f: vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - lane_departure_checker_param_path = LaunchConfiguration( - "lane_departure_checker_param_path" - ).perform(context) - with open(lane_departure_checker_param_path, "r") as f: + with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - operation_mode_transition_manager_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "operation_mode_transition_manager", - "operation_mode_transition_manager.param.yaml", - ) - with open(operation_mode_transition_manager_param_path, "r") as f: + with open( + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + ) as f: operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - shift_decider_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "shift_decider", - "shift_decider.param.yaml", - ) - with open(shift_decider_param_path, "r") as f: + with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] - - obstacle_collision_checker_param_path = os.path.join( - LaunchConfiguration("tier4_control_launch_param_path").perform(context), - "obstacle_collision_checker", - "obstacle_collision_checker.param.yaml", - ) - - with open(obstacle_collision_checker_param_path, "r") as f: + with open( + LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" + ) as f: obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] controller_component = ComposableNode( @@ -235,7 +191,7 @@ def launch_setup(context, *args, **kwargs): ("control_mode_request", "/control/control_mode_request"), ], parameters=[ - nearest_search_param_path, + nearest_search_param, operation_mode_transition_manager_param, vehicle_info_param, ], @@ -328,46 +284,24 @@ def add_launch_arg(name: str, default_value=None, description=None): DeclareLaunchArgument(name, default_value=default_value, description=description) ) - # parameter - add_launch_arg( - "tier4_control_launch_param_path", - [ - FindPackageShare("tier4_control_launch"), - "/config", - ], - "tier4_control_launch parameter path", - ) - - # lateral controller - add_launch_arg( - "lateral_controller_mode", - "mpc_follower", - "lateral controller mode: `mpc_follower` or `pure_pursuit`", - ) - - # longitudinal controller mode - add_launch_arg( - "longitudinal_controller_mode", - "pid", - "longitudinal controller mode: `pid`", - ) - - add_launch_arg( - "vehicle_info_param_file", - [ - FindPackageShare("vehicle_info_util"), - "/config/vehicle_info.param.yaml", - ], - "path to the parameter file of vehicle information", - ) - - add_launch_arg( - "lane_departure_checker_param_path", - [FindPackageShare("lane_departure_checker"), "/config/lane_departure_checker.param.yaml"], - ) - - # obstacle collision checker - add_launch_arg("enable_obstacle_collision_checker", "false", "use obstacle collision checker") + # option + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_id") + add_launch_arg("enable_obstacle_collision_checker") + add_launch_arg("check_external_emergency_heartbeat") + add_launch_arg("lateral_controller_mode") + add_launch_arg("longitudinal_controller_mode") + # common param path + add_launch_arg("nearest_search_param_path") + # package param path + add_launch_arg("lat_controller_param_path") + add_launch_arg("lon_controller_param_path") + add_launch_arg("vehicle_cmd_gate_param_path") + add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("operation_mode_transition_manager_param_path") + add_launch_arg("shift_decider_param_path") + add_launch_arg("obstacle_collision_checker_param_path") # velocity controller add_launch_arg("show_debug_info", "false", "show debug information") From cc33997f75076a85d68838a31a95c82ce9cab1ef Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Wed, 21 Dec 2022 17:39:02 +0900 Subject: [PATCH 30/37] feat(tier4_simulator_launch): remove configs and move to autoware_launch (#2541) * feat(tier4_perception_launch): remove configs and move to autoware_launch Signed-off-by: kminoda * update readme Signed-off-by: kminoda * first commit Signed-off-by: kminoda * remove config Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_simulator_launch/CMakeLists.txt | 1 - .../config/fault_injection.param.yaml | 37 ------------------- .../launch/simulator.launch.xml | 9 +++-- 3 files changed, 5 insertions(+), 42 deletions(-) delete mode 100644 launch/tier4_simulator_launch/config/fault_injection.param.yaml diff --git a/launch/tier4_simulator_launch/CMakeLists.txt b/launch/tier4_simulator_launch/CMakeLists.txt index 6817e31c4876e..8a670fdefa19c 100644 --- a/launch/tier4_simulator_launch/CMakeLists.txt +++ b/launch/tier4_simulator_launch/CMakeLists.txt @@ -5,6 +5,5 @@ find_package(autoware_cmake REQUIRED) autoware_package() ament_auto_package(INSTALL_TO_SHARE - config launch ) diff --git a/launch/tier4_simulator_launch/config/fault_injection.param.yaml b/launch/tier4_simulator_launch/config/fault_injection.param.yaml deleted file mode 100644 index 1a57b852f7361..0000000000000 --- a/launch/tier4_simulator_launch/config/fault_injection.param.yaml +++ /dev/null @@ -1,37 +0,0 @@ -/**: - ros__parameters: - event_diag_list: - vehicle_is_out_of_lane: "lane_departure" - trajectory_deviation_is_high: "trajectory_deviation" - localization_matching_score_is_low: "ndt_scan_matcher" - localization_accuracy_is_low: "localization_accuracy" - map_version_is_different: "map_version" - trajectory_is_invalid: "trajectory_point_validation" - cpu_temperature_is_high: "CPU Temperature" - cpu_usage_is_high: "CPU Usage" - cpu_is_in_thermal_throttling: "CPU Thermal Throttling" - storage_temperature_is_high: "HDD Temperature" - storage_usage_is_high: "HDD Usage" - network_usage_is_high: "Network Usage" - clock_error_is_large: "NTP Offset" - gpu_temperature_is_high: "GPU Temperature" - gpu_usage_is_high: "GPU Usage" - gpu_memory_usage_is_high: "GPU Memory Usage" - gpu_is_in_thermal_throttling: "GPU Thermal Throttling" - driving_recorder_storage_error: "driving_recorder" - debug_data_logger_storage_error: "bagpacker" - emergency_stop_operation: "emergency_stop_operation" - vehicle_error_occurred: "vehicle_errors" - vehicle_ecu_connection_is_lost: "can_bus_connection" - obstacle_crash_sensor_is_activated: "obstacle_crash" - /control/command_gate/node_alive_monitoring: "vehicle_cmd_gate: heartbeat" - /control/autonomous_driving/node_alive_monitoring: "control_topic_status" - /control/external_command_selector/node_alive_monitoring: "external_cmd_selector: heartbeat" - /localization/node_alive_monitoring: "localization_topic_status" - /map/node_alive_monitoring: "map_topic_status" - /planning/node_alive_monitoring: "planning_topic_status" - /sensing/lidar/node_alive_monitoring: "lidar_topic_status" - /sensing/imu/node_alive_monitoring: "imu_connection" - /sensing/gnss/node_alive_monitoring: "gnss_connection" - /system/node_alive_monitoring: "system_topic_status" - /vehicle/node_alive_monitoring: "vehicle_topic_status" diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8705ba30a9350..d48485bc3ff06 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -1,6 +1,7 @@ - - + + + @@ -20,7 +21,7 @@ - + @@ -80,7 +81,7 @@ - + From 9376697f5d8fb8415975ec24e7658d63bcad549a Mon Sep 17 00:00:00 2001 From: ito-san <57388357+ito-san@users.noreply.github.com> Date: Wed, 21 Dec 2022 23:25:11 +0900 Subject: [PATCH 31/37] fix(system_monitor): prevent nethogs from monitoring all networks due to high CPU load (#2474) * fix(system_monitor): prevent nethogs from monitoring all networks due to high CPU load Signed-off-by: ito-san * ci(pre-commit): autofix * fix(system_monitor): fix include guards Signed-off-by: ito-san * fix(system_monitor): fix build error Signed-off-by: ito-san * fix(net_monitor): change lower camel case to snake case Signed-off-by: ito-san * fix(net_monitor): fix clang-tidy errors and warnings Signed-off-by: ito-san * ci(pre-commit): autofix * fix(net_monitor): fix clang-tidy warnings Signed-off-by: ito-san * ci(pre-commit): autofix * fix(net_monitor: fix clang-tidy warnings) Signed-off-by: ito-san * fix(net_monitor): fix clang-tidy warnings Signed-off-by: ito-san * fix(net_monitor): change C-style socket to boost::asio Signed-off-by: ito-san * fix(net_monitor): fix clang-tidy warnings Signed-off-by: ito-san * fix(net_monitor): fix clang-tidy warnings Signed-off-by: ito-san * fix(net_monitor): first refactoring Signed-off-by: ito-san * fix(net_monitor): refactoring Signed-off-by: ito-san * fix(net_monitor): fix clang-tidy errors Signed-off-by: ito-san * fix(net_monitor): update README Signed-off-by: ito-san * fix(net_monitor): add lock guard to protect variable Signed-off-by: ito-san Signed-off-by: ito-san Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/system_monitor/CMakeLists.txt | 3 +- system/system_monitor/README.md | 3 +- .../config/net_monitor.param.yaml | 1 - system/system_monitor/docs/ros_parameters.md | 16 +- .../system_monitor/docs/topics_net_monitor.md | 53 +- .../net_monitor/net_monitor.hpp | 329 ++++--- ...c_reader.hpp => traffic_reader_common.hpp} | 34 +- .../traffic_reader/traffic_reader_service.hpp | 110 +++ .../reader/traffic_reader/traffic_reader.cpp | 351 ------- .../traffic_reader/traffic_reader_main.cpp | 89 ++ .../traffic_reader/traffic_reader_service.cpp | 288 ++++++ .../src/net_monitor/net_monitor.cpp | 883 ++++++++++-------- 12 files changed, 1252 insertions(+), 908 deletions(-) rename system/system_monitor/include/traffic_reader/{traffic_reader.hpp => traffic_reader_common.hpp} (63%) create mode 100644 system/system_monitor/include/traffic_reader/traffic_reader_service.hpp delete mode 100644 system/system_monitor/reader/traffic_reader/traffic_reader.cpp create mode 100644 system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp create mode 100644 system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp diff --git a/system/system_monitor/CMakeLists.txt b/system/system_monitor/CMakeLists.txt index 97eddcb590a5f..391c82a09c960 100644 --- a/system/system_monitor/CMakeLists.txt +++ b/system/system_monitor/CMakeLists.txt @@ -118,7 +118,8 @@ ament_auto_add_executable(hdd_reader ) ament_auto_add_executable(traffic_reader - reader/traffic_reader/traffic_reader.cpp + reader/traffic_reader/traffic_reader_main.cpp + reader/traffic_reader/traffic_reader_service.cpp ) find_library(NL3 nl-3 REQUIRED) diff --git a/system/system_monitor/README.md b/system/system_monitor/README.md index 87107d3937a82..6203ad45d4b3a 100644 --- a/system/system_monitor/README.md +++ b/system/system_monitor/README.md @@ -73,7 +73,8 @@ Every topic is published in 1 minute interval. | | HDD WriteIOPS | ✓ | ✓ | ✓ | | | | HDD Connection | ✓ | ✓ | ✓ | | | Memory Monitor | Memory Usage | ✓ | ✓ | ✓ | | -| Net Monitor | Network Usage | ✓ | ✓ | ✓ | | +| Net Monitor | Network Connection | ✓ | ✓ | ✓ | | +| | Network Usage | ✓ | ✓ | ✓ | Notification of usage only, normally error not generated. | | | Network CRC Error | ✓ | ✓ | ✓ | Warning occurs when the number of CRC errors in the period reaches the threshold value. The number of CRC errors that occur is the same as the value that can be confirmed with the ip command. | | | IP Packet Reassembles Failed | ✓ | ✓ | ✓ | | | NTP Monitor | NTP Offset | ✓ | ✓ | ✓ | | diff --git a/system/system_monitor/config/net_monitor.param.yaml b/system/system_monitor/config/net_monitor.param.yaml index cb7e1b48380a3..f2c68adda2efe 100644 --- a/system/system_monitor/config/net_monitor.param.yaml +++ b/system/system_monitor/config/net_monitor.param.yaml @@ -1,7 +1,6 @@ /**: ros__parameters: devices: ["*"] - traffic_reader_port: 7636 monitor_program: "greengrass" crc_error_check_duration: 1 crc_error_count_threshold: 1 diff --git a/system/system_monitor/docs/ros_parameters.md b/system/system_monitor/docs/ros_parameters.md index bba2c3fb40408..1d081d513c85c 100644 --- a/system/system_monitor/docs/ros_parameters.md +++ b/system/system_monitor/docs/ros_parameters.md @@ -61,14 +61,14 @@ mem_monitor: net_monitor: -| Name | Type | Unit | Default | Notes | -| :-------------------------------- | :----------: | :-----: | :-----: | :--------------------------------------------------------------------------------------------------------------------------------------------------- | -| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | -| usage_warn | float | %(1e-2) | 0.95 | Generates warning when network usage reaches a specified value or higher. | -| crc_error_check_duration | int | sec | 1 | CRC error check duration. | -| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | -| reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | -| reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | +| Name | Type | Unit | Default | Notes | +| :-------------------------------- | :----------: | :--: | :--------: | :--------------------------------------------------------------------------------------------------------------------------------------------------- | +| devices | list[string] | n/a | none | The name of network interface to monitor. (e.g. eth0, \* for all network interfaces) | +| monitor_program | string | n/a | greengrass | program name to be monitored by nethogs name. | +| crc_error_check_duration | int | sec | 1 | CRC error check duration. | +| crc_error_count_threshold | int | n/a | 1 | Generates warning when count of CRC errors during CRC error check duration reaches a specified value or higher. | +| reassembles_failed_check_duration | int | sec | 1 | IP packet reassembles failed check duration. | +| reassembles_failed_check_count | int | n/a | 1 | Generates warning when count of IP packet reassembles failed during IP packet reassembles failed check duration reaches a specified value or higher. | ## NTP Monitor diff --git a/system/system_monitor/docs/topics_net_monitor.md b/system/system_monitor/docs/topics_net_monitor.md index d223b359cdb08..edb067c0a5be7 100644 --- a/system/system_monitor/docs/topics_net_monitor.md +++ b/system/system_monitor/docs/topics_net_monitor.md @@ -1,5 +1,23 @@ # ROS topics: Net Monitor +## Network Connection + +/diagnostics/net_monitor: Network Connection + +[summary] + +| level | message | +| ----- | -------------- | +| OK | OK | +| WARN | no such device | + +[values] + +| key | value (example) | +| --------------------- | ------------------- | +| Network [0-9]: status | OK / no such device | +| HDD [0-9]: name | wlp82s0 | + ## Network Usage /diagnostics/net_monitor: Network Usage @@ -38,31 +56,19 @@ | ----- | ------- | | OK | OK | -[values] program +[values when specified program is detected] | key | value (example) | | -------------------------------- | ------------------------------------------- | -| nethogs [0-9]: PROGRAM | /lambda/greengrassSystemComponents/1384/999 | -| nethogs [0-9]: SENT (KB/Sec) | 1.13574 | -| nethogs [0-9]: RECEIVED (KB/Sec) | 0.261914 | - -[values] all - -| key | value (example) | -| --------------------- | -------------------------------------------------------------- | -| nethogs: all (KB/Sec) | python3.7/1520/999 0.274414 0.354883 | -| | /lambda/greengrassSystemComponents/1299/999 0.487305 0.0966797 | -| | sshd: muser@pts/5/15917/1002 0.396094 0.0585938 | -| | /usr/bin/python3.7/2371/999 0 0 | -| | /greengrass/ggc/packages/1.10.0/bin/daemon/906/0 0 0 | -| | python3.7/4362/999 0 0 | -| | unknown TCP/0/0 0 0 | +| nethogs [0-9]: program | /lambda/greengrassSystemComponents/1384/999 | +| nethogs [0-9]: sent (KB/Sec) | 1.13574 | +| nethogs [0-9]: received (KB/Sec) | 0.261914 | -[values] error +[values when error is occurring] -| key | value (example) | -| ----- | ----------------------------------------------------- | -| error | [nethogs -t] execve failed: No such file or directory | +| key | value (example) | +| ----- | ---------------------------------------- | +| error | execve failed: No such file or directory | ## Network CRC Error @@ -70,9 +76,10 @@ [summary] -| level | message | -| ----- | ------- | -| OK | OK | +| level | message | +| ----- | --------- | +| OK | OK | +| WARN | CRC error | [values] diff --git a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp index 3ddb078fa00e1..ded9c398c6cd1 100644 --- a/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp +++ b/system/system_monitor/include/system_monitor/net_monitor/net_monitor.hpp @@ -21,207 +21,290 @@ #define SYSTEM_MONITOR__NET_MONITOR__NET_MONITOR_HPP_ #include "system_monitor/net_monitor/nl80211.hpp" +#include "traffic_reader/traffic_reader_common.hpp" #include +#include + #include #include #include +#include #include #include -#define toMbit(X) (static_cast(X) / 1000000 * 8) +template +constexpr auto to_mbit(T value) +{ + return (static_cast(value) / 1000000 * 8); +} + +/** + * @brief Network information + */ +struct NetworkInfomation +{ + bool is_invalid; //!< @brief Valid network to be checked + int mtu_error_code; //!< @brief Error code set by ioctl() with SIOCGIFMTU + int ethtool_error_code; //!< @brief Error code set by ioctl() with SIOCETHTOOL + bool is_running; //!< @brief Resource allocated flag + std::string interface_name; //!< @brief Interface name + double speed; //!< @brief Network capacity + int mtu; //!< @brief MTU + double rx_traffic; //!< @brief Traffic received + double tx_traffic; //!< @brief Traffic transmitted + double rx_usage; //!< @brief Network capacity usage rate received + double tx_usage; //!< @brief Network capacity usage rate transmitted + unsigned int rx_bytes; //!< @brief Total bytes received + unsigned int rx_errors; //!< @brief Bad packets received + unsigned int tx_bytes; //!< @brief Total bytes transmitted + unsigned int tx_errors; //!< @brief Packet transmit problems + unsigned int collisions; //!< @brief Number of collisions during packet transmissions +}; /** * @brief Bytes information */ -typedef struct bytes +struct Bytes { - unsigned int rx_bytes; //!< @brief total bytes received - unsigned int tx_bytes; //!< @brief total bytes transmitted + unsigned int rx_bytes; //!< @brief Total bytes received + unsigned int tx_bytes; //!< @brief Total bytes transmitted +}; - bytes() : rx_bytes(0), tx_bytes(0) {} -} bytes; +/** + * @brief CRC errors information + */ +struct CrcErrors +{ + std::deque errors_queue{}; //!< @brief queue that holds count of CRC errors + unsigned int last_rx_crc_errors{0}; //!< @brief rx_crc_error at the time of the last monitoring +}; + +namespace local = boost::asio::local; class NetMonitor : public rclcpp::Node { public: /** - * @brief constructor + * @brief Constructor * @param [in] options Options associated with this node. */ explicit NetMonitor(const rclcpp::NodeOptions & options); + /** - * @brief destructor + * @brief Destructor */ - ~NetMonitor(); + ~NetMonitor() override; /** - * @brief Shutdown nl80211 object + * @brief Copy constructor */ - void shutdown_nl80211(); + NetMonitor(const NetMonitor &) = delete; + + /** + * @brief Copy assignment operator + */ + NetMonitor & operator=(const NetMonitor &) = delete; + + /** + * @brief Move constructor + */ + NetMonitor(const NetMonitor &&) = delete; + + /** + * @brief Move assignment operator + */ + NetMonitor & operator=(const NetMonitor &&) = delete; protected: using DiagStatus = diagnostic_msgs::msg::DiagnosticStatus; /** - * @brief check CPU usage - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Check network connection + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_connection(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Check network usage + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void check_usage(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Monitor network traffic + * @param [out] status diagnostic message passed directly to diagnostic publish calls + */ + void monitor_traffic(diagnostic_updater::DiagnosticStatusWrapper & status); + + /** + * @brief Check CRC error + * @param [out] status diagnostic message passed directly to diagnostic publish calls */ - void checkUsage( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void check_crc_error(diagnostic_updater::DiagnosticStatusWrapper & status); /** - * @brief monitor traffic - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Check IP packet reassembles failed + * @param [out] status diagnostic message passed directly to diagnostic publish calls */ - void monitorTraffic( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status); /** - * @brief check CRC error - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Timer callback */ - void checkCrcError( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + void on_timer(); /** - * @brief check IP packet reassembles failed - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @note NOLINT syntax is needed since diagnostic_updater asks for a non-const reference - * to pass diagnostic message updated in this function to diagnostic publish calls. + * @brief Determine if it is a supported network + * @param [in] network Network infomation + * @param [in] index Index of network infomation index + * @param [out] status Diagnostic message passed directly to diagnostic publish calls + * @param [out] error_message Error message */ - void checkReassemblesFailed( - diagnostic_updater::DiagnosticStatusWrapper & stat); // NOLINT(runtime/references) + static void make_invalid_diagnostic_status( + const NetworkInfomation & network, int index, + diagnostic_updater::DiagnosticStatusWrapper & status, std::string & error_message); /** - * @brief get wireless speed - * @param [in] ifa_name interface name - * @return wireless speed + * @brief Update list of network information */ - float getWirelessSpeed(const char * ifa_name); + void update_network_list(); /** - * @brief timer callback + * @brief Update network information by using socket + * @param [out] network Network information */ - void onTimer(); + void update_network_information_by_socket(NetworkInfomation & network); /** - * @brief update Network information list + * @brief Update network information about MTU + * @param [out] network Network information + * @param [in] socket File descriptor to socket */ - void updateNetworkInfoList(); + static void update_mtu(NetworkInfomation & network, int socket); /** - * @brief check NetMonitor General Infomation - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @return check result + * @brief Update network information about network capacity + * @param [out] network Network information + * @param [in] socket File descriptor to socket */ - bool checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat); + void update_network_capacity(NetworkInfomation & network, int socket); /** - * @brief Network information + * @brief Update network information by using routing netlink stats + * @param [out] network Network information + * @param [in] data Pointer to routing netlink stats + * @param [in] duration Time from previous measurement */ - struct NetworkInfo - { - int mtu_errno; //!< @brief errno set by ioctl() with SIOCGIFMTU - int ethtool_errno; //!< @brief errno set by ioctl() with SIOCETHTOOL - bool is_running; //!< @brief resource allocated flag - std::string interface_name; //!< @brief interface name - float speed; //!< @brief network capacity - int mtu; //!< @brief MTU - float rx_traffic; //!< @brief traffic received - float tx_traffic; //!< @brief traffic transmitted - float rx_usage; //!< @brief network capacity usage rate received - float tx_usage; //!< @brief network capacity usage rate transmitted - unsigned int rx_bytes; //!< @brief total bytes received - unsigned int rx_errors; //!< @brief bad packets received - unsigned int tx_bytes; //!< @brief total bytes transmitted - unsigned int tx_errors; //!< @brief packet transmit problems - unsigned int collisions; //!< @brief number of collisions during packet transmissions + void update_network_information_by_routing_netlink( + NetworkInfomation & network, void * data, const rclcpp::Duration & duration); - NetworkInfo() - : mtu_errno(0), - ethtool_errno(0), - is_running(false), - interface_name(""), - speed(0.0), - mtu(0), - rx_traffic(0.0), - tx_traffic(0.0), - rx_usage(0.0), - tx_usage(0.0), - rx_bytes(0), - rx_errors(0), - tx_bytes(0), - tx_errors(0), - collisions(0) - { - } - }; + /** + * @brief Update network information about network traffic + * @param [out] network Network information + * @param [in] stats Pointer to routing netlink stats + * @param [in] duration Time from previous measurement + */ + void update_traffic( + NetworkInfomation & network, const struct rtnl_link_stats * stats, + const rclcpp::Duration & duration); + + /** + * @brief Update network information about CRC error + * @param [out] network Network information + * @param [in] stats Pointer to routing netlink stats + */ + void update_crc_error(NetworkInfomation & network, const struct rtnl_link_stats * stats); + + /** + * @brief Shutdown nl80211 object + */ + void shutdown_nl80211(); + + /** + * @brief Send request to start nethogs + */ + void send_start_nethogs_request(); + + /** + * @brief Get result of nethogs + * @param [out] result result of nethogs + */ + void get_nethogs_result(traffic_reader_service::Result & result); + + /** + * @brief Connect to traffic-reader service + * @return true on success, false on error + */ + bool connect_service(); + + /** + * @brief Send data to traffic-reader service + * @param [in] request Request to traffic-reader service + * @return true on success, false on error + */ + bool send_data(traffic_reader_service::Request request); /** - * @brief determine if it is a supported network - * @param [in] net_info network infomation - * @param [in] index index of network infomation index - * @param [out] stat diagnostic message passed directly to diagnostic publish calls - * @param [out] error_str error string - * @return result of determining whether it is a supported network + * @brief Send data to traffic-reader service with parameters + * @param [in] request Request to traffic-reader service + * @param [in] parameters List of parameters + * @param[in] program_name Filter by program name + * @return true on success, false on error */ - bool isSupportedNetwork( - const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, - std::string & error_str); + bool send_data_with_parameters( + traffic_reader_service::Request request, std::vector & parameters, + std::string & program_name); /** - * @brief search column index of IP packet reassembles failed in /proc/net/snmp + * @brief Receive data from traffic-reader service + * @param [out] result Status from traffic-reader service */ - void searchReassemblesFailedColumnIndex(); + void receive_data(traffic_reader_service::Result & result); + + /** + * @brief Close connection with traffic-reader service + */ + void close_connection(); + + /** + * @brief Get column index of IP packet reassembles failed from `/proc/net/snmp` + */ + void get_reassembles_failed_column_index(); /** * @brief get IP packet reassembles failed * @param [out] reassembles_failed IP packet reassembles failed * @return execution result */ - bool getReassemblesFailed(uint64_t & reassembles_failed); + bool get_reassembles_failed(uint64_t & reassembles_failed); diagnostic_updater::Updater updater_; //!< @brief Updater class which advertises to /diagnostics rclcpp::TimerBase::SharedPtr timer_; //!< @brief timer to get Network information - char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name - std::map bytes_; //!< @brief list of bytes - rclcpp::Time last_update_time_; //!< @brief last update time - std::vector device_params_; //!< @brief list of devices - NL80211 nl80211_; //!< @brief 802.11 netlink-based interface - int getifaddrs_errno_; //!< @brief errno set by getifaddrs() - std::vector net_info_list_; //!< @brief list of Network information + char hostname_[HOST_NAME_MAX + 1]; //!< @brief host name + std::map bytes_; //!< @brief list of bytes + rclcpp::Time last_update_time_; //!< @brief last update time + std::vector device_params_; //!< @brief list of devices + NL80211 nl80211_; //!< @brief 802.11 netlink-based interface + int getifaddrs_error_code_; //!< @brief Error code set by getifaddrs() + std::vector network_list_; //!< @brief List of Network information - /** - * @brief CRC errors information - */ - typedef struct crc_errors - { - std::deque errors_queue; //!< @brief queue that holds count of CRC errors - unsigned int last_rx_crc_errors; //!< @brief rx_crc_error at the time of the last monitoring + std::string monitor_program_; //!< @brief nethogs monitor program name + std::string socket_path_; //!< @brief Path of UNIX domain socket + boost::asio::io_service io_service_; //!< @brief Core I/O functionality + std::unique_ptr acceptor_; //!< @brief UNIX domain acceptor + std::unique_ptr socket_; //!< @brief UNIX domain socket - crc_errors() : last_rx_crc_errors(0) {} - } crc_errors; - std::map crc_errors_; //!< @brief list of CRC errors + std::map crc_errors_; //!< @brief list of CRC errors + unsigned int crc_error_check_duration_; //!< @brief CRC error check duration + unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold std::deque reassembles_failed_queue_; //!< @brief queue that holds count of IP packet reassembles failed uint64_t last_reassembles_failed_; //!< @brief IP packet reassembles failed at the time of the //!< last monitoring - - std::string monitor_program_; //!< @brief nethogs monitor program name - bool nethogs_all_; //!< @brief nethogs result all mode - int traffic_reader_port_; //!< @brief port number to connect to traffic_reader - unsigned int crc_error_check_duration_; //!< @brief CRC error check duration - unsigned int crc_error_count_threshold_; //!< @brief CRC error count threshold unsigned int reassembles_failed_check_duration_; //!< @brief IP packet reassembles failed check duration unsigned int @@ -229,10 +312,16 @@ class NetMonitor : public rclcpp::Node unsigned int reassembles_failed_column_index_; //!< @brief column index of IP Reassembles failed //!< in /proc/net/snmp + /** + * @brief Network connection status messages + */ + const std::map connection_messages_ = { + {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "no such device"}, {DiagStatus::ERROR, "unused"}}; + /** * @brief Network usage status messages */ - const std::map usage_dict_ = { + const std::map usage_messages_ = { {DiagStatus::OK, "OK"}, {DiagStatus::WARN, "high load"}, {DiagStatus::ERROR, "down"}}; }; diff --git a/system/system_monitor/include/traffic_reader/traffic_reader.hpp b/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp similarity index 63% rename from system/system_monitor/include/traffic_reader/traffic_reader.hpp rename to system/system_monitor/include/traffic_reader/traffic_reader_common.hpp index 7b0276dee4c07..9d84072a42fc3 100644 --- a/system/system_monitor/include/traffic_reader/traffic_reader.hpp +++ b/system/system_monitor/include/traffic_reader/traffic_reader_common.hpp @@ -17,21 +17,33 @@ * @brief traffic reader definitions */ -#ifndef TRAFFIC_READER__TRAFFIC_READER_HPP_ -#define TRAFFIC_READER__TRAFFIC_READER_HPP_ +#ifndef TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ +#define TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ #include #include +#include #include +namespace traffic_reader_service +{ + +static constexpr char socket_path[] = "/tmp/traffic_reader"; + +enum Request { + NONE = 0, + START_NETHOGS, + GET_RESULT, +}; + /** - * @brief traffic information + * @brief Result of nethogs */ -struct TrafficReaderResult +struct Result { - int error_code_; //!< @brief error code, 0 on success, otherwise error - std::string str_; //!< @brief nethogs result string + int error_code; //!< @brief Error code, 0 on success, otherwise error + std::string output; //!< @brief Result output of nethogs /** * @brief Load or save data members. @@ -43,13 +55,13 @@ struct TrafficReaderResult template void serialize(archive & ar, const unsigned /*version*/) // NOLINT(runtime/references) { - ar & error_code_; - ar & str_; + ar & error_code; + ar & output; } }; -constexpr std::string_view GET_ALL_STR{""}; //!< @brief nethogs result all request string +// constexpr std::string_view GET_ALL_STR{""}; //!< @brief nethogs result all request string -constexpr int TRAFFIC_READER_PORT = 7636; //!< @brief traffic reader port: 7634-7647 Unassigned +} // namespace traffic_reader_service -#endif // TRAFFIC_READER__TRAFFIC_READER_HPP_ +#endif // TRAFFIC_READER__TRAFFIC_READER_COMMON_HPP_ diff --git a/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp b/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp new file mode 100644 index 0000000000000..a66d0b121ed46 --- /dev/null +++ b/system/system_monitor/include/traffic_reader/traffic_reader_service.hpp @@ -0,0 +1,110 @@ +// Copyright 2022 The Autoware Contributors +// +// 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 TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ +#define TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ + +#include "traffic_reader/traffic_reader_common.hpp" + +#include +#include + +#include +#include +#include +#include +#include + +namespace traffic_reader_service +{ + +namespace local = boost::asio::local; + +class TrafficReaderService +{ +public: + /** + * @brief Constructor + * @param[in] socket_path Path of UNIX domain socket + */ + explicit TrafficReaderService(std::string socket_path); + + /** + * @brief Initialization + * @return true on success, false on error + */ + bool initialize(); + + /** + * @brief Shutdown + */ + void shutdown(); + + /** + * @brief Main loop + */ + void run(); + +protected: + /** + * @brief Handle message + * @param[in] buffer Pointer to data received + */ + void handle_message(const char * buffer); + + /** + * @brief Start nethogs + * @param[in] archive Archive object for loading + */ + void start_nethogs(boost::archive::text_iarchive & archive); + + /** + * @brief Get command line of process from nethogs output + * @param[in] line nethogs output + * @return Command line of process + */ + static std::string get_command_line(const std::string & line); + + /** + * @brief Get command line of process from PID + * @param[in] pid PID + * @return Command line of process + */ + static std::string get_command_line_with_pid(pid_t pid); + + /** + * @brief Return result of nethogs + */ + void get_result(); + + /** + * @brief Execute nethogs + */ + void execute_nethogs(); + + std::string socket_path_; //!< @brief Path of UNIX domain socket + boost::asio::io_service io_service_; //!< @brief Core I/O functionality + std::unique_ptr acceptor_; //!< @brief UNIX domain acceptor + std::unique_ptr socket_; //!< @brief UNIX domain socket + std::thread thread_; //!< @brief Thread to run nethogs + std::mutex mutex_; //!< @brief Mutex guard for the flag + bool stop_; //!< @brief Flag to stop thread + std::vector devices_; //!< @brief List of devices + std::string program_name_; //!< @brief Filter by program name + traffic_reader_service::Result result_; //!< @brief Result of nethogs +}; + +} // namespace traffic_reader_service + +#endif // TRAFFIC_READER__TRAFFIC_READER_SERVICE_HPP_ diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader.cpp deleted file mode 100644 index ebb6d976d9921..0000000000000 --- a/system/system_monitor/reader/traffic_reader/traffic_reader.cpp +++ /dev/null @@ -1,351 +0,0 @@ -// Copyright 2021 Autoware Foundation -// -// 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. - -/** - * @file traffic_reader.cpp - * @brief traffic information read class - */ - -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace bp = boost::process; - -/** - * @brief thread safe nethogs result - */ -class NethogsResult -{ -public: - /** - * @brief get nethogs result string - * @param [out] str nethogs result string - * @return error code - */ - int get(std::string & str) - { - std::lock_guard lock(mutex_); - str = str_; - return err_; - } - /** - * @brief set nethogs result string and error code - * @param [in] str nethogs result string - * @param [in] err error code - * @return error code - */ - void set(const std::string & str, int err) - { - std::lock_guard lock(mutex_); - str_ = str; - err_ = err; - } - -private: - std::mutex mutex_; //!< @brief lock for thread safe - std::string str_; //!< @brief nethogs result string - int err_; //!< @brief error code, 0 on success, otherwise error -}; -static NethogsResult g_NethogsString; //!< @brief nethogs result -static volatile bool g_Running = false; //!< @brief run status, true running, false exit -static volatile int g_WatchDogCount = 0; //!< @brief executeNethogs watchdog, 0 refresh - -/** - * @brief print usage - */ -void usage() -{ - printf("Usage: traffic_reader [options]\n"); - printf(" -h --help : Display help\n"); - printf(" -p --port # : Port number to listen to.\n"); - printf("\n"); -} - -/** - * @brief thread function to execute nethogs - */ -void executeNethogs() -{ - const std::string cmd = "nethogs -t"; - // Output format of `nethogs -t` - // Blank Line | - // Start Line | Refreshing: - // Result Line 1 | PROGRAM/PID/UID SENT RECEIVED - // ... | ... - // Result Line n | PROGRAM/PID/UID SENT RECEIVED - // Last Line | unknown TCP/0/0 - std::string error_str; - std::string log_str; - - g_WatchDogCount = 0; - - while (g_Running) { - bp::ipstream is_out; - bp::ipstream is_err; - boost::process::child c; - try { - c = bp::child(cmd, bp::std_out > is_out, bp::std_err > is_err); - std::string result_buf; - std::string line; - while (std::getline(is_out, line)) { - if (!g_Running) { - // run() exit - return; - } - if (line.empty()) { - // Blank Line - if (!result_buf.empty()) { - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } - } else if (line.find("unknown TCP/0/0") != std::string::npos) { - // Last Line - result_buf.append(line); - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } else if (line.find("/0/0\t") != std::string::npos) { - // no-pid and root data skip. - } else if (line == std::string("Refreshing:")) { - // Start Line - g_WatchDogCount = 0; - if (!result_buf.empty()) { - g_NethogsString.set(result_buf, 0); - result_buf = ""; - } - if (!log_str.empty()) { - syslog(LOG_INFO, "[%s] command started.\n", cmd.c_str()); - log_str = ""; - } - } else { - // Result Line - result_buf.append(line + "\n"); - } - } // while - - c.wait(); - std::ostringstream os; - is_err >> os.rdbuf(); - error_str = "command terminate. " + os.str(); - } catch (boost::process::process_error & e) { - error_str = e.what(); - c.terminate(); - signal(SIGCHLD, SIG_IGN); - } - - int err_code = c.exit_code(); - // The err_code when killing a child process is usually 9. However, - // Because there were times when it was 0, 0 is changed to an error value. - if (err_code == 0) { - err_code = -1; - } - g_NethogsString.set("[" + cmd + "] " + error_str, err_code); - - if (log_str != error_str) { - log_str = error_str; - syslog(LOG_ERR, "[%s] err(%d) %s\n", cmd.c_str(), err_code, error_str.c_str()); - } - - for (int cnt = 10; cnt > 0 && g_Running; --cnt) { - g_WatchDogCount = 0; - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - } // while - - std::terminate(); -} - -/** - * @brief check NET temperature - * @param [in] port port to listen - */ -void run(int port) -{ - // Create a new socket - int sock = socket(AF_INET, SOCK_STREAM, 0); - if (sock < 0) { - syslog(LOG_ERR, "Failed to create a new socket. %s\n", strerror(errno)); - return; - } - - // Allow address reuse - int ret = 0; - int opt = 1; - ret = setsockopt( - sock, SOL_SOCKET, SO_REUSEADDR, reinterpret_cast(&opt), (socklen_t)sizeof(opt)); - if (ret < 0) { - syslog(LOG_ERR, "Failed to set socket FD's option. %s\n", strerror(errno)); - close(sock); - return; - } - - // Give the socket FD the local address ADDR - sockaddr_in addr{}; - addr.sin_family = AF_INET; - addr.sin_port = htons(port); - addr.sin_addr.s_addr = htonl(INADDR_ANY); - ret = bind(sock, (struct sockaddr *)&addr, sizeof(addr)); - if (ret < 0) { - syslog(LOG_ERR, "Failed to give the socket FD the local address ADDR. %s\n", strerror(errno)); - close(sock); - return; - } - - // Prepare to accept connections on socket FD - ret = listen(sock, 5); - if (ret < 0) { - syslog(LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); - close(sock); - return; - } - - sockaddr_in client{}; - socklen_t len = sizeof(client); - - while (true) { - // Await a connection on socket FD - int new_sock = accept(sock, reinterpret_cast(&client), &len); - if (new_sock < 0) { - syslog( - LOG_ERR, "Failed to prepare to accept connections on socket FD. %s\n", strerror(errno)); - close(sock); - return; - } - - // Receive list of device from a socket - char buf[1024]{}; - ret = recv(new_sock, buf, sizeof(buf) - 1, 0); - if (ret < 0) { - syslog(LOG_ERR, "Failed to receive. %s\n", strerror(errno)); - close(new_sock); - close(sock); - return; - } - // No data received - if (ret == 0) { - syslog(LOG_ERR, "No data received. %s\n", strerror(errno)); - close(new_sock); - close(sock); - return; - } - - buf[sizeof(buf) - 1] = '\0'; - const std::string program_name = std::string(buf); - - // set result data - TrafficReaderResult result; - result.error_code_ = g_NethogsString.get(result.str_); - if (result.error_code_ == 0 && program_name != GET_ALL_STR) { - std::stringstream lines{result.str_}; - std::string line; - std::string result_str; - while (std::getline(lines, line)) { - if (line.find(program_name) != std::string::npos) { - result_str.append(line + "\n"); - } - } // while - result.str_ = result_str; - } - - std::ostringstream oss; - boost::archive::text_oarchive oa(oss); - oa << result; - // Write N bytes of BUF to FD - ret = write(new_sock, oss.str().c_str(), oss.str().length()); - if (ret < 0) { - syslog(LOG_ERR, "Failed to write N bytes of BUF to FD. %s\n", strerror(errno)); - } - - // Close the file descriptor FD - ret = close(new_sock); - if (ret < 0) { - syslog(LOG_ERR, "Failed to close the file descriptor FD. %s\n", strerror(errno)); - } - - if (++g_WatchDogCount >= 3) { - syslog(LOG_ERR, "nethogs command thread error\n"); - close(sock); - return; - } - } - - close(sock); -} - -int main(int argc, char ** argv) -{ - static struct option long_options[] = { - {"help", no_argument, nullptr, 'h'}, - {"port", required_argument, nullptr, 'p'}, - {nullptr, 0, nullptr, 0}}; - - // Parse command-line options - int c = 0; - int option_index = 0; - int port = TRAFFIC_READER_PORT; - while ((c = getopt_long(argc, argv, "hp:", long_options, &option_index)) != -1) { - switch (c) { - case 'h': - usage(); - return EXIT_SUCCESS; - - case 'p': - try { - port = boost::lexical_cast(optarg); - } catch (const boost::bad_lexical_cast & e) { - printf("Error: %s\n", e.what()); - return EXIT_FAILURE; - } - break; - - default: - break; - } - } - - // Put the program in the background - if (daemon(0, 0) < 0) { - printf("Failed to put the program in the background. %s\n", strerror(errno)); - return errno; - } - - // Open connection to system logger - openlog(nullptr, LOG_PID, LOG_DAEMON); - - g_Running = true; - std::thread th = std::thread(&executeNethogs); - th.detach(); - - run(port); - - g_Running = false; - - // Close descriptor used to write to system logger - closelog(); - - return EXIT_SUCCESS; -} diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp new file mode 100644 index 0000000000000..0af3b0d89d312 --- /dev/null +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_main.cpp @@ -0,0 +1,89 @@ +// Copyright 2022 Autoware Foundation +// +// 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. + +#include + +#include +#include + +#include +#include + +/** + * @brief print usage + */ +void usage() +{ + printf("Usage: traffic_reader [options]\n"); + printf(" -h --help : Display help\n"); + printf(" -s --socket # : Path of UNIX domain socket.\n"); + printf("\n"); +} + +int main(int argc, char ** argv) +{ + static struct option long_options[] = { + {"help", no_argument, nullptr, 'h'}, + {"socket", required_argument, nullptr, 's'}, + {nullptr, 0, nullptr, 0}}; + + // Parse command-line options + int c = 0; + int option_index = 0; + std::string socket_path = traffic_reader_service::socket_path; + + while ((c = getopt_long(argc, argv, "hs:", long_options, &option_index)) != -1) { + switch (c) { + case 'h': + usage(); + return EXIT_SUCCESS; + + case 's': + socket_path = optarg; + break; + + default: + break; + } + } + + // Put the program in the background + if (daemon(0, 0) < 0) { + printf("Failed to put the program in the background. %s\n", strerror(errno)); + return errno; + } + + // Open connection to system logger + openlog(nullptr, LOG_PID, LOG_DAEMON); // NOLINT [hicpp-signed-bitwise] + + // Initialize traffic-reader service + traffic_reader_service::TrafficReaderService service(socket_path); + + if (!service.initialize()) { + service.shutdown(); + closelog(); + return EXIT_FAILURE; + } + + // Run main loop + service.run(); + + // Shutdown traffic-reader service + service.shutdown(); + + // Close descriptor used to write to system logger + closelog(); + + return EXIT_SUCCESS; +} diff --git a/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp new file mode 100644 index 0000000000000..90d8867235e37 --- /dev/null +++ b/system/system_monitor/reader/traffic_reader/traffic_reader_service.cpp @@ -0,0 +1,288 @@ +// Copyright 2022 Autoware Foundation +// +// 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. + +/** + * @file traffic_reader_service.cpp + * @brief traffic information read class + */ + +#include + +#include +#include + +#include +#include + +namespace process = boost::process; + +namespace traffic_reader_service +{ + +TrafficReaderService::TrafficReaderService(std::string socket_path) +: socket_path_(std::move(socket_path)), stop_(false) +{ +} + +bool TrafficReaderService::initialize() +{ + // Remove previous binding + remove(socket_path_.c_str()); + + local::stream_protocol::endpoint endpoint(socket_path_); + acceptor_ = std::make_unique(io_service_, endpoint); + + // Run I/O service processing loop + boost::system::error_code error_code; + io_service_.run(error_code); + if (error_code) { + syslog(LOG_ERR, "Failed to run I/O service. %s\n", error_code.message().c_str()); + return false; + } + + // Give permission to other users to access to socket + // NOLINTNEXTLINE [hicpp-signed-bitwise] + if (chmod(socket_path_.c_str(), S_IRWXU | S_IRWXG | S_IRWXO) != 0) { + syslog(LOG_ERR, "Failed to give permission to unix domain socket. %s\n", strerror(errno)); + return false; + } + + return true; +} + +void TrafficReaderService::shutdown() { io_service_.stop(); } + +void TrafficReaderService::run() +{ + while (true) { + socket_ = std::make_unique(io_service_); + + // Accept a new connection + boost::system::error_code error_code; + acceptor_->accept(*socket_, error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to accept new connection. %s\n", error_code.message().c_str()); + socket_->close(); + continue; + } + + // Read data from socket + char buffer[1024]{}; + socket_->read_some(boost::asio::buffer(buffer, sizeof(buffer)), error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to read data from socket. %s\n", error_code.message().c_str()); + socket_->close(); + continue; + } + + // Handle message + handle_message(buffer); + + socket_->close(); + } +} + +void TrafficReaderService::handle_message(const char * buffer) +{ + uint8_t request_id = Request::NONE; + + // Restore request from ros node + std::istringstream in_stream(buffer); + boost::archive::text_iarchive archive(in_stream); + + try { + archive >> request_id; + } catch (const std::exception & e) { + syslog(LOG_ERR, "Failed to restore message. %s\n", e.what()); + return; + } + + switch (request_id) { + case Request::START_NETHOGS: + start_nethogs(archive); + break; + case Request::GET_RESULT: + get_result(); + break; + default: + syslog(LOG_WARNING, "Unknown message. %d\n", request_id); + break; + } +} + +void TrafficReaderService::start_nethogs(boost::archive::text_iarchive & archive) +{ + syslog(LOG_INFO, "Starting nethogs...\n"); + + // Restore list of devices from ros node + try { + archive >> devices_; + archive >> program_name_; + } catch (const std::exception & e) { + syslog(LOG_ERR, "Failed to restore optional commands. %s\n", e.what()); + devices_.clear(); + } + + // Stop thread if already running + if (thread_.joinable()) { + { + std::lock_guard lock(mutex_); + stop_ = true; + } + thread_.join(); + } + + // Run nethogs + stop_ = false; + thread_ = std::thread(&TrafficReaderService::execute_nethogs, this); +} + +void TrafficReaderService::get_result() +{ + // Inform ros node + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & Request::GET_RESULT; + { + std::lock_guard lock(mutex_); + archive & result_; + } + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + syslog(LOG_ERR, "Failed to write data to socket. %s\n", error_code.message().c_str()); + } +} + +/** + * @brief thread function to execute nethogs + */ +void TrafficReaderService::execute_nethogs() +{ + std::stringstream command; + command << "nethogs -t"; + + for (const auto & device : devices_) { + command << " " << device; + } + + syslog(LOG_INFO, "%s\n", command.str().c_str()); + + process::ipstream is_out; + process::ipstream is_error; + std::string line; + + boost::process::child c(command.str(), process::std_out > is_out, process::std_err > is_error); + + // Output format of `nethogs -t [device(s)]` + // + // Refreshing: + // [PROGRAM/PID/UID SENT RECEIVED] + // usr/share/code/code/3102772/1000 0.565234 1.06855 + // ... + // unknown TCP/0/0 0 0 + while (std::getline(is_out, line)) { + // Exit loop when stop is requested + std::lock_guard lock(mutex_); + if (stop_) break; + + // Skip if line is empty + if (line.empty()) { + continue; + } + // Start of output + if (line == "Refreshing:") { + result_.error_code = EXIT_SUCCESS; + result_.output.clear(); + continue; + } + // Skip if no PID + if (line.find("/0/0\t") != std::string::npos) { + continue; + } + // End of output + if (line == "unknown TCP/0/0\t0\t0") { + continue; + } + + std::string command_line = get_command_line(line); + if (!command_line.empty()) { + // Add nethogs output and command line + if (program_name_ == "*" || command_line.find(program_name_) != std::string::npos) { + result_.output.append(fmt::format("{}\t{}\n", line, command_line)); + } + } + } + + c.terminate(); + + std::ostringstream out_stream; + is_error >> out_stream.rdbuf(); + + // Remove new line from output + std::string message = out_stream.str(); + message.erase(std::remove(message.begin(), message.end(), '\n'), message.cend()); + syslog(LOG_INFO, "%s\n", message.c_str()); + + { + std::lock_guard lock(mutex_); + result_.error_code = c.exit_code(); + result_.output = message; + } +} + +std::string TrafficReaderService::get_command_line(const std::string & line) +{ + std::vector tag_tokens; + std::vector slash_tokens; + + // usr/share/code/code/3102772/1000 0.565234 1.06855 + // Get first token separated by tab + boost::split(tag_tokens, line, boost::is_any_of("\t"), boost::token_compress_on); + + // usr/share/code/code/3102772/1000 + // Get second from the last separated by slash + boost::split( + slash_tokens, tag_tokens[0].c_str(), boost::is_any_of("/"), boost::token_compress_on); + if (slash_tokens.size() >= 3) { + return get_command_line_with_pid(std::atoi(slash_tokens[slash_tokens.size() - 2].c_str())); + } + + return ""; +} + +std::string TrafficReaderService::get_command_line_with_pid(pid_t pid) +{ + std::ifstream file(fmt::format("/proc/{}/cmdline", pid)); + + if (!file) { + return ""; + } + + std::string line; + getline(file, line); + + // cmdline is null separated, replace with space + std::replace(line.begin(), line.end(), '\0', ' '); + + return line; +} + +} // namespace traffic_reader_service diff --git a/system/system_monitor/src/net_monitor/net_monitor.cpp b/system/system_monitor/src/net_monitor/net_monitor.cpp index d9a42f8c03835..4ceec63ca862c 100644 --- a/system/system_monitor/src/net_monitor/net_monitor.cpp +++ b/system/system_monitor/src/net_monitor/net_monitor.cpp @@ -21,499 +21,268 @@ #include "system_monitor/system_monitor_utility.hpp" -#include +#include +#include #include -#include -// #include // workaround for build errors +#include +#define FMT_HEADER_ONLY #include #include #include #include #include -#include -#include -#include - -#include -#include -#include -#include -#include NetMonitor::NetMonitor(const rclcpp::NodeOptions & options) : Node("net_monitor", options), updater_(this), + hostname_(), last_update_time_{0, 0, this->get_clock()->get_clock_type()}, device_params_( declare_parameter>("devices", std::vector())), - last_reassembles_failed_(0), + getifaddrs_error_code_(0), monitor_program_(declare_parameter("monitor_program", "greengrass")), - traffic_reader_port_(declare_parameter("traffic_reader_port", TRAFFIC_READER_PORT)), + socket_path_(declare_parameter("socket_path", traffic_reader_service::socket_path)), crc_error_check_duration_(declare_parameter("crc_error_check_duration", 1)), crc_error_count_threshold_(declare_parameter("crc_error_count_threshold", 1)), + last_reassembles_failed_(0), reassembles_failed_check_duration_( declare_parameter("reassembles_failed_check_duration", 1)), reassembles_failed_check_count_(declare_parameter("reassembles_failed_check_count", 1)), reassembles_failed_column_index_(0) { - using namespace std::literals::chrono_literals; - if (monitor_program_.empty()) { - monitor_program_ = GET_ALL_STR; - nethogs_all_ = true; - } else { - nethogs_all_ = false; + monitor_program_ = "*"; } gethostname(hostname_, sizeof(hostname_)); updater_.setHardwareID(hostname_); - updater_.add("Network Usage", this, &NetMonitor::checkUsage); - updater_.add("Network Traffic", this, &NetMonitor::monitorTraffic); - updater_.add("Network CRC Error", this, &NetMonitor::checkCrcError); - updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::checkReassemblesFailed); + updater_.add("Network Connection", this, &NetMonitor::check_connection); + updater_.add("Network Usage", this, &NetMonitor::check_usage); + updater_.add("Network Traffic", this, &NetMonitor::monitor_traffic); + updater_.add("Network CRC Error", this, &NetMonitor::check_crc_error); + updater_.add("IP Packet Reassembles Failed", this, &NetMonitor::check_reassembles_failed); nl80211_.init(); - searchReassemblesFailedColumnIndex(); + // Run I/O service processing loop + boost::system::error_code error_code; + io_service_.run(error_code); + if (error_code) { + RCLCPP_WARN(get_logger(), "Failed to run I/O service. %s", error_code.message().c_str()); + } - // get Network information for the first time - updateNetworkInfoList(); + // Update list of network information + update_network_list(); + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::on_timer, this)); - timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&NetMonitor::onTimer, this)); + // Get column index of IP packet reassembles failed from `/proc/net/snmp` + get_reassembles_failed_column_index(); + + // Send request to start nethogs + send_start_nethogs_request(); } NetMonitor::~NetMonitor() { shutdown_nl80211(); } -void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } - -void NetMonitor::onTimer() { updateNetworkInfoList(); } - -// cspell: ignore ifas, ifrm, ifrc -void NetMonitor::updateNetworkInfoList() +void NetMonitor::check_connection(diagnostic_updater::DiagnosticStatusWrapper & status) { - net_info_list_.clear(); + // Remember start time to measure elapsed time + const auto t_start = SystemMonitorUtility::startMeasurement(); if (device_params_.empty()) { + status.summary(DiagStatus::ERROR, "invalid device parameter"); return; } - - const struct ifaddrs * ifa; - struct ifaddrs * ifas = nullptr; - - rclcpp::Duration duration = this->now() - last_update_time_; - - // Get network interfaces - getifaddrs_errno_ = 0; - if (getifaddrs(&ifas) < 0) { - getifaddrs_errno_ = errno; + if (getifaddrs_error_code_ != 0) { + status.summary(DiagStatus::ERROR, "getifaddrs error"); + status.add("getifaddrs", strerror(getifaddrs_error_code_)); return; } - for (ifa = ifas; ifa; ifa = ifa->ifa_next) { - // Skip no addr - if (!ifa->ifa_addr) { - continue; - } - // Skip loopback - if (ifa->ifa_flags & IFF_LOOPBACK) { - continue; - } - // Skip non AF_PACKET - if (ifa->ifa_addr->sa_family != AF_PACKET) { - continue; - } - // Skip device not specified - if ( - boost::find(device_params_, ifa->ifa_name) == device_params_.end() && - boost::find(device_params_, "*") == device_params_.end()) { - continue; - } - - int fd; - struct ifreq ifrm; - struct ifreq ifrc; - struct ethtool_cmd edata; - - net_info_list_.emplace_back(); - auto & net_info = net_info_list_.back(); - - net_info.interface_name = std::string(ifa->ifa_name); - - // Get MTU information - fd = socket(AF_INET, SOCK_DGRAM, 0); - strncpy(ifrm.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); - if (ioctl(fd, SIOCGIFMTU, &ifrm) < 0) { - net_info.mtu_errno = errno; - close(fd); - continue; - } + int index = 0; + int whole_level = DiagStatus::OK; + std::string error_message; - // Get network capacity - strncpy(ifrc.ifr_name, ifa->ifa_name, IFNAMSIZ - 1); - ifrc.ifr_data = (caddr_t)&edata; - edata.cmd = ETHTOOL_GSET; - if (ioctl(fd, SIOCETHTOOL, &ifrc) < 0) { - // possibly wireless connection, get bitrate(MBit/s) - net_info.speed = nl80211_.getBitrate(ifa->ifa_name); - if (net_info.speed <= 0) { - net_info.ethtool_errno = errno; - close(fd); - continue; - } + for (const auto & network : network_list_) { + if (network.is_invalid) { + make_invalid_diagnostic_status(network, index, status, error_message); } else { - net_info.speed = edata.speed; - } - - net_info.is_running = (ifa->ifa_flags & IFF_RUNNING); - - auto * stats = (struct rtnl_link_stats *)ifa->ifa_data; - if (bytes_.find(net_info.interface_name) != bytes_.end()) { - net_info.rx_traffic = - toMbit(stats->rx_bytes - bytes_[net_info.interface_name].rx_bytes) / duration.seconds(); - net_info.tx_traffic = - toMbit(stats->tx_bytes - bytes_[net_info.interface_name].tx_bytes) / duration.seconds(); - net_info.rx_usage = net_info.rx_traffic / net_info.speed; - net_info.tx_usage = net_info.tx_traffic / net_info.speed; + status.add(fmt::format("Network {}: status", index), connection_messages_.at(DiagStatus::OK)); + status.add(fmt::format("Network {}: name", index), network.interface_name); } + ++index; + } - net_info.mtu = ifrm.ifr_mtu; - net_info.rx_bytes = stats->rx_bytes; - net_info.rx_errors = stats->rx_errors; - net_info.tx_bytes = stats->tx_bytes; - net_info.tx_errors = stats->tx_errors; - net_info.collisions = stats->collisions; - - close(fd); + // Check if specified device exists + for (const auto & device : device_params_) { + // Skip if device not specified + if (device == "*") continue; - bytes_[net_info.interface_name].rx_bytes = stats->rx_bytes; - bytes_[net_info.interface_name].tx_bytes = stats->tx_bytes; + // Check if device exists in detected networks + const auto object = std::find_if( + network_list_.begin(), network_list_.end(), + [&device](const auto & network) { return network.interface_name == device; }); - // Get the count of CRC errors - crc_errors & crc_ers = crc_errors_[net_info.interface_name]; - crc_ers.errors_queue.push_back(stats->rx_crc_errors - crc_ers.last_rx_crc_errors); - while (crc_ers.errors_queue.size() > crc_error_check_duration_) { - crc_ers.errors_queue.pop_front(); + if (object == network_list_.end()) { + whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); + error_message = "no such device"; + status.add( + fmt::format("Network {}: status", index), connection_messages_.at(DiagStatus::WARN)); + status.add(fmt::format("Network {}: name", index), device); } - crc_ers.last_rx_crc_errors = stats->rx_crc_errors; + + ++index; } - freeifaddrs(ifas); + if (!error_message.empty()) { + status.summary(whole_level, error_message); + } else { + status.summary(whole_level, connection_messages_.at(whole_level)); + } - last_update_time_ = this->now(); + // Measure elapsed time since start time and report + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkUsage(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_usage(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!checkGeneralInfo(stat)) { - return; - } - int level = DiagStatus::OK; - int whole_level = DiagStatus::OK; int index = 0; std::string error_str; std::vector interface_names; - for (const auto & net_info : net_info_list_) { - if (!isSupportedNetwork(net_info, index, stat, error_str)) { - ++index; - interface_names.push_back(net_info.interface_name); - continue; - } - - level = net_info.is_running ? DiagStatus::OK : DiagStatus::ERROR; - - stat.add(fmt::format("Network {}: status", index), usage_dict_.at(level)); - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", net_info.rx_usage * 1e+2); - stat.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", net_info.tx_usage * 1e+2); - stat.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", net_info.rx_traffic); - stat.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", net_info.tx_traffic); - stat.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", net_info.speed); - stat.add(fmt::format("Network {}: mtu", index), net_info.mtu); - stat.add(fmt::format("Network {}: rx_bytes", index), net_info.rx_bytes); - stat.add(fmt::format("Network {}: rx_errors", index), net_info.rx_errors); - stat.add(fmt::format("Network {}: tx_bytes", index), net_info.tx_bytes); - stat.add(fmt::format("Network {}: tx_errors", index), net_info.tx_errors); - stat.add(fmt::format("Network {}: collisions", index), net_info.collisions); + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; + + level = network.is_running ? DiagStatus::OK : DiagStatus::ERROR; + + status.add(fmt::format("Network {}: status", index), usage_messages_.at(level)); + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.addf(fmt::format("Network {}: rx_usage", index), "%.2f%%", network.rx_usage * 1e+2); + status.addf(fmt::format("Network {}: tx_usage", index), "%.2f%%", network.tx_usage * 1e+2); + status.addf(fmt::format("Network {}: rx_traffic", index), "%.2f MBit/s", network.rx_traffic); + status.addf(fmt::format("Network {}: tx_traffic", index), "%.2f MBit/s", network.tx_traffic); + status.addf(fmt::format("Network {}: capacity", index), "%.1f MBit/s", network.speed); + status.add(fmt::format("Network {}: mtu", index), network.mtu); + status.add(fmt::format("Network {}: rx_bytes", index), network.rx_bytes); + status.add(fmt::format("Network {}: rx_errors", index), network.rx_errors); + status.add(fmt::format("Network {}: tx_bytes", index), network.tx_bytes); + status.add(fmt::format("Network {}: tx_errors", index), network.tx_errors); + status.add(fmt::format("Network {}: collisions", index), network.collisions); ++index; - - interface_names.push_back(net_info.interface_name); } - // Check if specified device exists - for (const auto & device : device_params_) { - // Skip if all devices specified - if (device == "*") { - continue; - } - // Skip if device already appended - if (boost::find(interface_names, device) != interface_names.end()) { - continue; - } - - stat.add(fmt::format("Network {}: status", index), "No Such Device"); - stat.add(fmt::format("Network {}: interface name", index), device); - error_str = "no such device"; - ++index; - } - - if (!error_str.empty()) { - stat.summary(DiagStatus::ERROR, error_str); - } else { - stat.summary(whole_level, usage_dict_.at(whole_level)); - } + status.summary(DiagStatus::OK, "OK"); // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkCrcError(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_crc_error(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - if (!checkGeneralInfo(stat)) { - return; - } - int whole_level = DiagStatus::OK; int index = 0; - std::string error_str; + std::string error_message; - for (const auto & net_info : net_info_list_) { - if (!isSupportedNetwork(net_info, index, stat, error_str)) { - ++index; - continue; - } + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; - crc_errors & crc_ers = crc_errors_[net_info.interface_name]; + CrcErrors & crc_errors = crc_errors_[network.interface_name]; unsigned int unit_rx_crc_errors = 0; - for (auto errors : crc_ers.errors_queue) { + for (auto errors : crc_errors.errors_queue) { unit_rx_crc_errors += errors; } - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add(fmt::format("Network {}: total rx_crc_errors", index), crc_ers.last_rx_crc_errors); - stat.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add( + fmt::format("Network {}: total rx_crc_errors", index), crc_errors.last_rx_crc_errors); + status.add(fmt::format("Network {}: rx_crc_errors per unit time", index), unit_rx_crc_errors); if (unit_rx_crc_errors >= crc_error_count_threshold_) { whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_str = "CRC error"; + error_message = "CRC error"; } ++index; } - if (!error_str.empty()) { - stat.summary(whole_level, error_str); + if (!error_message.empty()) { + status.summary(whole_level, error_message); } else { - stat.summary(whole_level, "OK"); + status.summary(whole_level, "OK"); } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -bool NetMonitor::checkGeneralInfo(diagnostic_updater::DiagnosticStatusWrapper & stat) -{ - if (device_params_.empty()) { - stat.summary(DiagStatus::ERROR, "invalid device parameter"); - return false; - } - - if (getifaddrs_errno_ != 0) { - stat.summary(DiagStatus::ERROR, "getifaddrs error"); - stat.add("getifaddrs", strerror(getifaddrs_errno_)); - return false; - } - return true; -} - -bool NetMonitor::isSupportedNetwork( - const NetworkInfo & net_info, int index, diagnostic_updater::DiagnosticStatusWrapper & stat, - std::string & error_str) -{ - // MTU information - if (net_info.mtu_errno != 0) { - if (net_info.mtu_errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add("ioctl(SIOCGIFMTU)", strerror(net_info.mtu_errno)); - return false; - } - - // network capacity - if (net_info.speed <= 0) { - if (net_info.ethtool_errno == ENOTSUP) { - stat.add(fmt::format("Network {}: status", index), "Not Supported"); - } else { - stat.add(fmt::format("Network {}: status", index), "Error"); - error_str = "ioctl error"; - } - - stat.add(fmt::format("Network {}: interface name", index), net_info.interface_name); - stat.add("ioctl(SIOCETHTOOL)", strerror(net_info.ethtool_errno)); - return false; - } - return true; -} - -#include // workaround for build errors - -void NetMonitor::monitorTraffic(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::monitor_traffic(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); - // Create a new socket - int sock = socket(AF_INET, SOCK_STREAM, 0); - if (sock < 0) { - stat.summary(DiagStatus::ERROR, "socket error"); - stat.add("socket", strerror(errno)); - return; - } - - // Specify the receiving timeouts until reporting an error - struct timeval tv; - tv.tv_sec = 10; - tv.tv_usec = 0; - int ret = setsockopt(sock, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "setsockopt error"); - stat.add("setsockopt", strerror(errno)); - close(sock); - return; - } - - // Connect the socket referred to by the file descriptor - sockaddr_in addr; - memset(&addr, 0, sizeof(sockaddr_in)); - addr.sin_family = AF_INET; - addr.sin_port = htons(traffic_reader_port_); - addr.sin_addr.s_addr = htonl(INADDR_ANY); - ret = connect(sock, (struct sockaddr *)&addr, sizeof(addr)); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "connect error"); - stat.add("connect", strerror(errno)); - close(sock); - return; - } - - // Write list of devices to FD - ret = write(sock, monitor_program_.c_str(), monitor_program_.length()); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "write error"); - stat.add("write", strerror(errno)); - RCLCPP_ERROR(get_logger(), "write error"); - close(sock); - return; - } - - // Receive messages from a socket - std::string rcv_str; - char buf[16 * 1024 + 1]; - do { - ret = recv(sock, buf, sizeof(buf) - 1, 0); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", strerror(errno)); - close(sock); - return; - } - if (ret > 0) { - buf[ret] = '\0'; - rcv_str += std::string(buf); - } - } while (ret > 0); - - // Close the file descriptor FD - ret = close(sock); - if (ret < 0) { - stat.summary(DiagStatus::ERROR, "close error"); - stat.add("close", strerror(errno)); - return; - } - - // No data received - if (rcv_str.length() == 0) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", "No data received"); - return; - } - - // Restore information list - TrafficReaderResult result; - try { - std::istringstream iss(rcv_str); - boost::archive::text_iarchive oa(iss); - oa >> result; - } catch (const std::exception & e) { - stat.summary(DiagStatus::ERROR, "recv error"); - stat.add("recv", e.what()); - return; - } + // Get result of nethogs + traffic_reader_service::Result result; + get_nethogs_result(result); // traffic_reader result to output - if (result.error_code_ != 0) { - stat.summary(DiagStatus::ERROR, "traffic_reader error"); - stat.add("error", result.str_); + if (result.error_code != EXIT_SUCCESS) { + status.summary(DiagStatus::ERROR, "traffic_reader error"); + status.add("error", result.output); } else { - stat.summary(DiagStatus::OK, "OK"); + status.summary(DiagStatus::OK, "OK"); - if (result.str_.length() == 0) { - stat.add("nethogs: result", "nothing"); - } else if (nethogs_all_) { - stat.add("nethogs: all (KB/sec):", result.str_); + if (result.output.empty()) { + status.add("nethogs: result", fmt::format("No data monitored: {}", monitor_program_)); } else { - std::stringstream lines{result.str_}; + std::stringstream lines{result.output}; std::string line; std::vector list; - int idx = 0; + int index = 0; while (std::getline(lines, line)) { - if (line.empty()) { - continue; - } + if (line.empty()) continue; + boost::split(list, line, boost::is_any_of("\t"), boost::token_compress_on); if (list.size() >= 3) { - stat.add(fmt::format("nethogs {}: PROGRAM", idx), list[0].c_str()); - stat.add(fmt::format("nethogs {}: SENT (KB/sec)", idx), list[1].c_str()); - stat.add(fmt::format("nethogs {}: RECEIVED (KB/sec)", idx), list[2].c_str()); + status.add(fmt::format("nethogs {}: program", index), list[3].c_str()); + status.add(fmt::format("nethogs {}: sent (KB/s)", index), list[1].c_str()); + status.add(fmt::format("nethogs {}: received (KB/sec)", index), list[2].c_str()); } else { - stat.add(fmt::format("nethogs {}: result", idx), line); + status.add(fmt::format("nethogs {}: result", index), line); } - idx++; - } // while + ++index; + } } } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::checkReassemblesFailed(diagnostic_updater::DiagnosticStatusWrapper & stat) +void NetMonitor::check_reassembles_failed(diagnostic_updater::DiagnosticStatusWrapper & status) { // Remember start time to measure elapsed time const auto t_start = SystemMonitorUtility::startMeasurement(); int whole_level = DiagStatus::OK; - std::string error_str; + std::string error_message; uint64_t total_reassembles_failed = 0; uint64_t unit_reassembles_failed = 0; - if (getReassemblesFailed(total_reassembles_failed)) { + if (get_reassembles_failed(total_reassembles_failed)) { reassembles_failed_queue_.push_back(total_reassembles_failed - last_reassembles_failed_); while (reassembles_failed_queue_.size() > reassembles_failed_check_duration_) { reassembles_failed_queue_.pop_front(); @@ -523,32 +292,226 @@ void NetMonitor::checkReassemblesFailed(diagnostic_updater::DiagnosticStatusWrap unit_reassembles_failed += reassembles_failed; } - stat.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); - stat.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); + status.add(fmt::format("total packet reassembles failed"), total_reassembles_failed); + status.add(fmt::format("packet reassembles failed per unit time"), unit_reassembles_failed); if (unit_reassembles_failed >= reassembles_failed_check_count_) { whole_level = std::max(whole_level, static_cast(DiagStatus::WARN)); - error_str = "reassembles failed"; + error_message = "reassembles failed"; } last_reassembles_failed_ = total_reassembles_failed; } else { reassembles_failed_queue_.push_back(0); whole_level = std::max(whole_level, static_cast(DiagStatus::ERROR)); - error_str = "failed to read /proc/net/snmp"; + error_message = "failed to read /proc/net/snmp"; } - if (!error_str.empty()) { - stat.summary(whole_level, error_str); + if (!error_message.empty()) { + status.summary(whole_level, error_message); } else { - stat.summary(whole_level, "OK"); + status.summary(whole_level, "OK"); } // Measure elapsed time since start time and report - SystemMonitorUtility::stopMeasurement(t_start, stat); + SystemMonitorUtility::stopMeasurement(t_start, status); } -void NetMonitor::searchReassemblesFailedColumnIndex() +void NetMonitor::on_timer() +{ + // Update list of network information + update_network_list(); +} + +void NetMonitor::shutdown_nl80211() { nl80211_.shutdown(); } + +void NetMonitor::make_invalid_diagnostic_status( + const NetworkInfomation & network, int index, + diagnostic_updater::DiagnosticStatusWrapper & status, std::string & error_message) +{ + // MTU information + if (network.mtu_error_code != 0) { + if (network.mtu_error_code == ENOTSUP) { + status.add(fmt::format("Network {}: status", index), "Not Supported"); + // Assume no error, no error message + } else { + status.add(fmt::format("Network {}: status", index), "Error"); + error_message = "ioctl error"; + } + + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add("ioctl(SIOCGIFMTU)", strerror(network.mtu_error_code)); + return; + } + + // network capacity + if (network.speed <= 0) { + if (network.ethtool_error_code == ENOTSUP) { + status.add(fmt::format("Network {}: status", index), "Not Supported"); + // Assume no error, no error message + } else { + status.add(fmt::format("Network {}: status", index), "Error"); + error_message = "ioctl error"; + } + + status.add(fmt::format("Network {}: interface name", index), network.interface_name); + status.add("ioctl(SIOCETHTOOL)", strerror(network.ethtool_error_code)); + } +} + +void NetMonitor::update_network_list() +{ + rclcpp::Duration duration = this->now() - last_update_time_; + + struct ifaddrs * interfaces = {}; + network_list_.clear(); + + // Get network interfaces + if (getifaddrs(&interfaces) < 0) { + getifaddrs_error_code_ = errno; + return; + } + + for (const auto * interface = interfaces; interface; interface = interface->ifa_next) { + // Skip no addr + if (!interface->ifa_addr) { + continue; + } + // Skip loopback + if (interface->ifa_flags & IFF_LOOPBACK) { + continue; + } + // Skip non AF_PACKET + if (interface->ifa_addr->sa_family != AF_PACKET) { + continue; + } + // Skip device not specified + const auto object = std::find_if( + device_params_.begin(), device_params_.end(), + [&interface](const auto & device) { return device == "*" || device == interface->ifa_name; }); + if (object == device_params_.end()) { + continue; + } + + NetworkInfomation network{}; + network.interface_name = interface->ifa_name; + network.is_running = (interface->ifa_flags & IFF_RUNNING); + + // Update network information using socket + update_network_information_by_socket(network); + + // Update network information using routing netlink stats + update_network_information_by_routing_netlink(network, interface->ifa_data, duration); + + network_list_.emplace_back(network); + } + + freeifaddrs(interfaces); + + last_update_time_ = this->now(); +} + +void NetMonitor::update_network_information_by_socket(NetworkInfomation & network) +{ + // Get MTU information + int fd = socket(AF_INET, SOCK_DGRAM, 0); + + // Update MTU information + update_mtu(network, fd); + + // Update network capacity + update_network_capacity(network, fd); + + close(fd); +} + +void NetMonitor::update_mtu(NetworkInfomation & network, int socket) +{ + struct ifreq request = {}; + + // NOLINTNEXTLINE [cppcoreguidelines-pro-type-union-access] + strncpy(request.ifr_name, network.interface_name.c_str(), IFNAMSIZ - 1); + if (ioctl(socket, SIOCGIFMTU, &request) < 0) { + network.is_invalid = true; + network.mtu_error_code = errno; + return; + } + + network.mtu = request.ifr_mtu; // NOLINT [cppcoreguidelines-pro-type-union-access] +} + +void NetMonitor::update_network_capacity(NetworkInfomation & network, int socket) +{ + struct ifreq request = {}; + struct ethtool_cmd ether_request = {}; + + // NOLINTNEXTLINE [cppcoreguidelines-pro-type-union-access] + strncpy(request.ifr_name, network.interface_name.c_str(), IFNAMSIZ - 1); + request.ifr_data = (caddr_t)ðer_request; // NOLINT [cppcoreguidelines-pro-type-cstyle-cast] + + ether_request.cmd = ETHTOOL_GSET; + if (ioctl(socket, SIOCETHTOOL, &request) >= 0) { + network.speed = ether_request.speed; + return; + } + + // Possibly wireless connection, get bitrate(MBit/s) + float ret = nl80211_.getBitrate(network.interface_name.c_str()); + if (ret <= 0) { + network.is_invalid = true; + network.ethtool_error_code = errno; + } else { + network.speed = ret; + } +} + +void NetMonitor::update_network_information_by_routing_netlink( + NetworkInfomation & network, void * data, const rclcpp::Duration & duration) +{ + auto * stats = static_cast(data); + + update_traffic(network, stats, duration); + + update_crc_error(network, stats); +} + +void NetMonitor::update_traffic( + NetworkInfomation & network, const struct rtnl_link_stats * stats, + const rclcpp::Duration & duration) +{ + network.rx_bytes = stats->rx_bytes; + network.rx_errors = stats->rx_errors; + network.tx_bytes = stats->tx_bytes; + network.tx_errors = stats->tx_errors; + network.collisions = stats->collisions; + + // Calculate traffic and usage if interface is entried in bytes + const auto bytes_entry = bytes_.find(network.interface_name); + if (bytes_entry != bytes_.end()) { + network.rx_traffic = + to_mbit(stats->rx_bytes - bytes_entry->second.rx_bytes) / duration.seconds(); + network.tx_traffic = + to_mbit(stats->tx_bytes - bytes_entry->second.tx_bytes) / duration.seconds(); + network.rx_usage = network.rx_traffic / network.speed; + network.tx_usage = network.tx_traffic / network.speed; + } + + bytes_[network.interface_name].rx_bytes = stats->rx_bytes; + bytes_[network.interface_name].tx_bytes = stats->tx_bytes; +} + +void NetMonitor::update_crc_error(NetworkInfomation & network, const struct rtnl_link_stats * stats) +{ + // Get the count of CRC errors + CrcErrors & crc_errors = crc_errors_[network.interface_name]; + crc_errors.errors_queue.push_back(stats->rx_crc_errors - crc_errors.last_rx_crc_errors); + while (crc_errors.errors_queue.size() > crc_error_check_duration_) { + crc_errors.errors_queue.pop_front(); + } + crc_errors.last_rx_crc_errors = stats->rx_crc_errors; +} + +void NetMonitor::get_reassembles_failed_column_index() { std::ifstream ifs("/proc/net/snmp"); if (!ifs) { @@ -556,45 +519,43 @@ void NetMonitor::searchReassemblesFailedColumnIndex() return; } - // /proc/net/snmp - // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... - // Ip: 2 64 5636471397 ... 135 2303339 216166 270 ... - std::string line; - // Find column index of 'ReasmFails' + std::string line; if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); return; } - std::vector title_list; - boost::split(title_list, line, boost::is_space()); + // /proc/net/snmp + // Ip: Forwarding DefaultTTL InReceives ... ReasmTimeout ReasmReqds ReasmOKs ReasmFails ... + // Ip: 2 64 5636471397 ... 135 2303339 216166 270 .. + std::vector header_list; + boost::split(header_list, line, boost::is_space()); - if (title_list.empty()) { - RCLCPP_WARN(get_logger(), "/proc/net/snmp first line is empty."); + if (header_list.empty()) { + RCLCPP_WARN(get_logger(), "Failed to get header list of /proc/net/snmp."); return; } - if (title_list[0] != "Ip:") { + if (header_list[0] != "Ip:") { RCLCPP_WARN( - get_logger(), "/proc/net/snmp line title column is invalid. : %s", title_list[0].c_str()); + get_logger(), "Header column is invalid in /proc/net/snmp. %s", header_list[0].c_str()); return; } int index = 0; - for (auto itr = title_list.begin(); itr != title_list.end(); ++itr, ++index) { - if (*itr == "ReasmFails") { + for (const auto & header : header_list) { + if (header == "ReasmFails") { reassembles_failed_column_index_ = index; break; } + ++index; } } -bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) +bool NetMonitor::get_reassembles_failed(uint64_t & reassembles_failed) { if (reassembles_failed_column_index_ == 0) { - RCLCPP_WARN( - get_logger(), "reassembles failed column index is invalid. : %d", - reassembles_failed_column_index_); + RCLCPP_WARN(get_logger(), "Column index is invalid. : %d", reassembles_failed_column_index_); return false; } @@ -606,15 +567,15 @@ bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) std::string line; - // Skip title row + // Skip header row if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp first line."); + RCLCPP_WARN(get_logger(), "Failed to get header of /proc/net/snmp."); return false; } // Find a value of 'ReasmFails' if (!std::getline(ifs, line)) { - RCLCPP_WARN(get_logger(), "Failed to get /proc/net/snmp second line."); + RCLCPP_WARN(get_logger(), "Failed to get a line of /proc/net/snmp."); return false; } @@ -634,5 +595,143 @@ bool NetMonitor::getReassemblesFailed(uint64_t & reassembles_failed) return true; } +void NetMonitor::send_start_nethogs_request() +{ + // Connect to boot/shutdown service + if (!connect_service()) { + close_connection(); + return; + } + + diagnostic_updater::DiagnosticStatusWrapper stat; + std::vector interface_names; + + for (const auto & network : network_list_) { + // Skip if network is not supported + if (network.is_invalid) continue; + + interface_names.push_back(network.interface_name); + } + + // Send data to traffic-reader service + send_data_with_parameters( + traffic_reader_service::START_NETHOGS, interface_names, monitor_program_); + + // Close connection with traffic-reader service + close_connection(); +} + +void NetMonitor::get_nethogs_result(traffic_reader_service::Result & result) +{ + // Connect to traffic-reader service + if (!connect_service()) { + close_connection(); + return; + } + + // Send data to traffic-reader service + if (!send_data(traffic_reader_service::Request::GET_RESULT)) { + close_connection(); + return; + } + + // Receive data from traffic-reader service + receive_data(result); + + // Close connection with traffic-reader service + close_connection(); +} + +bool NetMonitor::connect_service() +{ + local::stream_protocol::endpoint endpoint(socket_path_); + socket_ = std::make_unique(io_service_); + + // Connect socket + boost::system::error_code error_code; + socket_->connect(endpoint, error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to connect socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +bool NetMonitor::send_data(traffic_reader_service::Request request) +{ + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & request; + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to write data to socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +bool NetMonitor::send_data_with_parameters( + traffic_reader_service::Request request, std::vector & parameters, + std::string & program_name) +{ + std::ostringstream out_stream; + boost::archive::text_oarchive archive(out_stream); + archive & request; + archive & parameters; + archive & program_name; + + // Write data to socket + boost::system::error_code error_code; + socket_->write_some( + boost::asio::buffer(out_stream.str().c_str(), out_stream.str().length()), error_code); + + if (error_code) { + RCLCPP_ERROR(get_logger(), "Failed to write data to socket. %s", error_code.message().c_str()); + return false; + } + + return true; +} + +void NetMonitor::receive_data(traffic_reader_service::Result & result) +{ + uint8_t request_id = traffic_reader_service::Request::NONE; + + // Read data from socket + char buffer[10240]{}; + boost::system::error_code error_code; + socket_->read_some(boost::asio::buffer(buffer, sizeof(buffer)), error_code); + + if (error_code) { + RCLCPP_ERROR( + get_logger(), "Failed to read data from socket. %s\n", error_code.message().c_str()); + return; + } + + // Restore device status list + try { + std::istringstream in_stream(buffer); + boost::archive::text_iarchive archive(in_stream); + archive >> request_id; + archive >> result; + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Failed to restore message. %s\n", e.what()); + } +} + +void NetMonitor::close_connection() +{ + // Close socket + socket_->close(); +} + #include RCLCPP_COMPONENTS_REGISTER_NODE(NetMonitor) From 3b6f48d6c834819b4078487d0fe49ec13360f1c4 Mon Sep 17 00:00:00 2001 From: Ismet Atabay <56237550+ismetatabay@users.noreply.github.com> Date: Wed, 21 Dec 2022 21:56:09 +0300 Subject: [PATCH 32/37] fix(mission_planner): combined lanelet calculation (#2546) combine_lanelets function fixed Signed-off-by: ismet atabay --- .../src/lanelet2_plugins/utility_functions.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp index eaadd019b462c..6f5ec16ddfc04 100644 --- a/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/utility_functions.cpp @@ -59,22 +59,23 @@ lanelet::ConstLanelet combine_lanelets(const lanelet::ConstLanelets & lanelets) lanelet::Points3d lefts; lanelet::Points3d rights; lanelet::Points3d centers; - std::vector bound_ids; + std::vector left_bound_ids; + std::vector right_bound_ids; for (const auto & llt : lanelets) { if (llt.id() != 0) { - bound_ids.push_back(llt.leftBound().id()); - bound_ids.push_back(llt.rightBound().id()); + left_bound_ids.push_back(llt.leftBound().id()); + right_bound_ids.push_back(llt.rightBound().id()); } } for (const auto & llt : lanelets) { - if (std::count(bound_ids.begin(), bound_ids.end(), llt.leftBound().id()) < 2) { + if (std::count(right_bound_ids.begin(), right_bound_ids.end(), llt.leftBound().id()) < 1) { for (const auto & pt : llt.leftBound()) { lefts.push_back(lanelet::Point3d(pt)); } } - if (std::count(bound_ids.begin(), bound_ids.end(), llt.rightBound().id()) < 2) { + if (std::count(left_bound_ids.begin(), left_bound_ids.end(), llt.rightBound().id()) < 1) { for (const auto & pt : llt.rightBound()) { rights.push_back(lanelet::Point3d(pt)); } From a7deda64979158442140438994a6ec7f1a940261 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 22 Dec 2022 09:35:47 +0900 Subject: [PATCH 33/37] fix(system_monitor): change default param path (#2560) Signed-off-by: kminoda Signed-off-by: kminoda --- .../launch/system_monitor.launch.xml | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/system/system_monitor/launch/system_monitor.launch.xml b/system/system_monitor/launch/system_monitor.launch.xml index 0d8a14981bd8a..c6d29dd6b772f 100644 --- a/system/system_monitor/launch/system_monitor.launch.xml +++ b/system/system_monitor/launch/system_monitor.launch.xml @@ -1,12 +1,12 @@ - - - - - - - - + + + + + + + + From f2fabfefac94979735554f6c2f573b55d7f3a7c6 Mon Sep 17 00:00:00 2001 From: kminoda <44218668+kminoda@users.noreply.github.com> Date: Thu, 22 Dec 2022 09:59:12 +0900 Subject: [PATCH 34/37] revert(tier4_map_launch): move config back to autoware.universe (#2561) * revert(tier4_map_launch): move config back to autoware.universe Signed-off-by: kminoda * fix map.launch.xml Signed-off-by: kminoda Signed-off-by: kminoda --- launch/tier4_map_launch/CMakeLists.txt | 1 + .../config/pointcloud_map_loader.param.yaml | 9 +++++++++ launch/tier4_map_launch/launch/map.launch.py | 6 +++++- launch/tier4_map_launch/launch/map.launch.xml | 2 +- 4 files changed, 16 insertions(+), 2 deletions(-) create mode 100644 launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml diff --git a/launch/tier4_map_launch/CMakeLists.txt b/launch/tier4_map_launch/CMakeLists.txt index 98f12b64f3ca4..b0e770d7c7619 100644 --- a/launch/tier4_map_launch/CMakeLists.txt +++ b/launch/tier4_map_launch/CMakeLists.txt @@ -7,4 +7,5 @@ autoware_package() ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml new file mode 100644 index 0000000000000..8f3ccbff00360 --- /dev/null +++ b/launch/tier4_map_launch/config/pointcloud_map_loader.param.yaml @@ -0,0 +1,9 @@ +/**: + ros__parameters: + enable_whole_load: true + enable_downsampled_whole_load: false + enable_partial_load: false + enable_differential_load: false + + # only used when downsample_whole_load enabled + leaf_size: 3.0 # downsample leaf size [m] diff --git a/launch/tier4_map_launch/launch/map.launch.py b/launch/tier4_map_launch/launch/map.launch.py index 231a805603a44..3be3082e58040 100644 --- a/launch/tier4_map_launch/launch/map.launch.py +++ b/launch/tier4_map_launch/launch/map.launch.py @@ -161,7 +161,11 @@ def add_launch_arg(name: str, default_value=None, description=None): ), add_launch_arg( "pointcloud_map_loader_param_path", - "", + [ + FindPackageShare("tier4_map_launch"), + "/config/pointcloud_map_loader.param.yaml", + # ToDo(kminoda): This file should eventually be removed as well as the other components + ], "path to pointcloud_map_loader param file", ), add_launch_arg("use_intra_process", "false", "use ROS2 component container communication"), diff --git a/launch/tier4_map_launch/launch/map.launch.xml b/launch/tier4_map_launch/launch/map.launch.xml index 7ecc61605eeac..39a398e7c1d77 100644 --- a/launch/tier4_map_launch/launch/map.launch.xml +++ b/launch/tier4_map_launch/launch/map.launch.xml @@ -1,6 +1,6 @@ - + From 8bdfe7cc54bca944afd1e77614730573210b8a12 Mon Sep 17 00:00:00 2001 From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Date: Thu, 22 Dec 2022 12:13:57 +0900 Subject: [PATCH 35/37] refactor(avoidance): generate shift line before the plan() function (#2533) * refactor(avoidance): generate shift line in updateData() Signed-off-by: satoshi-ota * refactor(avoidance): define helper function in header file Signed-off-by: satoshi-ota * fix(avoidance): fix function name Signed-off-by: satoshi-ota * fix(avoidance): fix function name Signed-off-by: satoshi-ota Signed-off-by: satoshi-ota --- .../avoidance/avoidance_module.hpp | 124 +++++++-- .../avoidance/avoidance_module_data.hpp | 11 + .../avoidance/avoidance_module.cpp | 257 ++++++++---------- 3 files changed, 215 insertions(+), 177 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp index a4783a445b177..68c3efd1b75a1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp @@ -28,6 +28,7 @@ #include #include +#include #include #include #include @@ -35,7 +36,12 @@ namespace behavior_path_planner { + +using motion_utils::calcSignedArcLength; +using motion_utils::findNearestIndex; + using tier4_planning_msgs::msg::AvoidanceDebugMsg; + class AvoidanceModule : public SceneModuleInterface { public: @@ -134,10 +140,10 @@ class AvoidanceModule : public SceneModuleInterface const Point ego_position = planner_data_->self_pose->pose.position; for (const auto & left_shift : left_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, left_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, left_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, left_shift.finish_pose.position); rtc_interface_left_.updateCooperateStatus( left_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -148,10 +154,10 @@ class AvoidanceModule : public SceneModuleInterface } for (const auto & right_shift : right_shift_array_) { - const double start_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.start_pose.position); - const double finish_distance = motion_utils::calcSignedArcLength( - path.points, ego_position, right_shift.finish_pose.position); + const double start_distance = + calcSignedArcLength(path.points, ego_position, right_shift.start_pose.position); + const double finish_distance = + calcSignedArcLength(path.points, ego_position, right_shift.finish_pose.position); rtc_interface_right_.updateCooperateStatus( right_shift.uuid, true, start_distance, finish_distance, clock_->now()); if (finish_distance > -1.0e-03) { @@ -200,6 +206,7 @@ class AvoidanceModule : public SceneModuleInterface void fillObjectMovingTime(ObjectData & object_data) const; void compensateDetectionLost( ObjectDataArray & target_objects, ObjectDataArray & other_objects) const; + void fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const; // data used in previous planning ShiftedPath prev_output_; @@ -225,14 +232,13 @@ class AvoidanceModule : public SceneModuleInterface void updateRegisteredObject(const ObjectDataArray & objects); // -- for shift point generation -- - AvoidLineArray calcShiftLines(AvoidLineArray & current_raw_shift_lines, DebugData & debug) const; + AvoidLineArray applyPreProcessToRawShiftLines( + AvoidLineArray & current_raw_shift_points, DebugData & debug) const; // shift point generation: generator double getShiftLength( const ObjectData & object, const bool & is_object_on_right, const double & avoid_margin) const; AvoidLineArray calcRawShiftLinesFromObjects(const ObjectDataArray & objects) const; - double getRightShiftBound() const; - double getLeftShiftBound() const; // shift point generation: combiner AvoidLineArray combineRawShiftLinesWithUniqueCheck( @@ -268,7 +274,7 @@ class AvoidanceModule : public SceneModuleInterface void fillAdditionalInfoFromLongitudinal(AvoidLineArray & shift_lines) const; // -- for new shift point approval -- - boost::optional findNewShiftLine( + AvoidLineArray findNewShiftLine( const AvoidLineArray & shift_lines, const PathShifter & shifter) const; void addShiftLineIfApproved(const AvoidLineArray & point); void addNewShiftLines(PathShifter & path_shifter, const AvoidLineArray & shift_lines) const; @@ -300,33 +306,91 @@ class AvoidanceModule : public SceneModuleInterface void updateAvoidanceDebugData(std::vector & avoidance_debug_msg_array) const; mutable std::vector debug_avoidance_initializer_for_shift_line_; mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_; - // ===================================== + // ========= helper functions ========== - // ===================================== + + double getEgoSpeed() const + { + return std::abs(planner_data_->self_odometry->twist.twist.linear.x); + } + + double getNominalAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); + } + + double getSharpAvoidanceEgoSpeed() const + { + return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); + } + + double getNominalPrepareDistance() const + { + const auto & p = parameters_; + const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. + const auto nominal_distance = + std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); + return nominal_distance + epsilon_m; + } + + double getNominalAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getSharpAvoidanceDistance(const double shift_length) const + { + const auto & p = parameters_; + const auto distance_by_jerk = PathShifter::calcLongitudinalDistFromJerk( + shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); + + return std::max(p->min_avoidance_distance, distance_by_jerk); + } + + double getRightShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return -parameters_->max_right_shift_length; + } + + double getLeftShiftBound() const + { + // TODO(Horibe) write me. Real lane boundary must be considered here. + return parameters_->max_left_shift_length; + } + + double getCurrentShift() const + { + return prev_output_.shift_length.at( + findNearestIndex(prev_output_.path.points, getEgoPosition())); + } + + double getCurrentLinearShift() const + { + return prev_linear_shift_path_.shift_length.at( + findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); + } + + double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } + + Point getEgoPosition() const { return planner_data_->self_pose->pose.position; } + + Pose getEgoPose() const { return planner_data_->self_pose->pose; } + + Pose getUnshiftedEgoPose(const ShiftedPath & prev_path) const; PathWithLaneId calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const; + const std::shared_ptr & planner_data, const Pose & pose) const; // TODO(Horibe): think later. // for unique ID mutable uint64_t original_unique_id = 0; // TODO(Horibe) remove mutable uint64_t getOriginalShiftLineUniqueId() const { return original_unique_id++; } - double getNominalAvoidanceDistance(const double shift_length) const; - double getNominalPrepareDistance() const; - double getNominalAvoidanceEgoSpeed() const; - - double getSharpAvoidanceDistance(const double shift_length) const; - double getSharpAvoidanceEgoSpeed() const; - - double getEgoSpeed() const; - Point getEgoPosition() const; - PoseStamped getEgoPose() const; - PoseStamped getUnshiftedEgoPose(const ShiftedPath & prev_path) const; - double getCurrentBaseShift() const { return path_shifter_.getBaseOffset(); } - double getCurrentShift() const; - double getCurrentLinearShift() const; - /** * avoidance module misc data */ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp index 25b29352973c1..426387c939165 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module_data.hpp @@ -286,11 +286,22 @@ struct AvoidancePlanningData // current driving lanelet lanelet::ConstLanelets current_lanelets; + // output path + ShiftedPath candidate_path; + // avoidance target objects ObjectDataArray target_objects; // the others ObjectDataArray other_objects; + + // raw shift point + AvoidLineArray unapproved_raw_sl{}; + + // new shift point + AvoidLineArray unapproved_new_sl{}; + + bool avoiding_now{false}; }; /* diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 1f7979c9b726c..61963bb9f0aaa 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -50,6 +50,22 @@ using tier4_autoware_utils::calcDistance2d; using tier4_autoware_utils::calcLateralDeviation; using tier4_planning_msgs::msg::AvoidanceDebugFactor; +namespace +{ + +AvoidLine getNonStraightShiftLine(const AvoidLineArray & shift_lines) +{ + for (const auto & sl : shift_lines) { + if (fabs(sl.getRelativeLength()) > 0.01) { + return sl; + } + } + + return {}; +} + +} // namespace + AvoidanceModule::AvoidanceModule( const std::string & name, rclcpp::Node & node, std::shared_ptr parameters) : SceneModuleInterface{name, node}, @@ -146,7 +162,7 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // reference pose const auto reference_pose = getUnshiftedEgoPose(prev_output_); - data.reference_pose = reference_pose.pose; + data.reference_pose = reference_pose; // center line path (output of this function must have size > 1) const auto center_path = calcCenterLinePath(planner_data_, reference_pose); @@ -177,8 +193,8 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb // lanelet info data.current_lanelets = util::calcLaneAroundPose( - planner_data_->route_handler, reference_pose.pose, - planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length); + planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, + planner_data_->parameters.backward_path_length); // target objects for avoidance fillAvoidanceTargetObjects(data, debug); @@ -503,6 +519,51 @@ void AvoidanceModule::fillObjectMovingTime(ObjectData & object_data) const } } +void AvoidanceModule::fillShiftLine(AvoidancePlanningData & data, DebugData & debug) const +{ + constexpr double AVOIDING_SHIFT_THR = 0.1; + data.avoiding_now = std::abs(getCurrentShift()) > AVOIDING_SHIFT_THR; + + auto path_shifter = path_shifter_; + + /** + * STEP 1 + * Create raw shift points from target object. The lateral margin between the ego and the target + * object varies depending on the relative speed between the ego and the target object. + */ + data.unapproved_raw_sl = calcRawShiftLinesFromObjects(data.target_objects); + + /** + * STEP 2 + * Modify the raw shift points. (Merging, Trimming) + */ + const auto processed_raw_sp = applyPreProcessToRawShiftLines(data.unapproved_raw_sl, debug); + + /** + * STEP 3 + * Find new shift point + */ + data.unapproved_new_sl = findNewShiftLine(processed_raw_sp, path_shifter); + + /** + * STEP 4 + * If there are new shift points, these shift points are registered in path_shifter. + */ + if (!data.unapproved_new_sl.empty()) { + addNewShiftLines(path_shifter, data.unapproved_new_sl); + } + + /** + * STEP 5 + * Generate avoidance path. + */ + data.candidate_path = generateAvoidancePath(path_shifter); + + { + debug.current_raw_shift = data.unapproved_raw_sl; + } +} + /** * updateRegisteredRawShiftLines * @@ -535,16 +596,9 @@ void AvoidanceModule::updateRegisteredRawShiftLines() debug_data_.registered_raw_shift = registered_raw_shift_lines_; } -AvoidLineArray AvoidanceModule::calcShiftLines( +AvoidLineArray AvoidanceModule::applyPreProcessToRawShiftLines( AvoidLineArray & current_raw_shift_lines, DebugData & debug) const { - /** - * Generate raw_shift_lines (shift length, avoidance start point, end point, return point, etc) - * for each object. These raw shift points are merged below to compute appropriate shift points. - */ - current_raw_shift_lines = calcRawShiftLinesFromObjects(avoidance_data_.target_objects); - debug.current_raw_shift = current_raw_shift_lines; - /** * Use all registered points. For the current points, if the similar one of the current * points are already registered, will not use it. @@ -1744,7 +1798,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // avoidance points: No, shift points: No -> set the ego position to the last shift point // so that the return-shift will be generated from ego position. if (!has_candidate_point && !has_registered_point) { - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = getCurrentBaseShift(); } @@ -1780,7 +1834,7 @@ void AvoidanceModule::addReturnShiftLineFromEgo( // set the return-shift from ego. DEBUG_PRINT( "return shift already exists, but they are all candidates. Add return shift for overwrite."); - last_sl.end = getEgoPose().pose; + last_sl.end = getEgoPose(); last_sl.end_idx = avoidance_data_.ego_closest_path_index; last_sl.end_shift_length = current_base_shift; } @@ -1885,18 +1939,6 @@ void AvoidanceModule::addReturnShiftLineFromEgo( DEBUG_PRINT("Return Shift is added."); } -double AvoidanceModule::getRightShiftBound() const -{ - // TODO(Horibe) write me. Real lane boundary must be considered here. - return -parameters_->max_right_shift_length; -} - -double AvoidanceModule::getLeftShiftBound() const -{ - // TODO(Horibe) write me. Real lane boundary must be considered here. - return parameters_->max_left_shift_length; -} - // TODO(murooka) judge when and which way to extend drivable area. current implementation is keep // extending during avoidance module // TODO(murooka) freespace during turning in intersection where there is no neighbour lanes @@ -2090,7 +2132,7 @@ void AvoidanceModule::modifyPathVelocityToPreventAccelerationOnAvoidance(Shifted // TODO(Horibe) clean up functions: there is a similar code in util as well. PathWithLaneId AvoidanceModule::calcCenterLinePath( - const std::shared_ptr & planner_data, const PoseStamped & pose) const + const std::shared_ptr & planner_data, const Pose & pose) const { const auto & p = planner_data->parameters; const auto & route_handler = planner_data->route_handler; @@ -2121,9 +2163,9 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( p.backward_path_length, longest_dist_to_shift_line, backward_length); const lanelet::ConstLanelets current_lanes = - util::calcLaneAroundPose(route_handler, pose.pose, p.forward_path_length, backward_length); + util::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); centerline_path = util::getCenterLinePath( - *route_handler, current_lanes, pose.pose, backward_length, p.forward_path_length, p); + *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); // for debug: check if the path backward distance is same as the desired length. // { @@ -2217,14 +2259,10 @@ boost::optional AvoidanceModule::calcIntersectionShiftLine( BehaviorModuleOutput AvoidanceModule::plan() { - DEBUG_PRINT("AVOIDANCE plan"); + const auto & data = avoidance_data_; resetPathCandidate(); - const auto shift_lines = calcShiftLines(current_raw_shift_lines_, debug_data_); - - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter_); - /** * Has new shift point? * Yes -> Is it approved? @@ -2234,26 +2272,20 @@ BehaviorModuleOutput AvoidanceModule::plan() * Yes -> clear WAIT_APPROVAL state. * No -> do nothing. */ - if (new_shift_lines) { - debug_data_.new_shift_lines = *new_shift_lines; - DEBUG_PRINT("new_shift_lines size = %lu", new_shift_lines->size()); - printShiftLines(*new_shift_lines, "new_shift_lines"); - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - if (new_shift_lines->at(i).getRelativeLength() > 0.0) { + if (!data.unapproved_new_sl.empty()) { + debug_data_.new_shift_lines = data.unapproved_new_sl; + DEBUG_PRINT("new_shift_lines size = %lu", data.unapproved_new_sl.size()); + printShiftLines(data.unapproved_new_sl, "new_shift_lines"); + + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + if (sl.getRelativeLength() > 0.0) { removePreviousRTCStatusRight(); - } else if (new_shift_lines->at(i).getRelativeLength() < 0.0) { + } else if (sl.getRelativeLength() < 0.0) { removePreviousRTCStatusLeft(); } else { RCLCPP_WARN_STREAM(getLogger(), "Direction is UNKNOWN"); } - addShiftLineIfApproved(*new_shift_lines); + addShiftLineIfApproved(data.unapproved_new_sl); } else if (isWaitingApproval()) { clearWaitingApproval(); removeCandidateRTCStatus(); @@ -2304,35 +2336,22 @@ BehaviorModuleOutput AvoidanceModule::plan() CandidateOutput AvoidanceModule::planCandidate() const { - DEBUG_PRINT("AVOIDANCE planCandidate start"); + const auto & data = avoidance_data_; + CandidateOutput output; - auto path_shifter = path_shifter_; - auto debug_data = debug_data_; - auto current_raw_shift_lines = current_raw_shift_lines_; + auto shifted_path = data.candidate_path; - const auto shift_lines = calcShiftLines(current_raw_shift_lines, debug_data); - const auto new_shift_lines = findNewShiftLine(shift_lines, path_shifter); - if (new_shift_lines) { - addNewShiftLines(path_shifter, *new_shift_lines); - } + if (!data.unapproved_new_sl.empty()) { // clip from shift start index for visualize + clipByMinStartIdx(data.unapproved_new_sl, shifted_path.path); - auto shifted_path = generateAvoidancePath(path_shifter); + const auto sl = getNonStraightShiftLine(data.unapproved_new_sl); + const auto sl_front = data.unapproved_new_sl.front(); + const auto sl_back = data.unapproved_new_sl.back(); - if (new_shift_lines) { // clip from shift start index for visualize - clipByMinStartIdx(*new_shift_lines, shifted_path.path); - - int i = new_shift_lines->size() - 1; - for (; i > 0; i--) { - if (fabs(new_shift_lines->at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } - output.lateral_shift = new_shift_lines->at(i).getRelativeLength(); - output.start_distance_to_path_change = new_shift_lines->front().start_longitudinal; - output.finish_distance_to_path_change = new_shift_lines->back().end_longitudinal; + output.lateral_shift = sl.getRelativeLength(); + output.start_distance_to_path_change = sl_front.start_longitudinal; + output.finish_distance_to_path_change = sl_back.end_longitudinal; const uint16_t steering_factor_direction = std::invoke([&output]() { if (output.lateral_shift > 0.0) { @@ -2341,7 +2360,7 @@ CandidateOutput AvoidanceModule::planCandidate() const return SteeringFactor::RIGHT; }); steering_factor_interface_ptr_->updateSteeringFactor( - {new_shift_lines->front().start, new_shift_lines->back().end}, + {sl_front.start, sl_back.end}, {output.start_distance_to_path_change, output.finish_distance_to_path_change}, SteeringFactor::AVOIDANCE_PATH_CHANGE, steering_factor_direction, SteeringFactor::APPROACHING, ""); @@ -2379,23 +2398,19 @@ void AvoidanceModule::addShiftLineIfApproved(const AvoidLineArray & shift_lines) const size_t prev_size = path_shifter_.getShiftLinesSize(); addNewShiftLines(path_shifter_, shift_lines); + current_raw_shift_lines_ = avoidance_data_.unapproved_raw_sl; + // register original points for consistency registerRawShiftLines(shift_lines); - int i = shift_lines.size() - 1; - for (; i > 0; i--) { - if (fabs(shift_lines.at(i).getRelativeLength()) < 0.01) { - continue; - } else { - break; - } - } + const auto sl = getNonStraightShiftLine(shift_lines); + const auto sl_front = shift_lines.front(); + const auto sl_back = shift_lines.back(); - if (shift_lines.at(i).getRelativeLength() > 0.0) { - left_shift_array_.push_back({uuid_left_, shift_lines.front().start, shift_lines.back().end}); - } else if (shift_lines.at(i).getRelativeLength() < 0.0) { - right_shift_array_.push_back( - {uuid_right_, shift_lines.front().start, shift_lines.back().end}); + if (sl.getRelativeLength() > 0.0) { + left_shift_array_.push_back({uuid_left_, sl_front.start, sl_back.end}); + } else if (sl.getRelativeLength() < 0.0) { + right_shift_array_.push_back({uuid_right_, sl_front.start, sl_back.end}); } uuid_left_ = generateUUID(); @@ -2448,7 +2463,7 @@ void AvoidanceModule::addNewShiftLines( path_shifter.setShiftLines(future); } -boost::optional AvoidanceModule::findNewShiftLine( +AvoidLineArray AvoidanceModule::findNewShiftLine( const AvoidLineArray & candidates, const PathShifter & shifter) const { (void)shifter; @@ -2530,25 +2545,7 @@ boost::optional AvoidanceModule::findNewShiftLine( return {}; } -double AvoidanceModule::getEgoSpeed() const -{ - return std::abs(planner_data_->self_odometry->twist.twist.linear.x); -} - -double AvoidanceModule::getNominalAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_nominal_avoidance_speed); -} -double AvoidanceModule::getSharpAvoidanceEgoSpeed() const -{ - return std::max(getEgoSpeed(), parameters_->min_sharp_avoidance_speed); -} - -Point AvoidanceModule::getEgoPosition() const { return planner_data_->self_pose->pose.position; } - -PoseStamped AvoidanceModule::getEgoPose() const { return *(planner_data_->self_pose); } - -PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const +Pose AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) const { const auto ego_pose = getEgoPose(); @@ -2557,43 +2554,17 @@ PoseStamped AvoidanceModule::getUnshiftedEgoPose(const ShiftedPath & prev_path) } // un-shifted fot current ideal pose - const auto closest = findNearestIndex(prev_path.path.points, ego_pose.pose.position); + const auto closest = findNearestIndex(prev_path.path.points, ego_pose.position); - PoseStamped unshifted_pose = ego_pose; - unshifted_pose.pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; + Pose unshifted_pose = ego_pose; + unshifted_pose.orientation = prev_path.path.points.at(closest).point.pose.orientation; - util::shiftPose(&unshifted_pose.pose, -prev_path.shift_length.at(closest)); - unshifted_pose.pose.orientation = ego_pose.pose.orientation; + util::shiftPose(&unshifted_pose, -prev_path.shift_length.at(closest)); + unshifted_pose.orientation = ego_pose.orientation; return unshifted_pose; } -double AvoidanceModule::getNominalAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->nominal_lateral_jerk, getNominalAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getSharpAvoidanceDistance(const double shift_length) const -{ - const auto & p = parameters_; - const auto distance_by_jerk = path_shifter_.calcLongitudinalDistFromJerk( - shift_length, parameters_->max_lateral_jerk, getSharpAvoidanceEgoSpeed()); - - return std::max(p->min_avoidance_distance, distance_by_jerk); -} - -double AvoidanceModule::getNominalPrepareDistance() const -{ - const auto & p = parameters_; - const auto epsilon_m = 0.01; // for floating error to pass "has_enough_distance" check. - const auto nominal_distance = std::max(getEgoSpeed() * p->prepare_time, p->min_prepare_distance); - return nominal_distance + epsilon_m; -} - ShiftedPath AvoidanceModule::generateAvoidancePath(PathShifter & path_shifter) const { DEBUG_PRINT("path_shifter: base shift = %f", getCurrentBaseShift()); @@ -2641,6 +2612,8 @@ void AvoidanceModule::updateData() if (prev_reference_.points.empty()) { prev_reference_ = avoidance_data_.reference_path; } + + fillShiftLine(avoidance_data_, debug_data_); } /* @@ -2882,16 +2855,6 @@ TurnSignalInfo AvoidanceModule::calcTurnSignalInfo(const ShiftedPath & path) con return turn_signal_info; } -double AvoidanceModule::getCurrentShift() const -{ - return prev_output_.shift_length.at(findNearestIndex(prev_output_.path.points, getEgoPosition())); -} -double AvoidanceModule::getCurrentLinearShift() const -{ - return prev_linear_shift_path_.shift_length.at( - findNearestIndex(prev_linear_shift_path_.path.points, getEgoPosition())); -} - void AvoidanceModule::setDebugData( const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const { From 0c93a2c25320e1511ddcba52155491d809c44bc7 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 14:46:46 +0900 Subject: [PATCH 36/37] fix(mission_planner): set goal in shoulder lane (#2548) * fix(mission_planner): set goal in shoulder lane Signed-off-by: kosuke55 * fix map without shoulder lane Signed-off-by: kosuke55 Signed-off-by: kosuke55 --- .../src/lanelet2_plugins/default_planner.cpp | 38 +++++++++---------- 1 file changed, 19 insertions(+), 19 deletions(-) diff --git a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp index b741bd4479763..55ed5a4cf57b0 100644 --- a/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp +++ b/planning/mission_planner/src/lanelet2_plugins/default_planner.cpp @@ -272,6 +272,25 @@ bool DefaultPlanner::is_goal_valid( const geometry_msgs::msg::Pose & goal, lanelet::ConstLanelets path_lanelets) { const auto logger = node_->get_logger(); + + const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); + + // check if goal is in shoulder lanelet + lanelet::Lanelet closest_shoulder_lanelet; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { + if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { + const auto lane_yaw = + lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); + const auto goal_yaw = tf2::getYaw(goal.orientation); + const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); + const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); + if (std::abs(angle_diff) < th_angle) { + return true; + } + } + } + lanelet::Lanelet closest_lanelet; if (!lanelet::utils::query::getClosestLanelet(road_lanelets_, goal, &closest_lanelet)) { return false; @@ -298,8 +317,6 @@ bool DefaultPlanner::is_goal_valid( return false; } - const auto goal_lanelet_pt = lanelet::utils::conversion::toLaneletPoint(goal.position); - if (is_in_lane(closest_lanelet, goal_lanelet_pt)) { const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_lanelet, goal.position); const auto goal_yaw = tf2::getYaw(goal.orientation); @@ -323,23 +340,6 @@ bool DefaultPlanner::is_goal_valid( return true; } - // check if goal is in shoulder lanelet - lanelet::Lanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - shoulder_lanelets_, goal, &closest_shoulder_lanelet)) { - return false; - } - // check if goal pose is in shoulder lane - if (is_in_lane(closest_shoulder_lanelet, goal_lanelet_pt)) { - const auto lane_yaw = lanelet::utils::getLaneletAngle(closest_shoulder_lanelet, goal.position); - const auto goal_yaw = tf2::getYaw(goal.orientation); - const auto angle_diff = tier4_autoware_utils::normalizeRadian(lane_yaw - goal_yaw); - - const double th_angle = tier4_autoware_utils::deg2rad(param_.goal_angle_threshold_deg); - if (std::abs(angle_diff) < th_angle) { - return true; - } - } return false; } From fea34b19001bb93fd127d07065c8b0b56fda48d0 Mon Sep 17 00:00:00 2001 From: Kosuke Takeuchi Date: Thu, 22 Dec 2022 15:51:02 +0900 Subject: [PATCH 37/37] feat(behavior_path_planner): plan shift pull over/out on curve (#2335) * feat(behavior_path_planner): plan shift pull over/out on curve Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 * Update planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp Co-authored-by: Fumiya Watanabe Signed-off-by: kosuke55 * remove pull_out_finish_judge_buffer Signed-off-by: kosuke55 * remove commentout Signed-off-by: kosuke55 Signed-off-by: kosuke55 Co-authored-by: Fumiya Watanabe --- .../motion_utils/trajectory/trajectory.hpp | 38 +++ planning/behavior_path_planner/README.md | 48 ++- .../config/pull_out/pull_out.param.yaml | 3 +- .../config/pull_over/pull_over.param.yaml | 1 - .../scene_module/pull_out/pull_out_module.hpp | 1 + .../pull_out/pull_out_parameters.hpp | 3 +- .../scene_module/pull_out/shift_pull_out.hpp | 3 + .../scene_module/pull_over/goal_searcher.hpp | 9 +- .../pull_over/goal_searcher_base.hpp | 11 +- .../pull_over/pull_over_module.hpp | 17 +- .../pull_over/pull_over_parameters.hpp | 1 - .../pull_over/pull_over_planner_base.hpp | 1 + .../pull_over/shift_pull_over.hpp | 19 +- .../scene_module/pull_over/util.hpp | 9 +- .../behavior_path_planner/utilities.hpp | 4 + .../src/behavior_path_planner_node.cpp | 4 +- .../scene_module/pull_out/pull_out_module.cpp | 44 ++- .../scene_module/pull_out/shift_pull_out.cpp | 199 ++++++----- .../scene_module/pull_over/goal_searcher.cpp | 114 +++++-- .../pull_over/pull_over_module.cpp | 230 ++++++------- .../pull_over/shift_pull_over.cpp | 316 +++++++++++------- .../src/scene_module/pull_over/util.cpp | 38 +-- .../utils/geometric_parallel_parking.cpp | 10 + .../behavior_path_planner/src/utilities.cpp | 55 +++ .../trajectory_analyzer.hpp | 2 +- .../include/planning_debug_tools/util.hpp | 15 - 26 files changed, 723 insertions(+), 472 deletions(-) diff --git a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp index 2a119fa02177d..f93e758c47332 100644 --- a/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp +++ b/common/motion_utils/include/motion_utils/trajectory/trajectory.hpp @@ -24,6 +24,7 @@ #include #include #include +#include #include namespace motion_utils @@ -580,6 +581,43 @@ double calcArcLength(const T & points) return calcSignedArcLength(points, 0, points.size() - 1); } +template +inline std::vector calcCurvature(const T & points) +{ + std::vector curvature_vec(points.size()); + + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + curvature_vec.at(i) = (tier4_autoware_utils::calcCurvature(p1, p2, p3)); + } + curvature_vec.at(0) = curvature_vec.at(1); + curvature_vec.at(curvature_vec.size() - 1) = curvature_vec.at(curvature_vec.size() - 2); + + return curvature_vec; +} + +template +inline std::vector> calcCurvatureAndArcLength(const T & points) +{ + // Note that arclength is for the segment, not the sum. + std::vector> curvature_arc_length_vec; + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + for (size_t i = 1; i < points.size() - 1; ++i) { + const auto p1 = tier4_autoware_utils::getPoint(points.at(i - 1)); + const auto p2 = tier4_autoware_utils::getPoint(points.at(i)); + const auto p3 = tier4_autoware_utils::getPoint(points.at(i + 1)); + const double curvature = tier4_autoware_utils::calcCurvature(p1, p2, p3); + const double arc_length = tier4_autoware_utils::calcDistance2d(points.at(i - 1), points.at(i)) + + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + curvature_arc_length_vec.push_back(std::pair(curvature, arc_length)); + } + curvature_arc_length_vec.push_back(std::pair(0.0, 0.0)); + + return curvature_arc_length_vec; +} + /** * @brief Calculate distance to the forward stop point from the given src index */ diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index d6bf2067c9cb7..b2a0f6085f532 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -271,15 +271,14 @@ The lateral jerk is searched for among the predetermined minimum and maximum val ###### Parameters for shift parking -| Name | Unit | Type | Description | Default value | -| :--------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | -| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | -| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | -| before_pull_over_straight_distance | [m] | double | straight line distance before pull over end point. a safe path should have a distance that includes this | 5.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------ | :------------ | +| enable_shift_parking | [-] | bool | flag whether to enable shift parking | true | +| pull_over_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| deceleration_interval | [m] | double | distance of deceleration section | 15.0 | +| after_pull_over_straight_distance | [m] | double | straight line distance after pull over end point | 5.0 | ##### **geometric parallel parking** @@ -362,13 +361,13 @@ The Pull Out module is activated when ego-vehicle is stationary and footprint of #### General parameters for pull_out -| Name | Unit | Type | Description | Default value | -| :--------------------------- | :---- | :----- | :------------------------------------------------------------- | :------------ | -| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | -| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | -| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | -| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | -| pull_out_finish_judge_buffer | [m] | double | threshold for finish judgment distance from pull out end point | 1.0 | +| Name | Unit | Type | Description | Default value | +| :-------------------------------- | :---- | :----- | :------------------------------------------------------------------- | :------------ | +| th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | +| th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | +| th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | +| collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 15.0 | #### **Safe check with obstacles in shoulder lane** @@ -396,15 +395,14 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral ###### parameters for shift pull out -| Name | Unit | Type | Description | Default value | -| :-------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | -| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | -| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | -| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | -| before_pull_out_straight_distance | [m] | double | distance before pull out start point | 0.0 | -| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | -| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | -| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ | +| enable_shift_pull_out | - | bool | flag whether to enable shift pull out | true | +| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 | +| pull_out_sampling_num | - | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 | +| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 | +| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.5 | +| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 20.0 | ##### **geometric pull out** diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 7a9a63f805d67..b092eb7fc8802 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -5,12 +5,11 @@ th_stopped_velocity: 0.01 th_stopped_time: 1.0 collision_check_margin: 1.0 - pull_out_finish_judge_buffer: 1.0 + collision_check_distance_from_end: 1.0 # shift pull out enable_shift_pull_out: true shift_pull_out_velocity: 2.0 pull_out_sampling_num: 4 - before_pull_out_straight_distance: 0.0 minimum_shift_pull_out_distance: 20.0 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index 8e277959bc069..75b5facaaaafe 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -35,7 +35,6 @@ minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 after_pull_over_straight_distance: 5.0 - before_pull_over_straight_distance: 5.0 # parallel parking path enable_arc_forward_parking: true enable_arc_backward_parking: true diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 92cd395e159dd..9dbd977a1a95d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -114,6 +114,7 @@ class PullOutModule : public SceneModuleInterface const std::vector & start_pose_candidates, const Pose & goal_pose); void planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose); + void generateStopPath(); void updatePullOutStatus(); static bool isOverlappedWithLane( const lanelet::ConstLanelet & candidate_lanelet, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp index 96f76278e50df..21b66d1ad8457 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_parameters.hpp @@ -26,12 +26,11 @@ struct PullOutParameters double th_stopped_velocity; double th_stopped_time; double collision_check_margin; - double pull_out_finish_judge_buffer; + double collision_check_distance_from_end; // shift pull out bool enable_shift_pull_out; double shift_pull_out_velocity; int pull_out_sampling_num; - double before_pull_out_straight_distance; double minimum_shift_pull_out_distance; double maximum_lateral_jerk; double minimum_lateral_jerk; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp index 4f43bb080fabd..7181f9bd8e9bf 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/shift_pull_out.hpp @@ -49,6 +49,9 @@ class ShiftPullOut : public PullOutPlannerBase const double pull_out_total_distance, const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, const bool isInGoalRouteSection, const Pose & goal_pose); + double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr); + std::shared_ptr lane_departure_checker_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp index 5ab414f0cb90e..f26d0e4cdebfe 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher.hpp @@ -18,11 +18,14 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp" #include "behavior_path_planner/scene_module/utils/occupancy_grid_based_collision_detector.hpp" +#include "autoware_auto_planning_msgs/msg/path_point_with_lane_id.hpp" + #include #include namespace behavior_path_planner { +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using tier4_autoware_utils::LinearRing2d; class GoalSearcher : public GoalSearcherBase @@ -32,12 +35,14 @@ class GoalSearcher : public GoalSearcherBase const PullOverParameters & parameters, const LinearRing2d & vehicle_footprint, const std::shared_ptr & occupancy_grid_map); - std::vector search(const Pose & original_goal_pose) override; + GoalCandidates search(const Pose & original_goal_pose) override; + +private: + void createAreaPolygons(std::vector original_search_poses); bool checkCollision(const Pose & pose) const; bool checkCollisionWithLongitudinalDistance( const Pose & ego_pose, const PredictedObjects & dynamic_objects) const; -private: LinearRing2d vehicle_footprint_{}; std::shared_ptr occupancy_grid_map_{}; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp index 261e586c7c123..2672d3c659500 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/goal_searcher_base.hpp @@ -28,22 +28,21 @@ namespace behavior_path_planner { using geometry_msgs::msg::Pose; +using tier4_autoware_utils::MultiPolygon2d; struct GoalCandidate { Pose goal_pose{}; double distance_from_original_goal{0.0}; double lateral_offset{0.0}; + size_t id{0}; bool operator<(const GoalCandidate & other) const noexcept { - // compare in order of decreasing lateral offset. - if (std::abs(lateral_offset - other.lateral_offset) > std::numeric_limits::epsilon()) { - return lateral_offset < other.lateral_offset; - } return distance_from_original_goal < other.distance_from_original_goal; } }; +using GoalCandidates = std::vector; class GoalSearcherBase { @@ -56,11 +55,13 @@ class GoalSearcherBase planner_data_ = planner_data; } - virtual std::vector search(const Pose & original_goal_pose) = 0; + MultiPolygon2d getAreaPolygons() { return area_polygons_; } + virtual GoalCandidates search(const Pose & original_goal_pose) = 0; protected: PullOverParameters parameters_; std::shared_ptr planner_data_; + MultiPolygon2d area_polygons_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp index 7047c102b5bf2..831d185cb3889 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_module.hpp @@ -65,6 +65,7 @@ struct PUllOverStatus bool prev_is_safe{false}; bool has_decided_velocity{false}; bool has_requested_approval{false}; + std::optional stop_pose{}; }; class PullOverModule : public SceneModuleInterface @@ -103,9 +104,6 @@ class PullOverModule : public SceneModuleInterface PullOverPath shift_parking_path_; vehicle_info_util::VehicleInfo vehicle_info_; - const double pull_over_lane_length_{200.0}; - const double check_distance_{100.0}; - rclcpp::Subscription::SharedPtr occupancy_grid_sub_; rclcpp::Publisher::SharedPtr goal_pose_pub_; @@ -113,7 +111,7 @@ class PullOverModule : public SceneModuleInterface std::shared_ptr occupancy_grid_map_; Pose modified_goal_pose_; Pose refined_goal_pose_; - std::vector goal_candidates_; + GoalCandidates goal_candidates_; GeometricParallelParking parallel_parking_planner_; ParallelParkingParameters parallel_parking_parameters_; std::deque odometry_buffer_; @@ -125,16 +123,11 @@ class PullOverModule : public SceneModuleInterface void incrementPathIndex(); PathWithLaneId getCurrentPath() const; PathWithLaneId getFullPath() const; - PathWithLaneId getReferencePath() const; - PathWithLaneId generateStopPath() const; - Pose calcRefinedGoal() const; + Pose calcRefinedGoal(const Pose & goal_pose) const; ParallelParkingParameters getGeometricPullOverParameters() const; - bool isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const; - bool isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer = 0) const; - double calcMinimumShiftPathDistance() const; std::pair calcDistanceToPathChange() const; + PathWithLaneId generateStopPath(); + PathWithLaneId generateEmergencyStopPath(); bool isStopped(); bool hasFinishedCurrentPath(); diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp index 5dc0876877e81..e0c7d5b86f9f1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_parameters.hpp @@ -56,7 +56,6 @@ struct PullOverParameters double pull_over_velocity; double pull_over_minimum_velocity; double after_pull_over_straight_distance; - double before_pull_over_straight_distance; // parallel parking bool enable_arc_forward_parking; bool enable_arc_backward_parking; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp index cfb4e488da1da..f6614595097ad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/pull_over_planner_base.hpp @@ -47,6 +47,7 @@ struct PullOverPath std::vector partial_paths{}; Pose start_pose{}; Pose end_pose{}; + std::vector debug_poses{}; }; class PullOverPlannerBase diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp index 19f723bd82385..3aaed89e2cca9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/shift_pull_over.hpp @@ -41,21 +41,22 @@ class ShiftPullOver : public PullOverPlannerBase boost::optional plan(const Pose & goal_pose) override; protected: - PathWithLaneId generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const; - PathWithLaneId generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const; + PathWithLaneId generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const; boost::optional generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const; + const Pose & goal_pose, const double lateral_jerk) const; bool hasEnoughDistance( const PathWithLaneId & path, const lanelet::ConstLanelets & road_lanes, const Pose & start_pose, const Pose & goal_pose, const double pull_over_distance) const; bool isSafePath(const PathWithLaneId & path) const; + static double calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double after_shifted_arc_length, const double dr); + static std::vector splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s); + static std::vector interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval); LaneDepartureChecker lane_departure_checker_{}; std::shared_ptr occupancy_grid_map_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp index 343938898e7b9..2dbef9948f368 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_over/util.hpp @@ -50,16 +50,15 @@ PredictedObjects filterObjectsByLateralDistance( const double distance_thresh, const bool filter_inside); // debug -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color); +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z); MarkerArray createPosesMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createTextsMarkerArray( const std::vector & poses, std::string && ns, const std_msgs::msg::ColorRGBA & color); MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color); + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color); } // namespace pull_over_utils } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index 2de16855391ff..c1f9f05587a93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -67,6 +67,7 @@ using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::Twist; using geometry_msgs::msg::Vector3; using route_handler::RouteHandler; +using tier4_autoware_utils::LinearRing2d; using tier4_autoware_utils::LineString2d; using tier4_autoware_utils::Point2d; using tier4_autoware_utils::Polygon2d; @@ -358,6 +359,9 @@ PathPointWithLaneId insertStopPoint(double length, PathWithLaneId * path); double getSignedDistanceFromShoulderLeftBoundary( const lanelet::ConstLanelets & shoulder_lanelets, const Pose & pose); +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose); double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 5f5de72b26160..9638de981f81a 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -449,7 +449,6 @@ PullOverParameters BehaviorPathPlannerNode::getPullOverParam() p.pull_over_velocity = dp("pull_over_velocity", 8.3); p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); - p.before_pull_over_straight_distance = dp("before_pull_over_straight_distance", 3.0); // parallel parking p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); @@ -512,12 +511,11 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); p.th_stopped_time = dp("th_stopped_time", 1.0); p.collision_check_margin = dp("collision_check_margin", 1.0); - p.pull_out_finish_judge_buffer = dp("pull_out_finish_judge_buffer", 1.0); + p.collision_check_distance_from_end = dp("collision_check_distance_from_end", 3.0); // shift pull out p.enable_shift_pull_out = dp("enable_shift_pull_out", true); p.shift_pull_out_velocity = dp("shift_pull_out_velocity", 8.3); p.pull_out_sampling_num = dp("pull_out_sampling_num", 4); - p.before_pull_out_straight_distance = dp("before_pull_out_straight_distance", 3.0); p.minimum_shift_pull_out_distance = dp("minimum_shift_pull_out_distance", 20.0); p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index e89ad45cdadd9..79c1a84fafce3 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -31,6 +31,7 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; + namespace behavior_path_planner { PullOutModule::PullOutModule( @@ -173,6 +174,7 @@ BehaviorModuleOutput PullOutModule::plan() BehaviorModuleOutput output; if (!status_.is_safe) { + RCLCPP_INFO(getLogger(), "not found safe path"); return output; } @@ -370,6 +372,7 @@ void PullOutModule::planWithPriorityOnEfficientPath( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; // plan with each planner for (const auto & planner : pull_out_planners_) { @@ -401,6 +404,7 @@ void PullOutModule::planWithPriorityOnShortBackDistance( const std::vector & start_pose_candidates, const Pose & goal_pose) { status_.is_safe = false; + status_.planner_type = PlannerType::NONE; for (size_t i = 0; i < start_pose_candidates.size(); ++i) { // pull out start pose is current_pose @@ -428,6 +432,33 @@ void PullOutModule::planWithPriorityOnShortBackDistance( } } +void PullOutModule::generateStopPath() +{ + const auto & current_pose = planner_data_->self_pose->pose; + constexpr double dummy_path_distance = 1.0; + const auto & moved_pose = calcOffsetPose(current_pose, dummy_path_distance, 0, 0); + + // convert Pose to PathPointWithLaneId with 0 velocity. + auto toPathPointWithLaneId = [this](const Pose & pose) { + PathPointWithLaneId p{}; + p.point.pose = pose; + p.point.longitudinal_velocity_mps = 0.0; + lanelet::Lanelet closest_shoulder_lanelet; + lanelet::utils::query::getClosestLanelet( + planner_data_->route_handler->getShoulderLanelets(), pose, &closest_shoulder_lanelet); + p.lane_ids.push_back(closest_shoulder_lanelet.id()); + return p; + }; + + PathWithLaneId path{}; + path.points.push_back(toPathPointWithLaneId(current_pose)); + path.points.push_back(toPathPointWithLaneId(moved_pose)); + + status_.pull_out_path.partial_paths.push_back(path); + status_.pull_out_path.start_pose = current_pose; + status_.pull_out_path.end_pose = current_pose; +} + void PullOutModule::updatePullOutStatus() { const auto & route_handler = planner_data_->route_handler; @@ -462,12 +493,14 @@ void PullOutModule::updatePullOutStatus() getLogger(), "search_priority should be efficient_path or short_back_distance, but %s is given.", parameters_.search_priority.c_str()); + throw std::domain_error("[pull_out] invalid search_priority"); } if (!status_.is_safe) { - RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Not found safe pull out path"); - status_.is_safe = false; - return; + RCLCPP_WARN_THROTTLE( + getLogger(), *clock_, 5000, "Not found safe pull out path, generate stop path"); + status_.back_finished = true; // no need to drive backward + generateStopPath(); } checkBackFinished(); @@ -550,8 +583,7 @@ bool PullOutModule::hasFinishedPullOut() const lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); // has passed pull out end point - return arclength_current.length - arclength_pull_out_end.length > - parameters_.pull_out_finish_judge_buffer; + return arclength_current.length - arclength_pull_out_end.length > 0.0; } void PullOutModule::checkBackFinished() @@ -633,7 +665,7 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const // pull out path does not overlap const double distance_from_end = motion_utils::calcSignedArcLength( path.points, status_.pull_out_path.end_pose.position, current_pose.position); - if (distance_from_end < parameters_.pull_out_finish_judge_buffer) { + if (distance_from_end < 0.0) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp index b72558260ed75..d16d69839ba36 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/shift_pull_out.cpp @@ -47,10 +47,6 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) return boost::none; } - const auto drivable_lanes = - util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); - const auto lanes = util::transformToLanelets(drivable_lanes); - // find candidate paths auto pull_out_paths = calcPullOutPaths( *route_handler, road_lanes, shoulder_lanes, start_pose, goal_pose, common_parameters, @@ -71,15 +67,33 @@ boost::optional ShiftPullOut::plan(Pose start_pose, Pose goal_pose) // check lane_departure and collision with path between pull_out_start to pull_out_end PathWithLaneId path_start_to_end{}; { - const auto pull_out_start_idx = findNearestIndex(shift_path.points, start_pose); - const auto pull_out_end_idx = findNearestIndex(shift_path.points, pull_out_path.end_pose); + const size_t pull_out_start_idx = findNearestIndex(shift_path.points, start_pose.position); + + // calculate collision check end idx + const size_t collision_check_end_idx = std::invoke([&]() { + const auto collision_check_end_pose = motion_utils::calcLongitudinalOffsetPose( + shift_path.points, pull_out_path.end_pose.position, + parameters_.collision_check_distance_from_end); + + if (collision_check_end_pose) { + return findNearestIndex(shift_path.points, collision_check_end_pose->position); + } else { + return shift_path.points.size() - 1; + } + }); path_start_to_end.points.insert( - path_start_to_end.points.begin(), shift_path.points.begin() + *pull_out_start_idx, - shift_path.points.begin() + *pull_out_end_idx + 1); + path_start_to_end.points.begin(), shift_path.points.begin() + pull_out_start_idx, + shift_path.points.begin() + collision_check_end_idx + 1); } // check lane departure - if (lane_departure_checker_->checkPathWillLeaveLane(lanes, path_start_to_end)) { + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_->checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), path_start_to_end)) { continue; } @@ -108,7 +122,7 @@ std::vector ShiftPullOut::calcPullOutPaths( const lanelet::ConstLanelets & shoulder_lanes, const Pose & start_pose, const Pose & goal_pose, const BehaviorPathPlannerParameters & common_parameter, const PullOutParameters & parameter) { - std::vector candidate_paths; + std::vector candidate_paths{}; if (road_lanes.empty() || shoulder_lanes.empty()) { return candidate_paths; @@ -117,7 +131,6 @@ std::vector ShiftPullOut::calcPullOutPaths( // rename parameter const double backward_path_length = common_parameter.backward_path_length; const double shift_pull_out_velocity = parameter.shift_pull_out_velocity; - const double before_pull_out_straight_distance = parameter.before_pull_out_straight_distance; const double minimum_shift_pull_out_distance = parameter.minimum_shift_pull_out_distance; const double minimum_lateral_jerk = parameter.minimum_lateral_jerk; const double maximum_lateral_jerk = parameter.maximum_lateral_jerk; @@ -127,64 +140,59 @@ std::vector ShiftPullOut::calcPullOutPaths( for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { - PathShifter path_shifter; - const double distance_to_road_center = getArcCoordinates(road_lanes, start_pose).distance; + // lateral distance from road center to start pose + const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + + PathWithLaneId road_lane_reference_path{}; + { + const auto arc_position = getArcCoordinates(road_lanes, start_pose); + const double s_start = std::max(arc_position.length - backward_path_length, 0.0); + const auto arc_position_goal = getArcCoordinates(road_lanes, goal_pose); + double s_end = arc_position_goal.length; + road_lane_reference_path = util::resamplePathWithSpline( + route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); + } + PathShifter path_shifter{}; + path_shifter.setPath(road_lane_reference_path); + + // calculate after/before shifted pull out distance const double pull_out_distance = std::max( - path_shifter.calcLongitudinalDistFromJerk( - abs(distance_to_road_center), lateral_jerk, shift_pull_out_velocity), + PathShifter::calcLongitudinalDistFromJerk( + abs(shift_length), lateral_jerk, shift_pull_out_velocity), minimum_shift_pull_out_distance); + const size_t shift_start_idx = + findNearestIndex(road_lane_reference_path.points, start_pose.position); + PathWithLaneId road_lane_reference_path_from_ego = road_lane_reference_path; + road_lane_reference_path_from_ego.points.erase( + road_lane_reference_path_from_ego.points.begin(), + road_lane_reference_path_from_ego.points.begin() + shift_start_idx); + // before means distance on road lane + const double before_shifted_pull_out_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_from_ego, pull_out_distance, shift_length); // check has enough distance - const double pull_out_total_distance = before_pull_out_straight_distance + pull_out_distance; const bool is_in_goal_route_section = route_handler.isInGoalRouteSection(road_lanes.back()); if (!hasEnoughDistance( - pull_out_total_distance, road_lanes, start_pose, is_in_goal_route_section, goal_pose)) { + before_shifted_pull_out_distance, road_lanes, start_pose, is_in_goal_route_section, + goal_pose)) { continue; } - PathWithLaneId shoulder_reference_path; - { - const auto arc_position = getArcCoordinates(shoulder_lanes, start_pose); - const double s_start = arc_position.length - backward_path_length; - double s_end = arc_position.length + before_pull_out_straight_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); - shoulder_reference_path = route_handler.getCenterLinePath(shoulder_lanes, s_start, s_end); - - // lateral shift to start_pose - for (auto & p : shoulder_reference_path.points) { - p.point.pose = calcOffsetPose(p.point.pose, 0, arc_position.distance, 0); - } - } - - PathWithLaneId road_lane_reference_path; - { - const lanelet::ArcCoordinates arc_position_shift = - getArcCoordinates(road_lanes, shoulder_reference_path.points.back().point.pose); - const lanelet::ArcCoordinates arc_position_goal = getArcCoordinates(road_lanes, goal_pose); - - double s_start = arc_position_shift.length; - double s_end = arc_position_goal.length; - road_lane_reference_path = route_handler.getCenterLinePath(road_lanes, s_start, s_end); - } - path_shifter.setPath(road_lane_reference_path); - - // get shift point start/end - const auto shift_line_start = shoulder_reference_path.points.back(); - const auto shift_line_end = [&]() { + // get shift end pose + const auto shift_end_pose = std::invoke([&]() { const auto arc_position_shift_start = - lanelet::utils::getArcCoordinates(road_lanes, shift_line_start.point.pose); - const double s_start = arc_position_shift_start.length + pull_out_distance; + lanelet::utils::getArcCoordinates(road_lanes, start_pose); + const double s_start = arc_position_shift_start.length + before_shifted_pull_out_distance; const double s_end = s_start + std::numeric_limits::epsilon(); const auto path = route_handler.getCenterLinePath(road_lanes, s_start, s_end, true); - return path.points.front(); - }(); - - ShiftLine shift_line; - { - shift_line.start = shift_line_start.point.pose; - shift_line.end = shift_line_end.point.pose; - shift_line.end_shift_length = distance_to_road_center; - } + return path.points.front().point.pose; + }); + + // create shift line + ShiftLine shift_line{}; + shift_line.start = start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_length; path_shifter.addShiftLine(shift_line); // offset front side @@ -194,43 +202,28 @@ std::vector ShiftPullOut::calcPullOutPaths( continue; } - const auto pull_out_end_idx = - findNearestIndex(shifted_path.path.points, shift_line_end.point.pose); - const auto goal_idx = findNearestIndex(shifted_path.path.points, goal_pose); - - if (pull_out_end_idx && goal_idx) { - // set velocity - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { - auto & point = shifted_path.path.points.at(i); - if (i < *pull_out_end_idx) { - point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); - continue; - } else if (i > *goal_idx) { - point.point.longitudinal_velocity_mps = 0.0; - continue; - } + // set velocity + const size_t pull_out_end_idx = + findNearestIndex(shifted_path.path.points, shift_end_pose.position); + const size_t goal_idx = findNearestIndex(shifted_path.path.points, goal_pose.position); + for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + auto & point = shifted_path.path.points.at(i); + if (i < pull_out_end_idx) { + point.point.longitudinal_velocity_mps = std::min( + point.point.longitudinal_velocity_mps, static_cast(shift_pull_out_velocity)); + continue; + } else if (i >= goal_idx) { + point.point.longitudinal_velocity_mps = 0.0; + continue; } - - PullOutPath candidate_path; - const auto combined_path = combineReferencePath(shoulder_reference_path, shifted_path.path); - candidate_path.partial_paths.push_back(util::resamplePathWithSpline(combined_path, 1.0)); - candidate_path.start_pose = shift_line.start; - candidate_path.end_pose = shift_line.end; - - // add goal pose because resampling removes it - // but this point will be removed by SmoothGoalConnection again - PathPointWithLaneId goal_path_point = shifted_path.path.points.back(); - goal_path_point.point.pose = goal_pose; - goal_path_point.point.longitudinal_velocity_mps = 0.0; - candidate_path.partial_paths.front().points.push_back(goal_path_point); - candidate_paths.push_back(candidate_path); - } else { - RCLCPP_ERROR_STREAM( - rclcpp::get_logger("behavior_path_planner").get_child("pull_out").get_child("util"), - "lane change end idx not found on target path."); - continue; } + + // add shifted path to candidates + PullOutPath candidate_path; + candidate_path.partial_paths.push_back(shifted_path.path); + candidate_path.start_pose = shift_line.start; + candidate_path.end_pose = shift_line.end; + candidate_paths.push_back(candidate_path); } return candidate_paths; @@ -255,4 +248,26 @@ bool ShiftPullOut::hasEnoughDistance( return true; } +double ShiftPullOut::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + double before_arc_length{0.0}; + double after_arc_length{0.0}; + + for (const auto & [k, segment_length] : motion_utils::calcCurvatureAndArcLength(path.points)) { + // after shifted segment length + const double after_segment_length = + k < 0 ? segment_length * (1 - k * dr) : segment_length / (1 + k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k < 0 ? offset / (1 - k * dr) : offset * (1 + k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp index af3abb2d10e48..dea04a5ab85fe 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/goal_searcher.cpp @@ -14,7 +14,9 @@ #include "behavior_path_planner/scene_module/pull_over/goal_searcher.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/pull_over/util.hpp" +#include "lanelet2_extension/utility/utilities.hpp" #include @@ -36,53 +38,78 @@ GoalSearcher::GoalSearcher( { } -std::vector GoalSearcher::search(const Pose & original_goal_pose) +GoalCandidates GoalSearcher::search(const Pose & original_goal_pose) { - std::vector goal_candidates{}; + GoalCandidates goal_candidates{}; - const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*(planner_data_->route_handler)); + const auto & route_handler = planner_data_->route_handler; + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double forward_length = parameters_.forward_goal_search_length; + const double backward_length = parameters_.backward_goal_search_length; + const double margin_from_boundary = parameters_.margin_from_boundary; + const double lateral_offset_interval = parameters_.lateral_offset_interval; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto pull_over_lanes = pull_over_utils::getPullOverLanes(*route_handler); auto lanes = util::getExtendedCurrentLanes(planner_data_); lanes.insert(lanes.end(), pull_over_lanes.begin(), pull_over_lanes.end()); + const auto goal_arc_coords = + lanelet::utils::getArcCoordinates(pull_over_lanes, original_goal_pose); + const double s_start = std::max(0.0, goal_arc_coords.length - backward_length); + const double s_end = goal_arc_coords.length + forward_length; + auto center_line_path = util::resamplePathWithSpline( + route_handler->getCenterLinePath(pull_over_lanes, s_start, s_end), + parameters_.goal_search_interval); + const auto shoulder_lane_objects = util::filterObjectsByLanelets(*(planner_data_->dynamic_object), pull_over_lanes); - for (double dx = -parameters_.backward_goal_search_length; - dx <= parameters_.forward_goal_search_length; dx += parameters_.goal_search_interval) { - // search goal_pose in lateral direction + std::vector original_search_poses{}; + for (size_t goal_id = 0; goal_id < center_line_path.points.size(); ++goal_id) { + const auto & center_pose = center_line_path.points.at(goal_id).point.pose; + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + pull_over_lanes, vehicle_footprint_, center_pose); + if (!distance_from_left_bound) continue; + + const double offset_from_center_line = distance_from_left_bound.value() + margin_from_boundary; + const Pose original_search_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + original_search_poses.push_back(original_search_pose); // for createAreaPolygon Pose search_pose{}; + // search goal_pose in lateral direction bool found_lateral_no_collision_pose = false; double lateral_offset = 0.0; - for (double dy = 0; dy <= parameters_.max_lateral_offset; - dy += parameters_.lateral_offset_interval) { + for (double dy = 0; dy <= max_lateral_offset; dy += lateral_offset_interval) { lateral_offset = dy; - search_pose = calcOffsetPose(original_goal_pose, dx, -dy, 0); + search_pose = calcOffsetPose(original_search_pose, 0, -dy, 0); - const auto & transformed_vehicle_footprint = + const auto transformed_vehicle_footprint = transformVector(vehicle_footprint_, tier4_autoware_utils::pose2transform(search_pose)); if (LaneDepartureChecker::isOutOfLane(lanes, transformed_vehicle_footprint)) { continue; } - if (checkCollision(search_pose)) { continue; } - // if finding objects near the search pose, + // if finding objects or detecting lane departure // shift search_pose in lateral direction one more - // because collision may be detected on other path points - if (dy > 0) { - search_pose = calcOffsetPose(search_pose, 0, -parameters_.lateral_offset_interval, 0); + // for avoiding them on other path points. + if (dy > 0 && dy < max_lateral_offset) { + search_pose = calcOffsetPose(search_pose, 0, -lateral_offset_interval, 0); } found_lateral_no_collision_pose = true; break; } - if (!found_lateral_no_collision_pose) continue; + if (!found_lateral_no_collision_pose) { + continue; + } constexpr bool filter_inside = true; const auto target_objects = pull_over_utils::filterObjectsByLateralDistance( - search_pose, planner_data_->parameters.vehicle_width, shoulder_lane_objects, + search_pose, vehicle_width, shoulder_lane_objects, parameters_.object_recognition_collision_check_margin, filter_inside); if (checkCollisionWithLongitudinalDistance(search_pose, target_objects)) { continue; @@ -91,13 +118,14 @@ std::vector GoalSearcher::search(const Pose & original_goal_pose) GoalCandidate goal_candidate{}; goal_candidate.goal_pose = search_pose; goal_candidate.lateral_offset = lateral_offset; + goal_candidate.id = goal_id; // use longitudinal_distance as distance_from_original_goal - // TODO(kosuke55): use arc length for curve lane - goal_candidate.distance_from_original_goal = - std::abs(inverseTransformPose(search_pose, original_goal_pose).position.x); - + goal_candidate.distance_from_original_goal = std::abs(motion_utils::calcSignedArcLength( + center_line_path.points, original_goal_pose.position, search_pose.position)); goal_candidates.push_back(goal_candidate); } + createAreaPolygons(original_search_poses); + // Sort with distance from original goal std::sort(goal_candidates.begin(), goal_candidates.end()); @@ -169,4 +197,48 @@ bool GoalSearcher::checkCollisionWithLongitudinalDistance( return false; } +void GoalSearcher::createAreaPolygons(std::vector original_search_poses) +{ + using tier4_autoware_utils::MultiPolygon2d; + using tier4_autoware_utils::Point2d; + using tier4_autoware_utils::Polygon2d; + + const double vehicle_width = planner_data_->parameters.vehicle_width; + const double base_link2front = planner_data_->parameters.base_link2front; + const double base_link2rear = planner_data_->parameters.base_link2rear; + const double max_lateral_offset = parameters_.max_lateral_offset; + + const auto appendPointToPolygon = + [](Polygon2d & polygon, const geometry_msgs::msg::Point & geom_point) { + Point2d point{}; + point.x() = geom_point.x; + point.y() = geom_point.y; + boost::geometry::append(polygon.outer(), point); + }; + + boost::geometry::clear(area_polygons_); + for (const auto p : original_search_poses) { + Polygon2d footprint{}; + const Point p_left_front = calcOffsetPose(p, base_link2front, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_front); + + const Point p_right_front = + calcOffsetPose(p, base_link2front, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_front); + + const Point p_right_back = + calcOffsetPose(p, -base_link2rear, -vehicle_width / 2 - max_lateral_offset, 0).position; + appendPointToPolygon(footprint, p_right_back); + + const Point p_left_back = calcOffsetPose(p, -base_link2rear, vehicle_width / 2, 0).position; + appendPointToPolygon(footprint, p_left_back); + + appendPointToPolygon(footprint, p_left_front); + + MultiPolygon2d current_result{}; + boost::geometry::union_(footprint, area_polygons_, current_result); + area_polygons_ = current_result; + } +} + } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp index 54b78686b1e2a..6544c581d2169 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/pull_over_module.cpp @@ -20,6 +20,7 @@ #include "behavior_path_planner/util/create_vehicle_footprint.hpp" #include "behavior_path_planner/utilities.hpp" +#include #include #include #include @@ -32,6 +33,7 @@ #include #include +using motion_utils::calcLongitudinalOffsetPose; using motion_utils::calcSignedArcLength; using motion_utils::findFirstNearestSegmentIndexWithSoftConstraints; using nav_msgs::msg::OccupancyGrid; @@ -166,7 +168,7 @@ void PullOverModule::onEntry() std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); // Use refined goal as modified goal when disabling goal research - refined_goal_pose_ = calcRefinedGoal(); + refined_goal_pose_ = calcRefinedGoal(planner_data_->route_handler->getGoalPose()); if (!parameters_.enable_goal_research) { goal_candidates_.clear(); GoalCandidate goal_candidate{}; @@ -246,24 +248,47 @@ bool PullOverModule::isExecutionRequested() const bool PullOverModule::isExecutionReady() const { return true; } -Pose PullOverModule::calcRefinedGoal() const +Pose PullOverModule::calcRefinedGoal(const Pose & goal_pose) const { - lanelet::ConstLanelet goal_lane; - Pose goal_pose = planner_data_->route_handler->getGoalPose(); - lanelet::Lanelet closest_shoulder_lanelet; lanelet::utils::query::getClosestLanelet( planner_data_->route_handler->getShoulderLanelets(), goal_pose, &closest_shoulder_lanelet); - const Pose center_pose = - lanelet::utils::getClosestCenterPose(closest_shoulder_lanelet, goal_pose.position); - - const double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - planner_data_->route_handler->getShoulderLanelets(), center_pose); - const double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - const Pose refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); + // calc closest center line pose + Pose center_pose; + { + // find position + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(goal_pose.position); + const auto segment = lanelet::utils::getClosestSegment( + lanelet::utils::to2D(lanelet_point), closest_shoulder_lanelet.centerline()); + const auto p1 = segment.front().basicPoint(); + const auto p2 = segment.back().basicPoint(); + const auto direction_vector = (p2 - p1).normalized(); + const auto p1_to_goal = lanelet_point.basicPoint() - p1; + const double s = direction_vector.dot(p1_to_goal); + const auto refined_point = p1 + direction_vector * s; + + center_pose.position.x = refined_point.x(); + center_pose.position.y = refined_point.y(); + center_pose.position.z = refined_point.z(); + + // find orientation + const double yaw = std::atan2(direction_vector.y(), direction_vector.x()); + tf2::Quaternion tf_quat; + tf_quat.setRPY(0, 0, yaw); + center_pose.orientation = tf2::toMsg(tf_quat); + } + + const auto distance_from_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( + planner_data_->route_handler->getShoulderLanelets(), vehicle_footprint_, center_pose); + if (!distance_from_left_bound) { + RCLCPP_ERROR(getLogger(), "fail to calculate refined goal"); + return goal_pose; + } + + const double offset_from_center_line = + distance_from_left_bound.value() + parameters_.margin_from_boundary; + const auto refined_goal_pose = calcOffsetPose(center_pose, 0, -offset_from_center_line, 0); return refined_goal_pose; } @@ -280,31 +305,6 @@ BT::NodeStatus PullOverModule::updateState() return current_state_; } -bool PullOverModule::isLongEnoughToParkingStart( - const PathWithLaneId & path, const Pose & parking_start_pose) const -{ - const auto dist_to_parking_start_pose = calcSignedArcLength( - path.points, planner_data_->self_pose->pose, parking_start_pose.position, - std::numeric_limits::max(), M_PI_2); - if (!dist_to_parking_start_pose) return false; - - const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - - // once stopped, it cannot start again if start_pose is close. - // so need enough distance to restart - const double eps_vel = 0.01; - // dist to restart should be less than decide_path_distance. - // otherwise, the goal would change immediately after departure. - const double dist_to_restart = parameters_.decide_path_distance / 2; - if (std::abs(current_vel) < eps_vel && *dist_to_parking_start_pose < dist_to_restart) { - return false; - } - - return *dist_to_parking_start_pose > current_to_stop_distance; -} - bool PullOverModule::planWithEfficientPath() { for (const auto & planner : pull_over_planners_) { @@ -427,30 +427,32 @@ BehaviorModuleOutput PullOverModule::plan() // Decelerate before the minimum shift distance from the goal search area. if (status_.is_safe) { auto & first_path = status_.pull_over_path.partial_paths.front(); - const auto arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, -arc_coordinates.distance, 0); + const auto search_start_pose = calcLongitudinalOffsetPose( + first_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + const Pose deceleration_pose = + search_start_pose ? *search_start_pose : first_path.points.front().point.pose; + constexpr double deceleration_buffer = 15.0; first_path = util::setDecelerationVelocity( - first_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + first_path, parameters_.pull_over_velocity, deceleration_pose, -deceleration_buffer, + parameters_.deceleration_interval); } - } - // generate drivable area for each partial path - for (size_t i = status_.current_path_idx; i < status_.pull_over_path.partial_paths.size(); ++i) { - auto & path = status_.pull_over_path.partial_paths.at(i); - const auto p = planner_data_->parameters; - const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); - const auto lane = util::expandLanelets( - shorten_lanes, parameters_.drivable_area_left_bound_offset, - parameters_.drivable_area_right_bound_offset); - util::generateDrivableArea(path, lane, p.vehicle_length, planner_data_); + // generate drivable area for each partial path + for (auto & path : status_.pull_over_path.partial_paths) { + const auto shorten_lanes = util::cutOverlappedLanes(path, status_.lanes); + const auto expanded_lanes = util::expandLanelets( + shorten_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + util::generateDrivableArea( + path, expanded_lanes, planner_data_->parameters.vehicle_length, planner_data_); + } } BehaviorModuleOutput output; // safe: use pull over path if (status_.is_safe) { + status_.stop_pose.reset(); output.path = std::make_shared(getCurrentPath()); path_candidate_ = std::make_shared(getFullPath()); } else { @@ -523,7 +525,7 @@ BehaviorModuleOutput PullOverModule::planWaitingApproval() updateOccupancyGrid(); BehaviorModuleOutput out; plan(); // update status_ - out.path = std::make_shared(getReferencePath()); + out.path = std::make_shared(generateStopPath()); path_candidate_ = status_.is_safe ? std::make_shared(getFullPath()) : out.path; const auto distance_to_path_change = calcDistanceToPathChange(); @@ -566,7 +568,7 @@ void PullOverModule::setParameters(const PullOverParameters & parameters) parameters_ = parameters; } -PathWithLaneId PullOverModule::getReferencePath() const +PathWithLaneId PullOverModule::generateStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; @@ -585,12 +587,13 @@ PathWithLaneId PullOverModule::getReferencePath() const route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); // if not approved, stop parking start position or goal search start position. - const auto refined_goal_arc_coordinates = - lanelet::utils::getArcCoordinates(status_.current_lanes, refined_goal_pose_); - const Pose search_start_pose = calcOffsetPose( - refined_goal_pose_, -parameters_.backward_goal_search_length, - -refined_goal_arc_coordinates.distance, 0); - const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : search_start_pose; + const auto search_start_pose = calcLongitudinalOffsetPose( + reference_path.points, refined_goal_pose_.position, + -parameters_.backward_goal_search_length - planner_data_->parameters.base_link2front); + if (!status_.is_safe && !search_start_pose) { + return generateEmergencyStopPath(); + } + const Pose stop_pose = status_.is_safe ? status_.pull_over_path.start_pose : *search_start_pose; // if stop pose is behind current pose, stop as soon as possible const size_t ego_idx = findEgoIndex(reference_path.points); @@ -600,48 +603,58 @@ PathWithLaneId PullOverModule::getReferencePath() const const double ego_to_stop_distance = calcSignedArcLength( reference_path.points, current_pose.position, ego_idx, stop_pose.position, stop_idx); if (ego_to_stop_distance < 0.0) { - return generateStopPath(); + return generateEmergencyStopPath(); } // slow down for turn signal, insert stop point to stop_pose reference_path = util::setDecelerationVelocityForTurnSignal( reference_path, stop_pose, planner_data_->parameters.turn_signal_search_time); + status_.stop_pose = stop_pose; // slow down before the search area. + constexpr double deceleration_buffer = 15.0; reference_path = util::setDecelerationVelocity( - reference_path, parameters_.pull_over_velocity, search_start_pose, - -calcMinimumShiftPathDistance(), parameters_.deceleration_interval); + reference_path, parameters_.pull_over_velocity, *search_start_pose, -deceleration_buffer, + parameters_.deceleration_interval); + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(reference_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); util::generateDrivableArea( - reference_path, lanes, common_parameters.vehicle_length, planner_data_); + reference_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return reference_path; } -PathWithLaneId PullOverModule::generateStopPath() const +PathWithLaneId PullOverModule::generateEmergencyStopPath() { const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_pose->pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; + // generate stop reference path const auto s_current = lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose).length; const double s_start = std::max(0.0, s_current - common_parameters.backward_path_length); const double s_end = s_current + common_parameters.forward_path_length; auto stop_path = route_handler->getCenterLinePath(status_.current_lanes, s_start, s_end, true); + // calc minimum stop distance under maximum deceleration + const double min_stop_distance = std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; + + // set stop point + const auto stop_idx = + motion_utils::insertStopPoint(current_pose, min_stop_distance, stop_path.points); + if (stop_idx) { + status_.stop_pose = stop_path.points.at(*stop_idx).point.pose; + } + // set deceleration velocity const size_t ego_idx = findEgoIndex(stop_path.points); - const double current_to_stop_distance = - std::pow(current_vel, 2) / parameters_.maximum_deceleration / 2; - motion_utils::insertStopPoint(current_pose, current_to_stop_distance, stop_path.points); - for (auto & point : stop_path.points) { auto & p = point.point; const size_t target_idx = findFirstNearestSegmentIndexWithSoftConstraints( @@ -652,7 +665,7 @@ PathWithLaneId PullOverModule::generateStopPath() const if (0.0 < distance_to_target) { p.longitudinal_velocity_mps = std::clamp( static_cast( - current_vel * (current_to_stop_distance - distance_to_target) / current_to_stop_distance), + current_vel * (min_stop_distance - distance_to_target) / min_stop_distance), 0.0f, p.longitudinal_velocity_mps); } else { p.longitudinal_velocity_mps = @@ -660,13 +673,14 @@ PathWithLaneId PullOverModule::generateStopPath() const } } + // generate drivable area const auto drivable_lanes = util::generateDrivableLanes(status_.current_lanes); const auto shorten_lanes = util::cutOverlappedLanes(stop_path, drivable_lanes); - const auto lanes = util::expandLanelets( + const auto expanded_lanes = util::expandLanelets( shorten_lanes, parameters_.drivable_area_left_bound_offset, parameters_.drivable_area_right_bound_offset); - - util::generateDrivableArea(stop_path, lanes, common_parameters.vehicle_length, planner_data_); + util::generateDrivableArea( + stop_path, expanded_lanes, common_parameters.vehicle_length, planner_data_); return stop_path; } @@ -700,40 +714,6 @@ PathWithLaneId PullOverModule::getFullPath() const return path; } -double PullOverModule::calcMinimumShiftPathDistance() const -{ - const double maximum_jerk = parameters_.maximum_lateral_jerk; - const double pull_over_velocity = parameters_.pull_over_velocity; - const auto current_pose = planner_data_->self_pose->pose; - const double distance_after_pull_over = parameters_.after_pull_over_straight_distance; - const double distance_before_pull_over = parameters_.before_pull_over_straight_distance; - const auto & route_handler = planner_data_->route_handler; - - double distance_to_left_bound = util::getSignedDistanceFromShoulderLeftBoundary( - route_handler->getShoulderLanelets(), current_pose); - double offset_from_center_line = distance_to_left_bound + - planner_data_->parameters.vehicle_width / 2 + - parameters_.margin_from_boundary; - - // calculate minimum pull over distance at pull over velocity, maximum jerk and side offset - const double pull_over_distance_min = PathShifter::calcLongitudinalDistFromJerk( - abs(offset_from_center_line), maximum_jerk, pull_over_velocity); - const double pull_over_total_distance_min = - distance_after_pull_over + pull_over_distance_min + distance_before_pull_over; - - return pull_over_total_distance_min; -} - -bool PullOverModule::isLongEnough( - const lanelet::ConstLanelets & lanelets, const Pose & goal_pose, const double buffer) const -{ - const auto current_pose = planner_data_->self_pose->pose; - const double distance_to_goal = - std::abs(util::getSignedDistance(current_pose, goal_pose, lanelets)); - - return distance_to_goal > calcMinimumShiftPathDistance() + buffer; -} - bool PullOverModule::isStopped() { odometry_buffer_.push_back(planner_data_->self_odometry); @@ -815,6 +795,7 @@ void PullOverModule::setDebugData() using marker_utils::createPathMarkerArray; using marker_utils::createPoseMarkerArray; + using motion_utils::createStopVirtualWallMarker; using tier4_autoware_utils::createMarkerColor; const auto add = [this](const MarkerArray & added) { @@ -823,17 +804,12 @@ void PullOverModule::setDebugData() if (parameters_.enable_goal_research) { // Visualize pull over areas - const Pose start_pose = - calcOffsetPose(refined_goal_pose_, -parameters_.backward_goal_search_length, 0, 0); - const Pose end_pose = - calcOffsetPose(refined_goal_pose_, parameters_.forward_goal_search_length, 0, 0); const auto header = planner_data_->route_handler->getRouteHeader(); const auto color = status_.has_decided_path ? createMarkerColor(1.0, 1.0, 0.0, 0.999) // yellow : createMarkerColor(0.0, 1.0, 0.0, 0.999); // green - const auto p = planner_data_->parameters; - debug_marker_.markers.push_back(pull_over_utils::createPullOverAreaMarker( - start_pose, end_pose, 0, header, p.base_link2front, p.base_link2rear, p.vehicle_width, - color)); + const double z = refined_goal_pose_.position.z; + add(pull_over_utils::createPullOverAreaMarkerArray( + goal_searcher_->getAreaPolygons(), header, color, z)); // Visualize goal candidates add(pull_over_utils::createGoalCandidatesMarkerArray(goal_candidates_, color)); @@ -844,9 +820,23 @@ void PullOverModule::setDebugData() add(createPoseMarkerArray( status_.pull_over_path.start_pose, "pull_over_start_pose", 0, 0.3, 0.3, 0.9)); add(createPoseMarkerArray( - status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.9, 0.9, 0.3)); + status_.pull_over_path.end_pose, "pull_over_end_pose", 0, 0.3, 0.3, 0.9)); add(createPathMarkerArray(getFullPath(), "full_path", 0, 0.0, 0.5, 0.9)); } + + // Visualize debug poses + const auto & debug_poses = status_.pull_over_path.debug_poses; + for (size_t i = 0; i < debug_poses.size(); ++i) { + add(createPoseMarkerArray( + debug_poses.at(i), "debug_pose_" + std::to_string(i), 0, 0.3, 0.3, 0.3)); + } + + // Visualize stop pose + if (status_.stop_pose) { + add(createStopVirtualWallMarker( + *status_.stop_pose, "pull_over", clock_->now(), 0, + planner_data_->parameters.base_link2front)); + } } void PullOverModule::printParkingPositionError() const diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp index 7307ab76278af..69ab6a56b0cf6 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/shift_pull_over.cpp @@ -34,61 +34,25 @@ ShiftPullOver::ShiftPullOver( occupancy_grid_map_{occupancy_grid_map} { } - boost::optional ShiftPullOver::plan(const Pose & goal_pose) { const auto & route_handler = planner_data_->route_handler; - const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; const double min_jerk = parameters_.minimum_lateral_jerk; const double max_jerk = parameters_.maximum_lateral_jerk; const int pull_over_sampling_num = parameters_.pull_over_sampling_num; const double jerk_resolution = std::abs(max_jerk - min_jerk) / pull_over_sampling_num; + // get road and shoulder lanes const auto road_lanes = util::getExtendedCurrentLanes(planner_data_); const auto shoulder_lanes = pull_over_utils::getPullOverLanes(*route_handler); if (road_lanes.empty() || shoulder_lanes.empty()) { return {}; } - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - - // calculate the pose whose longitudinal position is shift end - // and whose lateral position is center line - const auto shift_end_shoulder_center_pose = std::invoke([&]() { - const double s_start = std::max( - goal_shoulder_arc_coords.length - after_pull_over_straight_distance, 0.0); // shift end - const double s_end = s_start + std::numeric_limits::epsilon(); - const auto shoulder_lane_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end, true); - return shoulder_lane_path.points.front().point.pose; - }); - - // calculate shift end pose - const double shoulder_left_bound_to_center_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_end_shoulder_center_pose); - const double shoulder_left_bound_to_goal_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, goal_pose); - const double shoulder_center_to_goal_distance = - -shoulder_left_bound_to_center_distance + shoulder_left_bound_to_goal_distance; - const auto shift_end_pose = tier4_autoware_utils::calcOffsetPose( - shift_end_shoulder_center_pose, 0, shoulder_center_to_goal_distance, 0); - - // calculate lateral distances - lanelet::ConstLanelet goal_closest_road_lane{}; - lanelet::utils::query::getClosestLanelet(road_lanes, goal_pose, &goal_closest_road_lane); - const auto road_center_pose = - lanelet::utils::getClosestCenterPose(goal_closest_road_lane, goal_pose.position); - const double shoulder_left_bound_to_road_center = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, road_center_pose); - const double road_center_to_goal_distance = - -shoulder_left_bound_to_road_center + shoulder_left_bound_to_goal_distance; - + // find safe one from paths with different jerk for (double lateral_jerk = min_jerk; lateral_jerk <= max_jerk; lateral_jerk += jerk_resolution) { - const auto pull_over_path = generatePullOverPath( - road_lanes, shoulder_lanes, shift_end_pose, goal_pose, lateral_jerk, - road_center_to_goal_distance, shoulder_center_to_goal_distance, - shoulder_left_bound_to_goal_distance); + const auto pull_over_path = + generatePullOverPath(road_lanes, shoulder_lanes, goal_pose, lateral_jerk); if (!pull_over_path) continue; return *pull_over_path; } @@ -96,9 +60,8 @@ boost::optional ShiftPullOver::plan(const Pose & goal_pose) return {}; } -PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( - const lanelet::ConstLanelets & road_lanes, const Pose & shift_end_pose, - const double pull_over_distance) const +PathWithLaneId ShiftPullOver::generateReferencePath( + const lanelet::ConstLanelets & road_lanes, const Pose & end_pose) const { const auto & route_handler = planner_data_->route_handler; const Pose & current_pose = planner_data_->self_pose->pose; @@ -108,14 +71,10 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( const auto current_road_arc_coords = lanelet::utils::getArcCoordinates(road_lanes, current_pose); const double s_start = current_road_arc_coords.length - backward_path_length; - const auto shift_end_road_arc_coords = - lanelet::utils::getArcCoordinates(road_lanes, shift_end_pose); - double s_end = shift_end_road_arc_coords.length - pull_over_distance; - s_end = std::max(s_end, s_start + std::numeric_limits::epsilon()); + const double s_end = std::max( + lanelet::utils::getArcCoordinates(road_lanes, end_pose).length, + s_start + std::numeric_limits::epsilon()); auto road_lane_reference_path = route_handler->getCenterLinePath(road_lanes, s_start, s_end); - // resample road straight path and shift source path respectively - road_lane_reference_path = - util::resamplePathWithSpline(road_lane_reference_path, resample_interval_); // decelerate velocity linearly to minimum pull over velocity // (or keep original velocity if it is lower than pull over velocity) @@ -133,115 +92,134 @@ PathWithLaneId ShiftPullOver::generateRoadLaneReferencePath( return road_lane_reference_path; } -PathWithLaneId ShiftPullOver::generateShoulderLaneReferencePath( - const lanelet::ConstLanelets & shoulder_lanes, const Pose & shift_start_pose, - const Pose & goal_pose, const double shoulder_center_to_goal_distance) const -{ - const auto & route_handler = planner_data_->route_handler; - - const auto shift_start_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, shift_start_pose); - const double s_start = shift_start_shoulder_arc_coords.length; - const auto goal_shoulder_arc_coords = - lanelet::utils::getArcCoordinates(shoulder_lanes, goal_pose); - const double s_end = goal_shoulder_arc_coords.length; - auto shoulder_lane_reference_path = - route_handler->getCenterLinePath(shoulder_lanes, s_start, s_end); - // offset to goal line - for (auto & p : shoulder_lane_reference_path.points) { - p.point.pose = - tier4_autoware_utils::calcOffsetPose(p.point.pose, 0, shoulder_center_to_goal_distance, 0); - } - shoulder_lane_reference_path = - util::resamplePathWithSpline(shoulder_lane_reference_path, resample_interval_); - - return shoulder_lane_reference_path; -} - boost::optional ShiftPullOver::generatePullOverPath( const lanelet::ConstLanelets & road_lanes, const lanelet::ConstLanelets & shoulder_lanes, - const Pose & shift_end_pose, const Pose & goal_pose, const double lateral_jerk, - const double road_center_to_goal_distance, const double shoulder_center_to_goal_distance, - const double shoulder_left_bound_to_goal_distance) const + const Pose & goal_pose, const double lateral_jerk) const { const double pull_over_velocity = parameters_.pull_over_velocity; + const double after_pull_over_straight_distance = parameters_.after_pull_over_straight_distance; + + const Pose shift_end_pose = + tier4_autoware_utils::calcOffsetPose(goal_pose, -after_pull_over_straight_distance, 0, 0); + + // generate road lane reference path to shift end + const auto road_lane_reference_path_to_shift_end = util::resamplePathWithSpline( + generateReferencePath(road_lanes, shift_end_pose), resample_interval_); + + // calculate shift length + const Pose & shift_end_pose_road_lane = + road_lane_reference_path_to_shift_end.points.back().point.pose; + const double shift_end_road_to_target_distance = + tier4_autoware_utils::inverseTransformPoint(shift_end_pose.position, shift_end_pose_road_lane) + .y; - // generate road lane reference path and get shift start pose + // calculate shift start pose on road lane const double pull_over_distance = PathShifter::calcLongitudinalDistFromJerk( - road_center_to_goal_distance, lateral_jerk, pull_over_velocity); - const auto road_lane_reference_path = - generateRoadLaneReferencePath(road_lanes, shift_end_pose, pull_over_distance); - if (road_lane_reference_path.points.empty()) return {}; - const auto shift_start_pose = road_lane_reference_path.points.back().point.pose; - - // generate Shoulder lane reference path and set it path_shifter - const auto shoulder_lane_reference_path = generateShoulderLaneReferencePath( - shoulder_lanes, shift_start_pose, goal_pose, shoulder_center_to_goal_distance); - if (shoulder_lane_reference_path.points.empty()) return {}; + shift_end_road_to_target_distance, lateral_jerk, pull_over_velocity); + const double before_shifted_pull_over_distance = calcBeforeShiftedArcLength( + road_lane_reference_path_to_shift_end, pull_over_distance, shift_end_road_to_target_distance); + const auto shift_start_pose = motion_utils::calcLongitudinalOffsetPose( + road_lane_reference_path_to_shift_end.points, shift_end_pose_road_lane.position, + -before_shifted_pull_over_distance); + if (!shift_start_pose) return {}; + + // set path shifter and generate shifted path PathShifter path_shifter{}; - path_shifter.setPath(shoulder_lane_reference_path); - - // generate shift_line and set it to path_sifter + path_shifter.setPath(road_lane_reference_path_to_shift_end); ShiftLine shift_line{}; + shift_line.start = *shift_start_pose; + shift_line.end = shift_end_pose; + shift_line.end_shift_length = shift_end_road_to_target_distance; + path_shifter.addShiftLine(shift_line); + ShiftedPath shifted_path{}; + const bool offset_back = true; // offset front side from reference path + if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + + // set same orientation, because the reference center line orientation is not same to the + shifted_path.path.points.back().point.pose.orientation = shift_end_pose.orientation; + + // for debug. result of shift is not equal to the target + const Pose actual_shift_end_pose = shifted_path.path.points.back().point.pose; + + // interpolate between shift end pose to goal pose + std::vector interpolated_poses = + interpolatePose(shifted_path.path.points.back().point.pose, goal_pose, 0.5); + for (const auto & pose : interpolated_poses) { + PathPointWithLaneId p = shifted_path.path.points.back(); + p.point.pose = pose; + shifted_path.path.points.push_back(p); + } + + // set goal pose with velocity 0 { - shift_line.start = shift_start_pose; - shift_line.end = shift_end_pose; - const double shoulder_left_bound_to_shift_start_distance = - util::getSignedDistanceFromShoulderLeftBoundary(shoulder_lanes, shift_start_pose); - const double goal_to_shift_start_distance = - shoulder_left_bound_to_shift_start_distance - shoulder_left_bound_to_goal_distance; - shift_line.end_shift_length = goal_to_shift_start_distance; - path_shifter.addShiftLine(shift_line); + PathPointWithLaneId p{}; + p.point.longitudinal_velocity_mps = 0.0; + p.point.pose = goal_pose; + p.lane_ids = shifted_path.path.points.back().lane_ids; + for (const auto & lane : shoulder_lanes) { + p.lane_ids.push_back(lane.id()); + } + shifted_path.path.points.push_back(p); } - // offset front side from reference path - ShiftedPath shifted_path{}; - const bool offset_back = false; - if (!path_shifter.generate(&shifted_path, offset_back)) return {}; + // set the same z as the goal + for (auto & p : shifted_path.path.points) { + p.point.pose.position.z = goal_pose.position.z; + } // check lane departure with road and shoulder lanes - lanelet::ConstLanelets lanes = road_lanes; - lanes.insert(lanes.end(), shoulder_lanes.begin(), shoulder_lanes.end()); - if (lane_departure_checker_.checkPathWillLeaveLane(lanes, shifted_path.path)) return {}; + const auto drivable_lanes = + util::generateDrivableLanesWithShoulderLanes(road_lanes, shoulder_lanes); + const auto expanded_lanes = util::expandLanelets( + drivable_lanes, parameters_.drivable_area_left_bound_offset, + parameters_.drivable_area_right_bound_offset); + if (lane_departure_checker_.checkPathWillLeaveLane( + util::transformToLanelets(expanded_lanes), shifted_path.path)) { + return {}; + } // check collision - if (!isSafePath(shifted_path.path)) return {}; + PathWithLaneId collision_check_path = shifted_path.path; + collision_check_path.points.clear(); + std::copy( + shifted_path.path.points.begin() + path_shifter.getShiftLines().front().start_idx, + shifted_path.path.points.end(), std::back_inserter(collision_check_path.points)); + + if (!isSafePath(collision_check_path)) return {}; // set lane_id and velocity to shifted_path - const auto goal_idx = motion_utils::findNearestIndex(shifted_path.path.points, goal_pose); - lanelet::ConstLanelet target_shoulder_lanelet{}; - lanelet::utils::query::getClosestLanelet( - shoulder_lanes, shifted_path.path.points.back().point.pose, &target_shoulder_lanelet); - for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { + for (size_t i = path_shifter.getShiftLines().front().start_idx; + i < shifted_path.path.points.size() - 1; ++i) { auto & point = shifted_path.path.points.at(i); + // set velocity + point.point.longitudinal_velocity_mps = + std::min(point.point.longitudinal_velocity_mps, static_cast(pull_over_velocity)); + + // add target lanes to points after shift start // add road lane_ids if not found - for (const auto id : road_lane_reference_path.points.back().lane_ids) { + for (const auto id : shifted_path.path.points.back().lane_ids) { if (std::find(point.lane_ids.begin(), point.lane_ids.end(), id) == point.lane_ids.end()) { point.lane_ids.push_back(id); } } // add shoulder lane_id if not found - if ( - std::find(point.lane_ids.begin(), point.lane_ids.end(), target_shoulder_lanelet.id()) == - point.lane_ids.end()) { - point.lane_ids.push_back(target_shoulder_lanelet.id()); - } - // set velocity - if (i >= *goal_idx) { - // set velocity after goal - point.point.longitudinal_velocity_mps = 0.0; - } else { - point.point.longitudinal_velocity_mps = pull_over_velocity; + for (const auto & lane : shoulder_lanes) { + if ( + std::find(point.lane_ids.begin(), point.lane_ids.end(), lane.id()) == + point.lane_ids.end()) { + point.lane_ids.push_back(lane.id()); + } } } + // set pull over path PullOverPath pull_over_path{}; - pull_over_path.path = - pull_over_utils::combineReferencePath(road_lane_reference_path, shifted_path.path); - // shift path is connected to one, so partial_paths have only one + pull_over_path.path = shifted_path.path; pull_over_path.partial_paths.push_back(pull_over_path.path); pull_over_path.start_pose = path_shifter.getShiftLines().front().start; pull_over_path.end_pose = path_shifter.getShiftLines().front().end; + pull_over_path.debug_poses.push_back(shift_end_pose_road_lane); + pull_over_path.debug_poses.push_back(actual_shift_end_pose); // check enough distance if (!hasEnoughDistance( @@ -321,4 +299,86 @@ bool ShiftPullOver::isSafePath(const PathWithLaneId & path) const return true; } +double ShiftPullOver::calcBeforeShiftedArcLength( + const PathWithLaneId & path, const double target_after_arc_length, const double dr) +{ + // reverse path for checking from the end point + // note that the sign of curvature is also reversed + PathWithLaneId reversed_path{}; + std::reverse_copy( + path.points.begin(), path.points.end(), std::back_inserter(reversed_path.points)); + + double before_arc_length{0.0}; + double after_arc_length{0.0}; + for (const auto & [k, segment_length] : + motion_utils::calcCurvatureAndArcLength(reversed_path.points)) { + // after shifted segment length + const double after_segment_length = + k > 0 ? segment_length * (1 + k * dr) : segment_length / (1 - k * dr); + if (after_arc_length + after_segment_length > target_after_arc_length) { + const double offset = target_after_arc_length - after_arc_length; + before_arc_length += k > 0 ? offset / (1 + k * dr) : offset * (1 - k * dr); + break; + } + before_arc_length += segment_length; + after_arc_length += after_segment_length; + } + + return before_arc_length; +} + +// only two points is supported +std::vector ShiftPullOver::splineTwoPoints( + std::vector base_s, std::vector base_x, const double begin_diff, + const double end_diff, std::vector new_s) +{ + const double h = base_s.at(1) - base_s.at(0); + + const double c = begin_diff; + const double d = base_x.at(0); + const double a = (end_diff * h - 2 * base_x.at(1) + c * h + 2 * d) / std::pow(h, 3); + const double b = (3 * base_x.at(1) - end_diff * h - 2 * c * h - 3 * d) / std::pow(h, 2); + + std::vector res; + for (const auto & s : new_s) { + const double ds = s - base_s.at(0); + res.push_back(d + (c + (b + a * ds) * ds) * ds); + } + + return res; +} + +std::vector ShiftPullOver::interpolatePose( + const Pose & start_pose, const Pose & end_pose, const double resample_interval) +{ + std::vector interpolated_poses{}; // output + + const std::vector base_s{ + 0, tier4_autoware_utils::calcDistance2d(start_pose.position, end_pose.position)}; + const std::vector base_x{start_pose.position.x, end_pose.position.x}; + const std::vector base_y{start_pose.position.y, end_pose.position.y}; + std::vector new_s; + + constexpr double eps = 0.3; // prevent overlapping + for (double s = eps; s < base_s.back() - eps; s += resample_interval) { + new_s.push_back(s); + } + + const std::vector interpolated_x = splineTwoPoints( + base_s, base_x, std::cos(tf2::getYaw(start_pose.orientation)), + std::cos(tf2::getYaw(end_pose.orientation)), new_s); + const std::vector interpolated_y = splineTwoPoints( + base_s, base_y, std::sin(tf2::getYaw(start_pose.orientation)), + std::sin(tf2::getYaw(end_pose.orientation)), new_s); + for (size_t i = 0; i < interpolated_x.size(); ++i) { + Pose pose{}; + pose = util::lerpByPose(end_pose, start_pose, (base_s.back() - new_s.at(i)) / base_s.back()); + pose.position.x = interpolated_x.at(i); + pose.position.y = interpolated_y.at(i); + pose.position.z = end_pose.position.z; + interpolated_poses.push_back(pose); + } + + return interpolated_poses; +} } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp index addc437dfba7a..b2a7af91ef3ad 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_over/util.cpp @@ -86,30 +86,24 @@ PredictedObjects filterObjectsByLateralDistance( return filtered_objects; } -Marker createPullOverAreaMarker( - const Pose & start_pose, const Pose & end_pose, const int32_t id, - const std_msgs::msg::Header & header, const double base_link2front, const double base_link2rear, - const double vehicle_width, const std_msgs::msg::ColorRGBA & color) +MarkerArray createPullOverAreaMarkerArray( + const tier4_autoware_utils::MultiPolygon2d area_polygons, const std_msgs::msg::Header & header, + const std_msgs::msg::ColorRGBA & color, const double z) { - Marker marker = createDefaultMarker( - header.frame_id, header.stamp, "pull_over_area", id, - visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); - - auto p_left_front = calcOffsetPose(end_pose, base_link2front, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); - - auto p_right_front = calcOffsetPose(end_pose, base_link2front, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_front.x, p_right_front.y, p_right_front.z)); - - auto p_right_back = calcOffsetPose(start_pose, -base_link2rear, -vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_right_back.x, p_right_back.y, p_right_back.z)); - - auto p_left_back = calcOffsetPose(start_pose, -base_link2rear, vehicle_width / 2, 0).position; - marker.points.push_back(createPoint(p_left_back.x, p_left_back.y, p_left_back.z)); + MarkerArray marker_array{}; + for (size_t i = 0; i < area_polygons.size(); ++i) { + Marker marker = createDefaultMarker( + header.frame_id, header.stamp, "pull_over_area_" + std::to_string(i), i, + visualization_msgs::msg::Marker::LINE_STRIP, createMarkerScale(0.1, 0.0, 0.0), color); + const auto & poly = area_polygons.at(i); + for (const auto & p : poly.outer()) { + marker.points.push_back(createPoint(p.x(), p.y(), z)); + } - marker.points.push_back(createPoint(p_left_front.x, p_left_front.y, p_left_front.z)); + marker_array.markers.push_back(marker); + } - return marker; + return marker_array; } MarkerArray createPosesMarkerArray( @@ -149,7 +143,7 @@ MarkerArray createTextsMarkerArray( } MarkerArray createGoalCandidatesMarkerArray( - std::vector & goal_candidates, const std_msgs::msg::ColorRGBA & color) + GoalCandidates & goal_candidates, const std_msgs::msg::ColorRGBA & color) { // convert to pose vector std::vector pose_vector{}; diff --git a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp index 4ce7c253c51be..9b4ba77a3c7f6 100644 --- a/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp +++ b/planning/behavior_path_planner/src/scene_module/utils/geometric_parallel_parking.cpp @@ -163,6 +163,16 @@ std::vector GeometricParallelParking::generatePullOverPaths( // straight path from current to parking start const auto straight_path = generateStraightPath(start_pose); + // check the continuity of straight path and arc path + const Pose & road_path_last_pose = straight_path.points.back().point.pose; + const Pose & arc_path_first_pose = arc_paths.front().points.front().point.pose; + const double yaw_diff = tier4_autoware_utils::normalizeRadian( + tf2::getYaw(road_path_last_pose.orientation), tf2::getYaw(arc_path_first_pose.orientation)); + const double distance = calcDistance2d(road_path_last_pose, arc_path_first_pose); + if (yaw_diff > tier4_autoware_utils::deg2rad(5.0) || distance > 0.1) { + return std::vector{}; + } + // combine straight_path -> arc_path*2 auto paths = arc_paths; paths.insert(paths.begin(), straight_path); diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index c38b54086b3fd..b899439215a02 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1526,6 +1526,61 @@ double getSignedDistanceFromShoulderLeftBoundary( return arc_coordinates.distance; } +std::optional getSignedDistanceFromShoulderLeftBoundary( + const lanelet::ConstLanelets & shoulder_lanelets, const LinearRing2d & footprint, + const Pose & vehicle_pose) +{ + double min_distance = std::numeric_limits::max(); + const auto transformed_footprint = + transformVector(footprint, tier4_autoware_utils::pose2transform(vehicle_pose)); + bool found_neighbor_shoulder_bound = false; + for (const auto & vehicle_corner_point : transformed_footprint) { + // convert point of footprint to pose + Pose vehicle_corner_pose{}; + vehicle_corner_pose.position = tier4_autoware_utils::toMsg(vehicle_corner_point.to_3d()); + vehicle_corner_pose.orientation = vehicle_pose.orientation; + + // calculate distance to the shoulder bound directly next to footprint points + lanelet::ConstLanelet closest_shoulder_lanelet{}; + if (lanelet::utils::query::getClosestLanelet( + shoulder_lanelets, vehicle_corner_pose, &closest_shoulder_lanelet)) { + const auto & left_line_2d = lanelet::utils::to2D(closest_shoulder_lanelet.leftBound3d()); + + for (size_t i = 1; i < left_line_2d.size(); ++i) { + const Point p_front = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i - 1]); + const Point p_back = lanelet::utils::conversion::toGeomMsgPt(left_line_2d[i]); + + const Point inverse_p_front = + tier4_autoware_utils::inverseTransformPoint(p_front, vehicle_corner_pose); + const Point inverse_p_back = + tier4_autoware_utils::inverseTransformPoint(p_back, vehicle_corner_pose); + + const double dy_front = inverse_p_front.y; + const double dy_back = inverse_p_back.y; + const double dx_front = inverse_p_front.x; + const double dx_back = inverse_p_back.x; + // is in segment + if (dx_front < 0 && dx_back > 0) { + const double lateral_distance_from_pose_to_segment = + (dy_front * dx_back + dy_back * -dx_front) / (dx_back - dx_front); + min_distance = std::min(lateral_distance_from_pose_to_segment, min_distance); + found_neighbor_shoulder_bound = true; + break; + } + } + } + } + + if (!found_neighbor_shoulder_bound) { + RCLCPP_ERROR_STREAM( + rclcpp::get_logger("behavior_path_planner").get_child("utilities"), + "neighbor shoulder bound to footprint is not found."); + return {}; + } + + return -min_distance; +} + double getSignedDistanceFromRightBoundary( const lanelet::ConstLanelets & lanelets, const Pose & pose) { diff --git a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp index e3e4c6d75d6ed..a70ab56d98a94 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/trajectory_analyzer.hpp @@ -87,7 +87,7 @@ class TrajectoryAnalyzer TrajectoryDebugInfo data; data.stamp = node_->now(); data.size = points.size(); - data.curvature = calcCurvature(points); + data.curvature = motion_utils::calcCurvature(points); const auto arclength_offset = motion_utils::calcSignedArcLength(points, 0, ego_p); data.arclength = calcPathArcLengthArray(points, -arclength_offset); data.velocity = getVelocityArray(points); diff --git a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp index e3b4ab7639cb6..96488fbde4b96 100644 --- a/planning/planning_debug_tools/include/planning_debug_tools/util.hpp +++ b/planning/planning_debug_tools/include/planning_debug_tools/util.hpp @@ -114,21 +114,6 @@ std::vector calcPathArcLengthArray(const T & points, const double offset return out; } -template -inline std::vector calcCurvature(const T & points) -{ - std::vector curvature_arr; - curvature_arr.push_back(0.0); - for (size_t i = 1; i < points.size() - 1; ++i) { - const auto p1 = getPoint(points.at(i - 1)); - const auto p2 = getPoint(points.at(i)); - const auto p3 = getPoint(points.at(i + 1)); - curvature_arr.push_back(tier4_autoware_utils::calcCurvature(p1, p2, p3)); - } - curvature_arr.push_back(0.0); - return curvature_arr; -} - } // namespace planning_debug_tools #endif // PLANNING_DEBUG_TOOLS__UTIL_HPP_