tmp

planning/obstacle_velocity_planner/config/obstacle_velocity_planner.param.yaml

@@ -1,6 +1,9 @@
 /**:
   ros__parameters:
     common:
+      # planning_method: "rule_base"
+      planning_method: "optimization_base"
+
       max_accel:  1.0
       min_accel: -1.0
       max_jerk:  1.0
@@ -25,7 +28,7 @@
 
       max_obj_velocity_for_stop : 1.0
       safe_distance_margin : 5.0
-      strong_min_accel : -3.0
+      min_obstacle_stop_accel : -3.0
 
     optimization_based_planner:
       resampling_s_interval: 2.0

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/common_structs.hpp

@@ -79,9 +79,9 @@ struct ObstacleVelocityPlannerData
   std::vector<TargetObstacle> target_obstacles;
 };
 
-struct RSSLongitudinalInfo
+struct LongitudinalInfo
 {
-  RSSLongitudinalInfo(
+  LongitudinalInfo(
     const double arg_max_accel, const double arg_min_accel, const double arg_max_jerk,
     const double arg_min_jerk, const double arg_min_object_accel, const double arg_idling_time)
   : max_accel(arg_max_accel),

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/node.hpp

@@ -18,10 +18,10 @@
 #include "obstacle_velocity_planner/common_structs.hpp"
 #include "obstacle_velocity_planner/optimization_based_planner/optimization_based_planner.hpp"
 #include "obstacle_velocity_planner/rule_based_planner/rule_based_planner.hpp"
+#include "signal_processing/lowpass_filter_1d.hpp"
 
 #include <rclcpp/rclcpp.hpp>
 #include <tier4_autoware_utils/ros/self_pose_listener.hpp>
-#include "signal_processing/lowpass_filter_1d.hpp"
 
 #include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
 #include <autoware_auto_perception_msgs/msg/predicted_object.hpp>
@@ -43,6 +43,7 @@
 
 #include <memory>
 #include <mutex>
+#include <string>
 #include <vector>
 
 using autoware_auto_mapping_msgs::msg::HADMapBin;
@@ -53,9 +54,9 @@ using autoware_auto_perception_msgs::msg::PredictedPath;
 using autoware_auto_planning_msgs::msg::Trajectory;
 using autoware_auto_planning_msgs::msg::TrajectoryPoint;
 using nav_msgs::msg::Odometry;
+using tier4_planning_msgs::msg::StopReasonArray;
 using tier4_planning_msgs::msg::VelocityLimit;
 using tier4_planning_msgs::msg::VelocityLimitClearCommand;
-using tier4_planning_msgs::msg::StopReasonArray;
 using vehicle_info_util::VehicleInfo;
 
 namespace motion_planning
@@ -64,9 +65,10 @@ class ObstacleVelocityPlannerNode : public rclcpp::Node
 {
 public:
   explicit ObstacleVelocityPlannerNode(const rclcpp::NodeOptions & node_options);
+  enum class PlanningMethod { OPTIMIZATION_BASE, RULE_BASE, INVALID };
 
 private:
-  enum class PlanningMethod { OPTIMIZATION_BASE, RULE_BASE };
+  PlanningMethod getPlanningMethodType(const std::string & param) const;
 
   // Callback Functions
   rcl_interfaces::msg::SetParametersResult paramCallback(

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/optimization_based_planner/optimization_based_planner.hpp

@@ -36,21 +36,19 @@
 #include <tuple>
 #include <vector>
 
+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;
-using autoware_auto_perception_msgs::msg::PredictedPath;
-using autoware_auto_perception_msgs::msg::ObjectClassification;
 
 class OptimizationBasedPlanner : public PlannerInterface
 {
 public:
   OptimizationBasedPlanner(
-    rclcpp::Node & node, const double max_accel, const double min_accel, const double max_jerk,
-    const double min_jerk, const double min_object_accel, const double idling_time,
+    rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
-  Trajectory generateTrajectory(
-    const ObstacleVelocityPlannerData & planner_data) override;
+  Trajectory generateTrajectory(const ObstacleVelocityPlannerData & planner_data) override;
 
 private:
   struct TrajectoryData
@@ -77,28 +75,22 @@ class OptimizationBasedPlanner : public PlannerInterface
     const std::vector<double> & resolutions);
 
   std::tuple<double, double> calcInitialMotion(
-    const double current_vel, const Trajectory & input_traj,
-    const size_t input_closest, const Trajectory & prev_traj,
-    const double closest_stop_dist);
+    const double current_vel, const Trajectory & input_traj, const size_t input_closest,
+    const Trajectory & prev_traj, const double closest_stop_dist);
 
   TrajectoryPoint calcInterpolatedTrajectoryPoint(
-    const Trajectory & trajectory,
-    const geometry_msgs::msg::Pose & target_pose);
+    const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose);
 
-  bool checkHasReachedGoal(
-    const Trajectory & traj, const size_t closest_idx,
-    const double v0);
+  bool checkHasReachedGoal(const Trajectory & traj, const size_t closest_idx, const double v0);
 
-  TrajectoryData getTrajectoryData(
-    const Trajectory & traj, const size_t closest_idx);
+  TrajectoryData getTrajectoryData(const Trajectory & traj, const size_t closest_idx);
 
   TrajectoryData resampleTrajectoryData(
     const TrajectoryData & base_traj_data, const double resampling_s_interval,
     const double max_traj_length, const double stop_dist);
 
   Trajectory resampleTrajectory(
-    const std::vector<double> & base_index,
-    const Trajectory & base_trajectory,
+    const std::vector<double> & base_index, const Trajectory & base_trajectory,
     const std::vector<double> & query_index, const bool use_spline_for_pose = false);
 
   boost::optional<SBoundaries> getSBoundaries(
@@ -130,17 +122,15 @@ class OptimizationBasedPlanner : public PlannerInterface
     const lanelet::routing::LaneletPaths & candidate_paths,
     lanelet::ConstLanelets & valid_lanelets);
 
-  bool checkIsFrontObject(
-    const TargetObstacle & object, const Trajectory & traj);
+  bool checkIsFrontObject(const TargetObstacle & object, const Trajectory & traj);
 
   boost::optional<PredictedPath> resampledPredictedPath(
     const TargetObstacle & object, const rclcpp::Time & obj_base_time,
     const rclcpp::Time & current_time, const std::vector<double> & resolutions,
     const double horizon);
 
   boost::optional<geometry_msgs::msg::Pose> calcForwardPose(
-    const Trajectory & traj,
-    const geometry_msgs::msg::Point & point, const double target_length);
+    const Trajectory & traj, const geometry_msgs::msg::Point & point, const double target_length);
 
   boost::optional<geometry_msgs::msg::Pose> calcForwardPose(
     const TrajectoryData & ego_traj_data, const geometry_msgs::msg::Point & point,
@@ -164,9 +154,9 @@ class OptimizationBasedPlanner : public PlannerInterface
     const double v0, const VelocityOptimizer::OptimizationResult & opt_result);
 
   void publishDebugTrajectory(
-    const rclcpp::Time & current_time, const Trajectory & traj,
-    const size_t closest_idx, const std::vector<double> & time_vec,
-    const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result);
+    const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx,
+    const std::vector<double> & time_vec, const SBoundaries & s_boundaries,
+    const VelocityOptimizer::OptimizationResult & opt_result);
 
   // Calculation time watcher
   tier4_autoware_utils::StopWatch<std::chrono::milliseconds> stop_watch_;
@@ -179,8 +169,7 @@ class OptimizationBasedPlanner : public PlannerInterface
   // Publisher
   rclcpp::Publisher<Trajectory>::SharedPtr boundary_pub_;
   rclcpp::Publisher<Trajectory>::SharedPtr optimized_sv_pub_;
-  rclcpp::Publisher<Trajectory>::SharedPtr
-    optimized_st_graph_pub_;
+  rclcpp::Publisher<Trajectory>::SharedPtr optimized_st_graph_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr distance_to_closest_obj_pub_;
   rclcpp::Publisher<Float32Stamped>::SharedPtr debug_calculation_time_;
   rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_wall_marker_pub_;

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/planner_interface.hpp

@@ -16,10 +16,11 @@
 #define OBSTACLE_VELOCITY_PLANNER__PLANNER_INTERFACE_HPP_
 
 #include "obstacle_velocity_planner/common_structs.hpp"
+#include "tier4_autoware_utils/tier4_autoware_utils.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
-#include "tier4_planning_msgs/msg/velocity_limit.hpp"
 #include "autoware_auto_planning_msgs/msg/trajectory.hpp"
+#include "tier4_planning_msgs/msg/velocity_limit.hpp"
 
 #include <boost/optional.hpp>
 
@@ -33,19 +34,15 @@
 #include <memory>
 #include <vector>
 
-using tier4_planning_msgs::msg::VelocityLimit;
 using autoware_auto_planning_msgs::msg::Trajectory;
+using tier4_planning_msgs::msg::VelocityLimit;
 
 class PlannerInterface
 {
 public:
   PlannerInterface(
-    const double max_accel, const double min_accel, const double max_jerk, const double min_jerk,
-    const double min_object_accel, const double idling_time,
-    const vehicle_info_util::VehicleInfo & vehicle_info)
-  : longitudinal_info_(
-      RSSLongitudinalInfo(max_accel, min_accel, max_jerk, min_jerk, min_object_accel, idling_time)),
-    vehicle_info_(vehicle_info)
+    const LongitudinalInfo & longitudinal_info, const vehicle_info_util::VehicleInfo & vehicle_info)
+  : longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info)
   {
   }
 
@@ -57,10 +54,30 @@ class PlannerInterface
   // 2. generateTrajectory
   //   returns trajectory with planned velocity
   virtual boost::optional<size_t> getZeroVelocityIndexWithVelocityLimit(
-    [[maybe_unused]] const ObstacleVelocityPlannerData & planner_data, [[maybe_unused]] boost::optional<VelocityLimit> & vel_limit) { return {}; };
+    [[maybe_unused]] const ObstacleVelocityPlannerData & planner_data,
+    [[maybe_unused]] boost::optional<VelocityLimit> & vel_limit)
+  {
+    return {};
+  };
 
   virtual Trajectory generateTrajectory(
-    [[maybe_unused]] const ObstacleVelocityPlannerData & planner_data) { return Trajectory{}; }
+    [[maybe_unused]] const ObstacleVelocityPlannerData & planner_data)
+  {
+    return Trajectory{};
+  }
+
+  void updateCommonParam(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, "common.min_object_accel", i.min_object_accel);
+    tier4_autoware_utils::updateParam<double>(parameters, "common.idling_time", i.idling_time);
+  }
 
   virtual void updateParam([[maybe_unused]] const std::vector<rclcpp::Parameter> & parameters) {}
 
@@ -76,14 +93,11 @@ class PlannerInterface
   }
 
   // TODO(shimizu) remove this function
-  void setSmoothedTrajectory(const Trajectory::SharedPtr traj)
-  {
-    smoothed_trajectory_ptr_ = traj;
-  }
+  void setSmoothedTrajectory(const Trajectory::SharedPtr traj) { smoothed_trajectory_ptr_ = traj; }
 
 protected:
   // Parameters
-  RSSLongitudinalInfo longitudinal_info_;
+  LongitudinalInfo longitudinal_info_;
 
   // Vehicle Parameters
   vehicle_info_util::VehicleInfo vehicle_info_;

planning/obstacle_velocity_planner/include/obstacle_velocity_planner/rule_based_planner/rule_based_planner.hpp

@@ -16,15 +16,16 @@
 #define OBSTACLE_VELOCITY_PLANNER__RULE_BASED_PLANNER__RULE_BASED_PLANNER_HPP_
 
 #include "obstacle_velocity_planner/planner_interface.hpp"
-#include "obstacle_velocity_planner/rule_based_planner/pid_controller.hpp"
 #include "obstacle_velocity_planner/rule_based_planner/debug_values.hpp"
+#include "obstacle_velocity_planner/rule_based_planner/pid_controller.hpp"
 
-#include "visualization_msgs/msg/marker_array.hpp"
 #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
-#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
 #include "tier4_planning_msgs/msg/stop_reason_array.hpp"
+#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp"
+#include "visualization_msgs/msg/marker_array.hpp"
 
 #include <boost/optional.hpp>
+
 #include <memory>
 #include <vector>
 
@@ -35,12 +36,12 @@ class RuleBasedPlanner : public PlannerInterface
 {
 public:
   RuleBasedPlanner(
-    rclcpp::Node & node, const double max_accel, const double min_accel, const double max_jerk,
-    const double min_jerk, const double min_object_accel, const double idling_time,
+    rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
     const vehicle_info_util::VehicleInfo & vehicle_info);
 
   boost::optional<size_t> getZeroVelocityIndexWithVelocityLimit(
-    const ObstacleVelocityPlannerData & planner_data, boost::optional<VelocityLimit> & vel_limit) override;
+    const ObstacleVelocityPlannerData & planner_data,
+    boost::optional<VelocityLimit> & vel_limit) override;
 
   void updateParam(const std::vector<rclcpp::Parameter> & parameters) override;
 
@@ -51,7 +52,7 @@ class RuleBasedPlanner : public PlannerInterface
   double min_slow_down_target_vel_;
   double max_obj_velocity_for_stop_;
   double safe_distance_margin_;
-  double strong_min_accel_;
+  double min_obstacle_stop_accel_;
 
   // Publisher
   rclcpp::Publisher<tier4_planning_msgs::msg::StopReasonArray>::SharedPtr stop_reasons_pub_;
@@ -63,7 +64,8 @@ class RuleBasedPlanner : public PlannerInterface
   boost::optional<double> prev_target_vel_;
 
   size_t doStop(const ObstacleVelocityPlannerData & planner_data, const double dist_to_stop) const;
-  VelocityLimit doSlowDown(const ObstacleVelocityPlannerData & planner_data, const double dist_to_slow_down);
+  VelocityLimit doSlowDown(
+    const ObstacleVelocityPlannerData & planner_data, const double dist_to_slow_down);
 
   DebugValues debug_values_;
 };