Merge pull request autowarefoundation#703 from tier4/sync-upstream

chore: sync upstream

.cspell-partial.json

@@ -1,5 +1,5 @@
 {
-  "ignorePaths": ["**/control/**", "**/perception/**", "**/planning/**", "**/sensing/**"],
+  "ignorePaths": ["**/perception/**", "**/planning/**", "**/sensing/**"],
   "ignoreRegExpList": [],
   "words": []
 }

control/mpc_lateral_controller/model_predictive_control_algorithm.md

@@ -125,7 +125,7 @@ Substituting equation (8) into equation (9) and tidying up the equation for $U$.
 $$
 \begin{align}
 J(U) &= (H(Fx_{0}+GU+SW)-Y_{ref})^{T}Q(H(Fx_{0}+GU+SW)-Y_{ref})+(U-U_{ref})^{T}R(U-U_{ref}) \\
-& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Yref)^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10}
+& =U^{T}(G^{T}H^{T}QHG+R)U+2\left\{(H(Fx_{0}+SW)-Y_{ref})^{T}QHG-U_{ref}^{T}R\right\}U +(\rm{constant}) \tag{10}
 \end{align}
 $$
 
@@ -182,7 +182,7 @@ Where $\kappa_{r}\left(s\right)$ is the curvature along the trajectory parametri
 
 There are three expressions in the update equations that are subject to linear approximation: the lateral deviation (or lateral coordinate) $y$, the heading angle (or the heading angle error) $\theta$, and the steering $\delta$. We can make a small angle assumption on the heading angle $\theta$.
 
-In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackerman steering expression can be written as;
+In the path tracking problem, the curvature of the trajectory $\kappa_{r}$ is known in advance. At the lower speeds, the Ackermann formula approximates the reference steering angle $\theta_{r}$(this value corresponds to the $U_{ref}$ mentioned above). The Ackermann steering expression can be written as;
 
 $$
 \begin{align}
@@ -344,7 +344,7 @@ $$
 \end{align}
 $$
 
-Discretizing $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt the resulting constraint become linear and convex
+We discretize $\dot{u}$ as $\left(u_{k} - u_{k-1}\right)/\text{d}t$ and multiply both sides by dt, and the resulting constraint become linear and convex
 
 $$
 \begin{align}

launch/tier4_perception_launch/launch/perception.launch.xml

@@ -22,6 +22,7 @@
   <arg name="occupancy_grid_map_param_path"/>
   <arg name="occupancy_grid_map_updater"/>
   <arg name="occupancy_grid_map_updater_param_path"/>
+  <arg name="traffic_light_arbiter_param_path"/>
 
   <!-- centerpoint model configs -->
   <arg name="centerpoint_model_name" default="centerpoint_tiny" description="options: `centerpoint` or `centerpoint_tiny`"/>

localization/gyro_odometer/src/gyro_odometer_core.cpp

@@ -102,7 +102,7 @@ geometry_msgs::msg::TwistWithCovarianceStamped concatGyroAndOdometer(
 
 GyroOdometer::GyroOdometer(const rclcpp::NodeOptions & options)
 : Node("gyro_odometer", options),
-  output_frame_(declare_parameter("base_link", "base_link")),
+  output_frame_(declare_parameter("output_frame", "base_link")),
   message_timeout_sec_(declare_parameter("message_timeout_sec", 0.2)),
   vehicle_twist_arrived_(false),
   imu_arrived_(false)

perception/traffic_light_arbiter/CMakeLists.txt

@@ -12,4 +12,4 @@ rclcpp_components_register_nodes(${PROJECT_NAME}
   TrafficLightArbiter
 )
 
-ament_auto_package(INSTALL_TO_SHARE launch)
+ament_auto_package(INSTALL_TO_SHARE launch config)

perception/traffic_light_arbiter/config/traffic_light_arbiter.param.yaml

@@ -0,0 +1,5 @@
+/**:
+  ros__parameters:
+    external_time_tolerance: 5.0
+    perception_time_tolerance: 1.0
+    external_priority: false

perception/traffic_light_arbiter/launch/traffic_light_arbiter.launch.xml

@@ -2,6 +2,7 @@
   <arg name="perception_traffic_signals" default="/internal/traffic_signals"/>
   <arg name="external_traffic_signals" default="/external/traffic_signals"/>
   <arg name="output_traffic_signals" default="/traffic_light_arbiter/traffic_signals"/>
+  <arg name="traffic_light_arbiter_param_path" default="$(find-pkg-share traffic_light_arbiter)/config/traffic_light_arbiter.param.yaml"/>
 
   <push-ros-namespace namespace="traffic_light_arbiter"/>
   <node_container pkg="rclcpp_components" exec="component_container" name="container" namespace="">
@@ -10,6 +11,7 @@
       <remap from="~/sub/perception_traffic_signals" to="$(var perception_traffic_signals)"/>
       <remap from="~/sub/external_traffic_signals" to="$(var external_traffic_signals)"/>
       <remap from="~/pub/traffic_signals" to="$(var output_traffic_signals)"/>
+      <param from="$(var traffic_light_arbiter_param_path)"/>
     </composable_node>
   </node_container>
 </launch>

perception/traffic_light_arbiter/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The traffic_light_arbiter package</description>
   <maintainer email="kenzo.lobos@tier4.jp">Kenzo Lobos-Tsunekawa</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/traffic_light_classifier/launch/traffic_light_classifier.launch.xml

@@ -2,6 +2,9 @@
   <arg name="input/image" default="~/image_raw"/>
   <arg name="input/rois" default="~/rois"/>
   <arg name="output/traffic_signals" default="classified/traffic_signals"/>
+  <arg name="classifier_label_path" default="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt" description="classifier label path"/>
+  <arg name="classifier_model_path" default="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_mobilenetv2_batch_6.onnx" description="classifier onnx model path"/>
+  <arg name="classifier_precision" default="fp16"/>
 
   <arg name="use_gpu" default="true"/>
   <!-- classifier_type {hsv_filter: 0, cnn: 1} -->
@@ -16,9 +19,9 @@
     <remap from="~/output/traffic_signals" to="$(var output/traffic_signals)"/>
     <param name="approximate_sync" value="false"/>
     <param name="classifier_type" value="$(var classifier_type)"/>
-    <param name="classifier_model_path" value="$(find-pkg-share traffic_light_classifier)/data/traffic_light_classifier_efficientNet_b1.onnx"/>
-    <param name="classifier_label_path" value="$(find-pkg-share traffic_light_classifier)/data/lamp_labels.txt"/>
-    <param name="classifier_precision" value="fp16"/>
+    <param name="classifier_label_path" value="$(var classifier_label_path)"/>
+    <param name="classifier_model_path" value="$(var classifier_model_path)"/>
+    <param name="classifier_precision" value="$(var classifier_precision)"/>
     <param name="build_only" value="$(var build_only)"/>
   </node>
 </launch>

perception/traffic_light_fine_detector/launch/traffic_light_fine_detector.launch.xml

@@ -1,6 +1,6 @@
 <launch>
-  <arg name="fine_detector_model_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_yolo_s.onnx"/>
-  <arg name="fine_detector_label_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_labels.txt"/>
+  <arg name="fine_detector_label_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_labels.txt" description="fine detector label path"/>
+  <arg name="fine_detector_model_path" default="$(find-pkg-share traffic_light_fine_detector)/data/tlr_yolox_s_batch_6.onnx" description="fine detector onnx model path"/>
   <arg name="fine_detector_precision" default="fp16"/>
   <arg name="fine_detector_score_thresh" default="0.3"/>
   <arg name="fine_detector_nms_thresh" default="0.65"/>

perception/traffic_light_fine_detector/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The traffic_light_fine_detector package</description>
   <maintainer email="mingyu.li@tier4.jp">Mingyu Li</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/traffic_light_multi_camera_fusion/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The traffic_light_multi_camera_fusion package</description>
   <maintainer email="mingyu.li@tier4.jp">Mingyu Li</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

perception/traffic_light_occlusion_predictor/package.xml

@@ -5,6 +5,7 @@
   <version>0.1.0</version>
   <description>The traffic_light_occlusion_predictor package</description>
   <maintainer email="mingyu.li@tier4.jp">Mingyu Li</maintainer>
+  <maintainer email="shunsuke.miura@tier4.jp">Shunsuke Miura</maintainer>
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake_auto</buildtool_depend>

planning/behavior_path_planner/config/avoidance/avoidance.param.yaml

@@ -160,6 +160,7 @@
         # avoidance distance parameters
         longitudinal:
           prepare_time: 2.0                             # [s]
+          remain_buffer_distance: 30.0                  # [m]
           min_prepare_distance: 1.0                     # [m]
           min_slow_down_speed: 1.38                     # [m/s]
           buf_slow_down_speed: 0.56                     # [m/s]

planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md

@@ -157,6 +157,12 @@ stop
 
 See [safety check utils explanation](../docs/behavior_path_planner_safety_check.md)
 
+#### Objects selection and classification
+
+First, we divide the target objects into obstacles in the target lane, obstacles in the current lane, and obstacles in other lanes. Target lane indicates the lane that the ego vehicle is going to reach after the lane change and current lane mean the current lane where the ego vehicle is following before the lane change. Other lanes are lanes that do not belong to the target and current lanes. The following picture describes objects on each lane. Note that users can remove objects either on current and other lanes from safety check by changing the flag, which are `check_objects_on_current_lanes` and `check_objects_on_other_lanes`.
+
+![object lanes](../image/lane_change/lane_objects.drawio.svg)
+
 ##### Collision check in prepare phase
 
 The ego vehicle may need to secure ample inter-vehicle distance ahead of the target vehicle before attempting a lane change. The flag `enable_collision_check_at_prepare_phase` can be enabled to gain this behavior. The following image illustrates the differences between the `false` and `true` cases.
@@ -292,7 +298,8 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`.
 | `rear_vehicle_safety_time_margin`          | [s]     | double  | The time buffer for the rear vehicle to come into complete stop when its driver perform sudden braking.                                                        | 2.0           |
 | `enable_collision_check_at_prepare_phase`  | [-]     | boolean | Perform collision check starting from prepare phase. If `false`, collision check only evaluated for lane changing phase.                                       | true          |
 | `prepare_phase_ignore_target_speed_thresh` | [m/s]   | double  | Ignore collision check in prepare phase of object speed that is lesser that the configured value. `enable_collision_check_at_prepare_phase` must be `true`     | 0.1           |
-| `use_predicted_path_outside_lanelet`       | [-]     | boolean | If true, include collision check for predicted path that is out of lanelet (freespace).                                                                        | false         |
+| `check_objects_on_current_lanes`           | [-]     | boolean | If true, the lane change module include objects on current lanes.                                                                                              | true          |
+| `check_objects_on_other_lanes`             | [-]     | boolean | If true, the lane change module include objects on other lanes.                                                                                                | true          |
 | `use_all_predicted_path`                   | [-]     | boolean | If false, use only the predicted path that has the maximum confidence.                                                                                         | true          |
 
 (\*1) the value must be negative.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/avoidance_module.hpp

@@ -49,7 +49,7 @@ class AvoidanceModule : public SceneModuleInterface
 public:
   AvoidanceModule(
     const std::string & name, rclcpp::Node & node, std::shared_ptr<AvoidanceParameters> parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
   ModuleStatus updateState() override;
   CandidateOutput planCandidate() const override;
@@ -62,9 +62,9 @@ class AvoidanceModule : public SceneModuleInterface
   void updateData() override;
   void acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const override;
 
-  void updateModuleParams(const std::shared_ptr<AvoidanceParameters> & parameters)
+  void updateModuleParams(const std::any & parameters) override
   {
-    parameters_ = parameters;
+    parameters_ = std::any_cast<std::shared_ptr<AvoidanceParameters>>(parameters);
   }
   std::shared_ptr<AvoidanceDebugMsgArray> get_debug_msg_array() const;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/avoidance/manager.hpp

@@ -44,8 +44,6 @@ class AvoidanceModuleManager : public SceneModuleManagerInterface
 
 private:
   std::shared_ptr<AvoidanceParameters> parameters_;
-
-  std::vector<std::shared_ptr<AvoidanceModule>> registered_modules_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp

@@ -129,9 +129,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
     std::shared_ptr<DynamicAvoidanceParameters> parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
-  void updateModuleParams(const std::shared_ptr<DynamicAvoidanceParameters> & parameters)
+  void updateModuleParams(const std::any & parameters) override
   {
-    parameters_ = parameters;
+    parameters_ = std::any_cast<std::shared_ptr<DynamicAvoidanceParameters>>(parameters);
   }
 
   bool isExecutionRequested() const override;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/dynamic_avoidance/manager.hpp

@@ -44,7 +44,6 @@ class DynamicAvoidanceModuleManager : public SceneModuleManagerInterface
 
 private:
   std::shared_ptr<DynamicAvoidanceParameters> parameters_;
-  std::vector<std::shared_ptr<DynamicAvoidanceModule>> registered_modules_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp

@@ -92,9 +92,9 @@ class GoalPlannerModule : public SceneModuleInterface
     const std::shared_ptr<GoalPlannerParameters> & parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
-  void updateModuleParams(const std::shared_ptr<GoalPlannerParameters> & parameters)
+  void updateModuleParams(const std::any & parameters) override
   {
-    parameters_ = parameters;
+    parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
   }
 
   BehaviorModuleOutput run() override;
@@ -194,7 +194,7 @@ class GoalPlannerModule : public SceneModuleInterface
     std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
   bool hasFinishedCurrentPath();
   bool hasFinishedGoalPlanner();
-  bool isOnGoal() const;
+  bool isOnModifiedGoal() const;
   double calcModuleRequestLength() const;
   void resetStatus();
   bool needPathUpdate(const double path_update_duration) const;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/manager.hpp

@@ -42,8 +42,6 @@ class GoalPlannerModuleManager : public SceneModuleManagerInterface
 
 private:
   std::shared_ptr<GoalPlannerParameters> parameters_;
-
-  std::vector<std::shared_ptr<GoalPlannerModule>> registered_modules_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/interface.hpp

@@ -87,7 +87,7 @@ class LaneChangeInterface : public SceneModuleInterface
 
   void acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const override;
 
-  void updateModuleParams(const std::shared_ptr<LaneChangeParameters> & parameters);
+  void updateModuleParams(const std::any & parameters) override;
 
   void setData(const std::shared_ptr<const PlannerData> & data) override;
 

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp

@@ -44,8 +44,6 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface
 protected:
   std::shared_ptr<LaneChangeParameters> parameters_;
 
-  std::vector<std::shared_ptr<LaneChangeInterface>> registered_modules_;
-
   Direction direction_;
 
   LaneChangeModuleType type_;

planning/behavior_path_planner/include/behavior_path_planner/scene_module/scene_module_interface.hpp

@@ -37,6 +37,7 @@
 #include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
 #include <algorithm>
+#include <any>
 #include <limits>
 #include <memory>
 #include <random>
@@ -88,6 +89,8 @@ class SceneModuleInterface
 
   virtual ~SceneModuleInterface() = default;
 
+  virtual void updateModuleParams(const std::any & parameters) = 0;
+
   /**
    * @brief Return SUCCESS if plan is not needed or plan is successfully finished,
    *        FAILURE if plan has failed, RUNNING if plan is on going.

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/manager.hpp

@@ -43,8 +43,6 @@ class SideShiftModuleManager : public SceneModuleManagerInterface
 
 private:
   std::shared_ptr<SideShiftParameters> parameters_;
-
-  std::vector<std::shared_ptr<SideShiftModule>> registered_modules_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/side_shift/side_shift_module.hpp

@@ -43,7 +43,7 @@ class SideShiftModule : public SceneModuleInterface
   SideShiftModule(
     const std::string & name, rclcpp::Node & node,
     const std::shared_ptr<SideShiftParameters> & parameters,
-    const std::unordered_map<std::string, std::shared_ptr<RTCInterface> > & rtc_interface_ptr_map);
+    const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
   bool isExecutionRequested() const override;
   bool isExecutionReady() const override;
@@ -59,9 +59,9 @@ class SideShiftModule : public SceneModuleInterface
 
   void setParameters(const std::shared_ptr<SideShiftParameters> & parameters);
 
-  void updateModuleParams(const std::shared_ptr<SideShiftParameters> & parameters)
+  void updateModuleParams(const std::any & parameters) override
   {
-    parameters_ = parameters;
+    parameters_ = std::any_cast<std::shared_ptr<SideShiftParameters>>(parameters);
   }
 
   void acceptVisitor(

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/manager.hpp

@@ -42,8 +42,6 @@ class StartPlannerModuleManager : public SceneModuleManagerInterface
 
 private:
   std::shared_ptr<StartPlannerParameters> parameters_;
-
-  std::vector<std::shared_ptr<StartPlannerModule>> registered_modules_;
 };
 
 }  // namespace behavior_path_planner

planning/behavior_path_planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp

@@ -51,11 +51,7 @@ struct PullOutStatus
   size_t current_path_idx{0};
   PlannerType planner_type{PlannerType::NONE};
   PathWithLaneId backward_path{};
-  lanelet::ConstLanelets current_lanes{};
   lanelet::ConstLanelets pull_out_lanes{};
-  std::vector<DrivableLanes> lanes{};
-  std::vector<uint64_t> lane_follow_lane_ids{};
-  std::vector<uint64_t> pull_out_lane_ids{};
   bool is_safe{false};
   bool back_finished{false};
   Pose pull_out_start_pose{};
@@ -71,9 +67,9 @@ class StartPlannerModule : public SceneModuleInterface
     const std::shared_ptr<StartPlannerParameters> & parameters,
     const std::unordered_map<std::string, std::shared_ptr<RTCInterface>> & rtc_interface_ptr_map);
 
-  void updateModuleParams(const std::shared_ptr<StartPlannerParameters> & parameters)
+  void updateModuleParams(const std::any & parameters) override
   {
-    parameters_ = parameters;
+    parameters_ = std::any_cast<std::shared_ptr<StartPlannerParameters>>(parameters);
   }
 
   BehaviorModuleOutput run() override;
@@ -139,6 +135,8 @@ class StartPlannerModule : public SceneModuleInterface
     const std::vector<Pose> & start_pose_candidates, const Pose & goal_pose,
     const std::string search_priority);
   PathWithLaneId generateStopPath() const;
+  lanelet::ConstLanelets getPathLanes(const PathWithLaneId & path) const;
+  std::vector<DrivableLanes> generateDrivableLanes(const PathWithLaneId & path) const;
   void updatePullOutStatus();
   static bool isOverlappedWithLane(
     const lanelet::ConstLanelet & candidate_lanelet,

planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp

@@ -225,6 +225,9 @@ struct AvoidanceParameters
   // nominal avoidance sped
   double nominal_avoidance_speed;
 
+  // module try to return original path to keep this distance from edge point of the path.
+  double remain_buffer_distance;
+
   // The margin is configured so that the generated avoidance trajectory does not come near to the
   // road shoulder.
   double road_shoulder_safety_margin{1.0};