[pull] main from autowarefoundation:main

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;

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

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) {

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -44,6 +44,7 @@
   <arg name="freespace_planner_param_path"/>
   <!-- motion -->
   <arg name="obstacle_avoidance_planner_param_path"/>
+  <arg name="path_sampler_param_path"/>
   <arg name="obstacle_velocity_limiter_param_path"/>
   <arg name="surround_obstacle_checker_param_path"/>
   <arg name="obstacle_stop_planner_param_path"/>

launch/tier4_planning_launch/launch/scenario_planning/lane_driving/motion_planning/motion_planning.launch.py

@@ -64,6 +64,27 @@ def launch_setup(context, *args, **kwargs):
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # path sampler
+    with open(LaunchConfiguration("path_sampler_param_path").perform(context), "r") as f:
+        path_sampler_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    path_sampler_component = ComposableNode(
+        package="path_sampler",
+        plugin="path_sampler::PathSampler",
+        name="path_sampler",
+        namespace="",
+        remappings=[
+            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/output/path", "obstacle_avoidance_planner/trajectory"),
+        ],
+        parameters=[
+            nearest_search_param,
+            path_sampler_param,
+            vehicle_info_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
     # obstacle velocity limiter
     with open(
         LaunchConfiguration("obstacle_velocity_limiter_param_path").perform(context), "r"
@@ -209,7 +230,6 @@ def launch_setup(context, *args, **kwargs):
         package="rclcpp_components",
         executable=LaunchConfiguration("container_executable"),
         composable_node_descriptions=[
-            obstacle_avoidance_planner_component,
             obstacle_velocity_limiter_component,
         ],
     )
@@ -232,6 +252,18 @@ def launch_setup(context, *args, **kwargs):
         condition=LaunchConfigurationEquals("cruise_planner_type", "none"),
     )
 
+    obstacle_avoidance_planner_loader = LoadComposableNodes(
+        composable_node_descriptions=[obstacle_avoidance_planner_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_planner_type", "obstacle_avoidance_planner"),
+    )
+
+    path_sampler_loader = LoadComposableNodes(
+        composable_node_descriptions=[path_sampler_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_planner_type", "path_sampler"),
+    )
+
     surround_obstacle_checker_loader = LoadComposableNodes(
         composable_node_descriptions=[surround_obstacle_checker_component],
         target_container=container,
@@ -241,6 +273,8 @@ def launch_setup(context, *args, **kwargs):
     group = GroupAction(
         [
             container,
+            obstacle_avoidance_planner_loader,
+            path_sampler_loader,
             obstacle_stop_planner_loader,
             obstacle_cruise_planner_loader,
             obstacle_cruise_planner_relay_loader,
@@ -273,6 +307,9 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg(
         "cruise_planner_type"
     )  # select from "obstacle_stop_planner", "obstacle_cruise_planner", "none"
+    add_launch_arg(
+        "path_planner_type", "obstacle_avoidance_planner"
+    )  # select from "obstacle_avoidance_planner", "path_sampler"
 
     add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")

planning/behavior_path_planner/config/lane_change/lane_change.param.yaml

@@ -37,7 +37,8 @@
       enable_cancel_lane_change: true
       enable_abort_lane_change: false
 
-      abort_delta_time: 3.0                      # [s]
+      abort_delta_time: 1.0                      # [s]
+      aborting_time: 5.0                         # [s]
       abort_max_lateral_jerk: 1000.0             # [m/s3]
 
       # debug

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

@@ -205,7 +205,7 @@ class GoalPlannerModule : public SceneModuleInterface
   bool needPathUpdate(const double path_update_duration) const;
   bool isStuck();
   bool hasDecidedPath() const;
-  void requestApproval();
+  void decideVelocity();
 
   // validation
   bool hasEnoughDistance(const PullOverPath & pull_over_path) const;