Merge pull request #480 from tier4/sync-upstream

chore: sync upstream
tier4 · May 16, 2023 · 8593e6c · 8593e6c
2 parents a971e75 + b2adf8e
commit 8593e6c
Show file tree

Hide file tree

Showing 21 changed files with 248 additions and 103 deletions.
diff --git a/common/motion_utils/include/motion_utils/marker/marker_helper.hpp b/common/motion_utils/include/motion_utils/marker/marker_helper.hpp
@@ -28,15 +28,15 @@ using geometry_msgs::msg::Pose;
 
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0);
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0);
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const Pose & pose, const std::string & module_name, const rclcpp::Time & now, const int32_t id,
-  const double longitudinal_offset = 0.0);
+  const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
   const rclcpp::Time & now, const int32_t id);
@@ -58,29 +58,32 @@ class VirtualWallMarkerCreator
 
   using create_wall_function = std::function<visualization_msgs::msg::MarkerArray(
     const geometry_msgs::msg::Pose & pose, const std::string & module_name,
-    const rclcpp::Time & now, const int32_t id, const double longitudinal_offset)>;
+    const rclcpp::Time & now, const int32_t id, const double longitudinal_offset,
+    const std::string & ns_prefix)>;
 
   using delete_wall_function =
     std::function<visualization_msgs::msg::MarkerArray(const rclcpp::Time & now, const int32_t id)>;
 
   visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
     const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
-    const double longitudinal_offset = 0.0);
+    const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
 
   visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
     const std::vector<Pose> & slow_down_poses, const std::string & module_name,
-    const rclcpp::Time & now, const double longitudinal_offset = 0.0);
+    const rclcpp::Time & now, const double longitudinal_offset = 0.0,
+    const std::string & ns_prefix = "");
 
   visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
     const std::vector<Pose> & dead_line_poses, const std::string & module_name,
-    const rclcpp::Time & now, const double longitudinal_offset = 0.0);
+    const rclcpp::Time & now, const double longitudinal_offset = 0.0,
+    const std::string & ns_prefix = "");
 
 private:
   visualization_msgs::msg::MarkerArray handleVirtualWallMarker(
     const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
     create_wall_function function_create_wall_marker,
     delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
-    const double longitudinal_offset = 0.0);
+    const double longitudinal_offset = 0.0, const std::string & ns_prefix = "");
 
   size_t previous_stop_poses_nb_ = 0UL;
   size_t previous_slow_down_poses_nb_ = 0UL;

diff --git a/common/motion_utils/src/marker/marker_helper.cpp b/common/motion_utils/src/marker/marker_helper.cpp
@@ -86,32 +86,35 @@ namespace motion_utils
 {
 visualization_msgs::msg::MarkerArray createStopVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
 {
   const auto pose_with_offset =
     tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
   return createVirtualWallMarkerArray(
-    pose_with_offset, module_name, "stop_", now, id, createMarkerColor(1.0, 0.0, 0.0, 0.5));
+    pose_with_offset, module_name, ns_prefix + "stop_", now, id,
+    createMarkerColor(1.0, 0.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createSlowDownVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
 {
   const auto pose_with_offset =
     tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
   return createVirtualWallMarkerArray(
-    pose_with_offset, module_name, "slow_down_", now, id, createMarkerColor(1.0, 1.0, 0.0, 0.5));
+    pose_with_offset, module_name, ns_prefix + "slow_down_", now, id,
+    createMarkerColor(1.0, 1.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createDeadLineVirtualWallMarker(
   const geometry_msgs::msg::Pose & pose, const std::string & module_name, const rclcpp::Time & now,
-  const int32_t id, const double longitudinal_offset)
+  const int32_t id, const double longitudinal_offset, const std::string & ns_prefix)
 {
   const auto pose_with_offset =
     tier4_autoware_utils::calcOffsetPose(pose, longitudinal_offset, 0.0, 0.0);
   return createVirtualWallMarkerArray(
-    pose_with_offset, module_name, "dead_line_", now, id, createMarkerColor(0.0, 1.0, 0.0, 0.5));
+    pose_with_offset, module_name, ns_prefix + "dead_line_", now, id,
+    createMarkerColor(0.0, 1.0, 0.0, 0.5));
 }
 
 visualization_msgs::msg::MarkerArray createDeletedStopVirtualWallMarker(
@@ -136,7 +139,7 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall
   const std::vector<Pose> & poses, const std::string & module_name, const rclcpp::Time & now,
   create_wall_function function_create_wall_marker,
   delete_wall_function function_delete_wall_marker, size_t & previous_virtual_walls_nb,
-  const double longitudinal_offset)
+  const double longitudinal_offset, const std::string & ns_prefix)
 {
   visualization_msgs::msg::MarkerArray wall_marker;
 
@@ -145,7 +148,8 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall
 
   for (const auto & p : poses) {
     appendMarkerArray(
-      function_create_wall_marker(p, module_name, now, id++, longitudinal_offset), &wall_marker);
+      function_create_wall_marker(p, module_name, now, id++, longitudinal_offset, ns_prefix),
+      &wall_marker);
   }
 
   while (id < max_id) {
@@ -158,37 +162,38 @@ visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::handleVirtualWall
 
 visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createStopVirtualWallMarker(
   const std::vector<Pose> & stop_poses, const std::string & module_name, const rclcpp::Time & now,
-  const double longitudinal_offset)
+  const double longitudinal_offset, const std::string & ns_prefix)
 {
   create_wall_function creator = motion_utils::createStopVirtualWallMarker;
   delete_wall_function deleter = motion_utils::createDeletedStopVirtualWallMarker;
 
   return handleVirtualWallMarker(
-    stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset);
+    stop_poses, module_name, now, creator, deleter, previous_stop_poses_nb_, longitudinal_offset,
+    ns_prefix);
 }
 
 visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createSlowDownVirtualWallMarker(
   const std::vector<Pose> & slow_down_poses, const std::string & module_name,
-  const rclcpp::Time & now, const double longitudinal_offset)
+  const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
 {
   create_wall_function creator = motion_utils::createSlowDownVirtualWallMarker;
   delete_wall_function deleter = motion_utils::createDeletedSlowDownVirtualWallMarker;
 
   return handleVirtualWallMarker(
     slow_down_poses, module_name, now, creator, deleter, previous_slow_down_poses_nb_,
-    longitudinal_offset);
+    longitudinal_offset, ns_prefix);
 }
 
 visualization_msgs::msg::MarkerArray VirtualWallMarkerCreator::createDeadLineVirtualWallMarker(
   const std::vector<Pose> & dead_line_poses, const std::string & module_name,
-  const rclcpp::Time & now, const double longitudinal_offset)
+  const rclcpp::Time & now, const double longitudinal_offset, const std::string & ns_prefix)
 {
   create_wall_function creator = motion_utils::createDeadLineVirtualWallMarker;
   delete_wall_function deleter = motion_utils::createDeletedDeadLineVirtualWallMarker;
 
   return handleVirtualWallMarker(
     dead_line_poses, module_name, now, creator, deleter, previous_dead_line_poses_nb_,
-    longitudinal_offset);
+    longitudinal_offset, ns_prefix);
 }
 
 }  // namespace motion_utils
diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -526,6 +526,11 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     RCLCPP_INFO_SKIPFIRST_THROTTLE(node_->get_logger(), *node_->get_clock(), 5000, "%s", s);
   };
 
+  // if current operation mode is not autonomous mode, then change state to stopped
+  if (m_current_operation_mode.mode != OperationModeState::AUTONOMOUS) {
+    return changeState(ControlState::STOPPED);
+  }
+
   // transit state
   // in DRIVE state
   if (m_control_state == ControlState::DRIVE) {

diff --git a/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml b/...launch/launch/object_recognition/detection/camera_lidar_fusion_based_detection.launch.xml
@@ -19,7 +19,7 @@
   <arg name="camera_info6" default="/camera_info6"/>
   <arg name="image_raw7" default="/image_raw7"/>
   <arg name="camera_info7" default="/camera_info7"/>
-  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
+  <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
   <arg name="lidar_detection_model" default="centerpoint" description="options: `centerpoint`, `apollo`, `pointpainting`, `clustering`"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>

diff --git a/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml b/.../launch/object_recognition/detection/camera_lidar_radar_fusion_based_detection.launch.xml
@@ -26,7 +26,7 @@
   <arg name="camera_info6" default="/camera_info6"/>
   <arg name="image_raw7" default="/image_raw7"/>
   <arg name="camera_info7" default="/camera_info7"/>
-  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
+  <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
   <!-- camera lidar fusion based detection-->
   <group>

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
@@ -27,7 +27,7 @@
   <arg name="camera_info6" default=""/>
   <arg name="image_raw7" default=""/>
   <arg name="camera_info7" default=""/>
-  <arg name="image_number" default="1" description="choose image raw number(0-7)"/>
+  <arg name="image_number" default="1" description="choose image raw number(1-8)"/>
 
   <!-- camera lidar fusion param -->
   <arg name="remove_unknown" default="true"/>

diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml
@@ -35,7 +35,7 @@
   <arg name="camera_info6" default="/sensing/camera/camera6/camera_info"/>
   <arg name="image_raw7" default="/sensing/camera/camera7/image_rect_color"/>
   <arg name="camera_info7" default="/sensing/camera/camera7/camera_info"/>
-  <arg name="image_number" default="6" description="choose image raw number(0-7)"/>
+  <arg name="image_number" default="6" description="choose image raw number(1-8)"/>
   <arg name="use_vector_map" default="true" description="use vector map in prediction"/>
   <arg name="use_pointcloud_map" default="true" description="use pointcloud map in detection"/>
   <arg name="use_object_filter" default="true" description="use object filter"/>

diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml
@@ -1,7 +1,7 @@
 /**:
   ros__parameters:
     lane_change:
-      lane_changing_safety_check_duration: 8.0  # [s]
+      backward_lane_length: 200.0 #[m]
       prepare_duration: 4.0         # [s]
 
       backward_length_buffer_for_end_of_lane: 3.0 # [m]

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_goal_planner_design.md
@@ -2,50 +2,54 @@
 
 ## Purpose / Role
 
-Search for a space where there are no objects and goal planner there.
+Plan path around the goal.
+
+- Park at the designated goal.
+- Modify the goal to avoid obstacles or to pull over at the side of tha lane.
 
 ## Design
 
+If goal modification is not allowed, park at the designated fixed goal. (`fixed_goal_planner` in the figure below)
+When allowed, park in accordance with the specified policy(e.g pull over on left/right side of the lane). (`rough_goal_planner` in the figure below). Currently rough goal planner only support pull_over feature, but it would be desirable to be able to accommodate various parking policies in the future.
+
 ```plantuml
 @startuml
 package goal_planner{
-    abstract class PullOverPlannerBase {
-    }
-    abstract class GoalSeacherBase {
-    }
 
-    package lane_parking <<Rectangle>>{
-        class ShiftPullOver {
-        }
-        class GeometricPullOver {
+    class GoalPlannerModule {}
+
+    package rough_goal_planner <<Rectangle>>{
+
+        package lane_parking <<Rectangle>>{
+            class ShiftPullOver {}
+            class GeometricPullOver {}
         }
-    }
 
-    package freespace_parking <<Rectangle>>{
-        class FreeSpacePullOver {
+        package freespace_parking <<Rectangle>>{
+            class FreeSpacePullOver {}
         }
-    }
 
-    class GoalSeacher {
-    }
+        class GoalSeacher {}
 
-    class GoalPlannerModule {
-    }
+        struct GoalCandidates {}
+        struct PullOverPath{}
 
+        abstract class PullOverPlannerBase {}
+        abstract class GoalSeacherBase {}
 
-    struct GoalCandidates {
     }
 
-    struct PullOverPath{}
+    package fixed_goal_planner <<Rectangle>>{
+        abstract class FixedGoalPlannerBase {}
+        class DefaultFixedPlanner{}
+    }
 }
 
 
 package utils{
-    class PathShifter {
-    }
+    class PathShifter {}
 
-    class GeometricParallelParking {
-    }
+    class GeometricParallelParking {}
 }
 
 package freespace_planning_algorithms
@@ -59,6 +63,7 @@ ShiftPullOver --|> PullOverPlannerBase
 GeometricPullOver --|> PullOverPlannerBase
 FreeSpacePullOver --|> PullOverPlannerBase
 GoalSeacher --|> GoalSeacherBase
+DefaultFixedPlanner --|> FixedGoalPlannerBase
 
 PathShifter --o ShiftPullOver
 GeometricParallelParking --o GeometricPullOver
@@ -67,18 +72,55 @@ RRTStar --o FreeSpacePullOver
 
 PullOverPlannerBase --o GoalPlannerModule
 GoalSeacherBase --o GoalPlannerModule
+FixedGoalPlannerBase --o GoalPlannerModule
 
 PullOverPath --o PullOverPlannerBase
 GoalCandidates --o GoalSeacherBase
 
 @enduml
 ```
 
+## start condition
+
+Either one is activated when all conditions are met.
+
+### fixed_goal_planner
+
+- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- Route is set with `allow_goal_modification=false` by default.
+
+<img src="https://user-images.githubusercontent.com/39142679/237929955-c0adf01b-9e3c-45e3-848d-98cf11e52b65.png" width="600">
+
+### rough_goal_planner
+
+#### pull over on road lane
+
+- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- Route is set with `allow_goal_modification=true` .
+  - We can set this option with [SetRoute](https://github.com/autowarefoundation/autoware_adapi_msgs/blob/main/autoware_adapi_v1_msgs/routing/srv/SetRoute.srv#L2) api service.
+  - We support `2D Rough Goal Pose` with the key bind `r` in RViz, but in the future there will be a panel of tools to manipulate various Route API from RViz.
+- ego-vehicle is in the same lane as the goal.
+
+<img src="https://user-images.githubusercontent.com/39142679/237929950-989ca6c3-d48c-4bb5-81e5-e8d6a38911aa.png" width="600">
+
+#### pull over on shoulder lane
+
+- The distance between the goal and ego-vehicle is shorter than `minimum_request_length`.
+- Goal is set in the `road_shoulder`.
+
+<img src="https://user-images.githubusercontent.com/39142679/237929941-2ce26ea5-c84d-4d17-8cdc-103f5246db90.png" width="600">
+
+## finish condition
+
+- The distance to the goal from your vehicle is lower than threshold (default: < `1m`).
+- The ego-vehicle is stopped.
+  - The speed is lower than threshold (default: < `0.01m/s`).
+
 ## General parameters for goal_planner
 
 | Name                       | Unit   | Type   | Description                                                                                                                             | Default value |
 | :------------------------- | :----- | :----- | :-------------------------------------------------------------------------------------------------------------------------------------- | :------------ |
-| minimum_request_length     | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated.                          | 200.0         |
+| minimum_request_length     | [m]    | double | when the ego-vehicle approaches the goal by this distance or a safe distance to stop, the module is activated.                          | 100.0         |
 | th_arrived_distance        | [m]    | double | distance threshold for arrival of path termination                                                                                      | 1.0           |
 | th_stopped_velocity        | [m/s]  | double | velocity threshold for arrival of path termination                                                                                      | 0.01          |
 | th_stopped_time            | [s]    | double | time threshold for arrival of path termination                                                                                          | 2.0           |

diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_manager_design.md
@@ -13,15 +13,16 @@ The manager launches and executes scene modules in `behavior_path_planner` depen
 
 Support status:
 
-| Name                     | Simple exclusive execution | Advanced simultaneous execution |
-| :----------------------- | :------------------------: | :-----------------------------: |
-| Avoidance                |     :heavy_check_mark:     |       :heavy_check_mark:        |
-| Avoidance By Lane Change |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Lane Change              |     :heavy_check_mark:     |       :heavy_check_mark:        |
-| External Lane Change     |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Pull Over                |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Pull Out                 |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
-| Side Shift               |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Name                                     | Simple exclusive execution | Advanced simultaneous execution |
+| :--------------------------------------- | :------------------------: | :-----------------------------: |
+| Avoidance                                |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Avoidance By Lane Change                 |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Lane Change                              |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| External Lane Change                     |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Goal Planner (without goal modification) |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Goal Planner (with goal modification)    |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
+| Pull Out                                 |     :heavy_check_mark:     |       :heavy_check_mark:        |
+| Side Shift                               |     :heavy_check_mark:     |    :heavy_multiplication_x:     |
 
 Click [here](../README.md) for supported scene modules.
 

diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -238,9 +238,6 @@ class LaneChangeBase
 
   PathWithLaneId prev_approved_path_{};
 
-  double lane_change_lane_length_{200.0};
-  double check_length_{100.0};
-
   bool is_abort_path_approved_{false};
   bool is_abort_approval_requested_{false};
   bool is_activated_{false};