update

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Dec 19, 2022 · fe260b2 · fe260b2
1 parent 480edb7
commit fe260b2
Show file tree

Hide file tree

Showing 12 changed files with 153 additions and 207 deletions.
diff --git a/planning/obstacle_cruise_planner/README.md b/planning/obstacle_cruise_planner/README.md
@@ -4,30 +4,29 @@
 
 The `obstacle_cruise_planner` package has following modules.
 
-- obstacle stop planning
-  - inserting a stop point in the trajectory when there is a static obstacle on the trajectory.
-- adaptive cruise planning
-  - sending an external velocity limit to `motion_velocity_smoother` when there is a dynamic obstacle to cruise on the trajectory
+- Stop planning
+  - stop when there is a static obstacle near the trajectory.
+- Cruise planning
+  - slow down when there is a dynamic obstacle to cruise near the trajectory
 
 ## Interfaces
 
 ### Input topics
 
-| Name                          | Type                                            | Description                       |
-| ----------------------------- | ----------------------------------------------- | --------------------------------- |
-| `~/input/trajectory`          | autoware_auto_planning_msgs::Trajectory         | input trajectory                  |
-| `~/input/smoothed_trajectory` | autoware_auto_planning_msgs::Trajectory         | trajectory with smoothed velocity |
-| `~/input/objects`             | autoware_auto_perception_msgs::PredictedObjects | dynamic objects                   |
-| `~/input/odometry`            | nav_msgs::msg::Odometry                         | ego odometry                      |
+| Name                 | Type                                            | Description      |
+| -------------------- | ----------------------------------------------- | ---------------- |
+| `~/input/trajectory` | autoware_auto_planning_msgs::Trajectory         | input trajectory |
+| `~/input/objects`    | autoware_auto_perception_msgs::PredictedObjects | dynamic objects  |
+| `~/input/odometry`   | nav_msgs::msg::Odometry                         | ego odometry     |
 
 ### Output topics
 
-| Name                           | Type                                           | Description                           |
-| ------------------------------ | ---------------------------------------------- | ------------------------------------- |
-| `~output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
-| `~output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
-| `~output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
-| `~output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
+| Name                            | Type                                           | Description                           |
+| ------------------------------- | ---------------------------------------------- | ------------------------------------- |
+| `~/output/trajectory`           | autoware_auto_planning_msgs::Trajectory        | output trajectory                     |
+| `~/output/velocity_limit`       | tier4_planning_msgs::VelocityLimit             | velocity limit for cruising           |
+| `~/output/clear_velocity_limit` | tier4_planning_msgs::VelocityLimitClearCommand | clear command for velocity limit      |
+| `~/output/stop_reasons`         | tier4_planning_msgs::StopReasonArray           | reasons that make the vehicle to stop |
 
 ## Design
 

diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -12,8 +12,6 @@
       safe_distance_margin : 6.0 # This is also used as a stop margin [m]
       terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
 
-      nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index
-      nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index
       min_behavior_stop_margin: 3.0 # [m]
       obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]
       obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s]

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/common_structs.hpp
@@ -86,6 +86,50 @@ struct ObstacleCruisePlannerData
 
 struct LongitudinalInfo
 {
+  explicit LongitudinalInfo(rclcpp::Node & node)
+  {
+    max_accel = node.declare_parameter<double>("normal.max_acc");
+    min_accel = node.declare_parameter<double>("normal.min_acc");
+    max_jerk = node.declare_parameter<double>("normal.max_jerk");
+    min_jerk = node.declare_parameter<double>("normal.min_jerk");
+    limit_max_accel = node.declare_parameter<double>("limit.max_acc");
+    limit_min_accel = node.declare_parameter<double>("limit.min_acc");
+    limit_max_jerk = node.declare_parameter<double>("limit.max_jerk");
+    limit_min_jerk = node.declare_parameter<double>("limit.min_jerk");
+
+    idling_time = node.declare_parameter<double>("common.idling_time");
+    min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
+    min_object_accel_for_rss = node.declare_parameter<double>("common.min_object_accel_for_rss");
+
+    safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
+    terminal_safe_distance_margin =
+      node.declare_parameter<double>("common.terminal_safe_distance_margin");
+  }
+
+  void onParam(const std::vector<rclcpp::Parameter> & parameters)
+  {
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.max_accel", max_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.min_accel", min_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.max_jerk", max_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "normal.min_jerk", min_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", limit_max_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", limit_min_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", limit_max_jerk);
+    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", limit_min_jerk);
+
+    tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", idling_time);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss);
+
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.safe_distance_margin", safe_distance_margin);
+    tier4_autoware_utils::updateParam<double>(
+      parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
+  }
+
+  // common parameter
   double max_accel;
   double min_accel;
   double max_jerk;
@@ -94,9 +138,13 @@ struct LongitudinalInfo
   double limit_min_accel;
   double limit_max_jerk;
   double limit_min_jerk;
+
+  // rss parameter
   double idling_time;
   double min_ego_accel_for_rss;
   double min_object_accel_for_rss;
+
+  // distance margin
   double safe_distance_margin;
   double terminal_safe_distance_margin;
 };
@@ -114,10 +162,13 @@ struct DebugData
 
 struct EgoNearestParam
 {
-  EgoNearestParam(const double arg_dist_threshold, const double arg_yaw_threshold)
-  : dist_threshold(arg_dist_threshold), yaw_threshold(arg_yaw_threshold)
+  EgoNearestParam() = default;
+  explicit EgoNearestParam(rclcpp::Node & node)
   {
+    dist_threshold = node.declare_parameter<double>("ego_nearest_dist_threshold");
+    yaw_threshold = node.declare_parameter<double>("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
@@ -80,8 +80,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
 
   bool is_showing_debug_info_;
   double min_behavior_stop_margin_;
-  double nearest_dist_deviation_threshold_;
-  double nearest_yaw_deviation_threshold_;
   double obstacle_velocity_threshold_from_cruise_to_stop_;
   double obstacle_velocity_threshold_from_stop_to_cruise_;
 
@@ -103,15 +101,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_pub_;
 
   // subscriber
-  rclcpp::Subscription<Trajectory>::SharedPtr trajectory_sub_;
-  rclcpp::Subscription<Trajectory>::SharedPtr smoothed_trajectory_sub_;
+  rclcpp::Subscription<Trajectory>::SharedPtr traj_sub_;
   rclcpp::Subscription<PredictedObjects>::SharedPtr objects_sub_;
   rclcpp::Subscription<Odometry>::SharedPtr odom_sub_;
   rclcpp::Subscription<AccelWithCovarianceStamped>::SharedPtr acc_sub_;
 
-  // self pose listener
-  tier4_autoware_utils::SelfPoseListener self_pose_listener_;
-
   // data for callback functions
   PredictedObjects::ConstSharedPtr in_objects_ptr_{nullptr};
   Odometry::ConstSharedPtr current_odom_ptr_{nullptr};
@@ -164,13 +158,12 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
   ObstacleFilteringParam obstacle_filtering_param_;
 
   bool need_to_clear_vel_limit_{false};
+  EgoNearestParam ego_nearest_param_;
 
   bool is_driving_forward_{true};
   bool disable_stop_planning_{false};
 
   std::vector<TargetObstacle> prev_target_obstacles_;
-
-  std::shared_ptr<LowpassFilter1d> lpf_acc_ptr_;
 };
 }  // namespace motion_planning
 

diff --git a/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
@@ -94,6 +94,11 @@ class OptimizationBasedPlanner : public PlannerInterface
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_st_graph_pub_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_wall_marker_pub_;
 
+  // Subscriber
+  rclcpp::Subscription<Trajectory>::SharedPtr smoothed_traj_sub_;
+
+  Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr};
+
   // Resampling Parameter
   double dense_resampling_time_interval_;
   double sparse_resampling_time_interval_;

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp
@@ -46,14 +46,10 @@ class PlannerInterface
 
   PlannerInterface() = default;
 
-  void setParams(
-    const bool is_showing_debug_info, const double min_behavior_stop_margin,
-    const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold)
+  void setParam(const bool is_showing_debug_info, const double min_behavior_stop_margin)
   {
     is_showing_debug_info_ = is_showing_debug_info;
     min_behavior_stop_margin_ = min_behavior_stop_margin;
-    nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold;
-    nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold;
   }
 
   Trajectory generateStopTrajectory(
@@ -63,31 +59,10 @@ class PlannerInterface
     const ObstacleCruisePlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit,
     DebugData & debug_data) = 0;
 
-  void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
+  void onParam(const std::vector<rclcpp::Parameter> & parameters)
   {
-    auto & i = longitudinal_info_;
-
-    tier4_autoware_utils::updateParam<double>(parameters, "common.max_accel", i.max_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.min_accel", i.min_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.max_jerk", i.max_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.min_jerk", i.min_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_accel", i.limit_max_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_accel", i.limit_min_accel);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.max_jerk", i.limit_max_jerk);
-    tier4_autoware_utils::updateParam<double>(parameters, "limit.min_jerk", i.limit_min_jerk);
-    tier4_autoware_utils::updateParam<double>(
-      parameters, "common.min_ego_accel_for_rss", i.min_ego_accel_for_rss);
-    tier4_autoware_utils::updateParam<double>(
-      parameters, "common.min_object_accel_for_rss", i.min_object_accel_for_rss);
-    tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", i.idling_time);
-  }
-
-  virtual void updateParam([[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) {}
-
-  // TODO(shimizu) remove this function
-  void setSmoothedTrajectory(const Trajectory::ConstSharedPtr traj)
-  {
-    smoothed_trajectory_ptr_ = traj;
+    updateCommonParam(parameters);
+    updateParam(parameters);
   }
 
   Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const
@@ -105,8 +80,6 @@ class PlannerInterface
   bool is_showing_debug_info_{false};
   LongitudinalInfo longitudinal_info_;
   double min_behavior_stop_margin_;
-  double nearest_dist_deviation_threshold_;
-  double nearest_yaw_deviation_threshold_;
 
   // Publishers
   rclcpp::Publisher<StopReasonArray>::SharedPtr stop_reasons_pub_;
@@ -121,9 +94,6 @@ class PlannerInterface
   // debug info
   StopPlanningDebugInfo stop_planning_debug_info_;
 
-  // TODO(shimizu) remove these parameters
-  Trajectory::ConstSharedPtr smoothed_trajectory_ptr_;
-
   double calcDistanceToCollisionPoint(
     const ObstacleCruisePlannerData & planner_data,
     const geometry_msgs::msg::Point & collision_point);
@@ -138,6 +108,13 @@ class PlannerInterface
     return rss_dist_with_margin;
   }
 
+  void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
+  {
+    longitudinal_info_.onParam(parameters);
+  }
+
+  virtual void updateParam([[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) {}
+
   size_t findEgoIndex(const Trajectory & traj, const geometry_msgs::msg::Pose & ego_pose) const
   {
     const auto traj_points = motion_utils::convertToTrajectoryPointArray(traj);

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp
@@ -30,8 +30,6 @@
 
 namespace obstacle_cruise_utils
 {
-bool isVehicle(const uint8_t label);
-
 Marker getObjectMarker(
   const geometry_msgs::msg::Pose & obstacle_pose, size_t idx, const std::string & ns,
   const double r, const double g, const double b);