Merge pull request #656 from tier4/sync-upstream

chore: sync upstream

.github/workflows/build-and-test.yaml

@@ -27,6 +27,14 @@ jobs:
       - name: Check out repository
         uses: actions/checkout@v3
 
+      - name: Free disk space (Ubuntu)
+        uses: jlumbroso/free-disk-space@v1.2.0
+        with:
+          tool-cache: false
+          dotnet: false
+          swap-storage: false
+          large-packages: false
+
       - name: Remove exec_depend
         uses: autowarefoundation/autoware-github-actions/remove-exec-depend@v1
 

common/tier4_planning_rviz_plugin/src/tools/max_velocity.cpp

@@ -77,7 +77,7 @@ void MaxVelocityDisplay::onInitialize()
   overlay_->updateTextureSize(property_length_->getInt(), property_length_->getInt());
   overlay_->setPosition(property_left_->getInt(), property_top_->getInt());
   overlay_->setDimensions(overlay_->getTextureWidth(), overlay_->getTextureHeight());
-  processMessage(last_msg_ptr_);
+  updateVisualization();
 
   // QColor background_color;
   // background_color.setAlpha(0);

launch/tier4_planning_launch/launch/planning.launch.xml

@@ -4,7 +4,8 @@
   <arg name="vehicle_param_file"/>
   <arg name="use_pointcloud_container"/>
   <arg name="pointcloud_container_name"/>
-  <arg name="smoother_type"/>
+  <arg name="path_smoother_type"/>
+  <arg name="velocity_smoother_type"/>
   <arg name="vehicle_param_file"/>
   <arg name="use_pointcloud_container"/>
   <arg name="pointcloud_container_name"/>
@@ -43,7 +44,7 @@
   <arg name="obstacle_cruise_planner_param_path"/>
   <!-- motion velocity smoother -->
   <arg name="motion_velocity_smoother_param_path"/>
-  <arg name="smoother_type_param_path"/>
+  <arg name="velocity_smoother_type_param_path"/>
   <!-- planning validator -->
   <arg name="planning_validator_param_path"/>
 

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

@@ -21,6 +21,7 @@
 from launch.conditions import LaunchConfigurationEquals
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
+from launch.substitutions import PythonExpression
 from launch_ros.actions import ComposableNodeContainer
 from launch_ros.actions import LoadComposableNodes
 from launch_ros.descriptions import ComposableNode
@@ -41,6 +42,39 @@ def launch_setup(context, *args, **kwargs):
     with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f:
         nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"]
 
+    # path smoother
+    smoother_output_path_topic = "path_smoother/path"
+    with open(LaunchConfiguration("elastic_band_smoother_param_path").perform(context), "r") as f:
+        elastic_band_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+    elastic_band_smoother_component = ComposableNode(
+        package="path_smoother",
+        plugin="path_smoother::ElasticBandSmoother",
+        name="elastic_band_smoother",
+        namespace="",
+        remappings=[
+            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/odometry", "/localization/kinematic_state"),
+            ("~/output/path", smoother_output_path_topic),
+        ],
+        parameters=[
+            nearest_search_param,
+            elastic_band_smoother_param,
+        ],
+        extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
+    )
+
+    planner_input_path_topic = PythonExpression(
+        [
+            "'",
+            LaunchConfiguration("input_path_topic"),
+            "' if '",
+            LaunchConfiguration("path_smoother_type"),
+            "' == 'none'",
+            " else '",
+            smoother_output_path_topic,
+            "'",
+        ]
+    )
     # obstacle avoidance planner
     with open(
         LaunchConfiguration("obstacle_avoidance_planner_param_path").perform(context), "r"
@@ -52,7 +86,7 @@ def launch_setup(context, *args, **kwargs):
         name="obstacle_avoidance_planner",
         namespace="",
         remappings=[
-            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/path", planner_input_path_topic),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
@@ -73,7 +107,7 @@ def launch_setup(context, *args, **kwargs):
         name="path_sampler",
         namespace="",
         remappings=[
-            ("~/input/path", LaunchConfiguration("input_path_topic")),
+            ("~/input/path", planner_input_path_topic),
             ("~/input/odometry", "/localization/kinematic_state"),
             ("~/output/path", "obstacle_avoidance_planner/trajectory"),
         ],
@@ -251,6 +285,12 @@ def launch_setup(context, *args, **kwargs):
         condition=LaunchConfigurationEquals("cruise_planner_type", "none"),
     )
 
+    smoother_loader = LoadComposableNodes(
+        composable_node_descriptions=[elastic_band_smoother_component],
+        target_container=container,
+        condition=LaunchConfigurationEquals("path_smoother_type", "elastic_band"),
+    )
+
     obstacle_avoidance_planner_loader = LoadComposableNodes(
         composable_node_descriptions=[obstacle_avoidance_planner_component],
         target_container=container,
@@ -272,6 +312,7 @@ def launch_setup(context, *args, **kwargs):
     group = GroupAction(
         [
             container,
+            smoother_loader,
             obstacle_avoidance_planner_loader,
             path_sampler_loader,
             obstacle_stop_planner_loader,
@@ -309,6 +350,7 @@ def add_launch_arg(name: str, default_value=None, description=None):
     add_launch_arg(
         "path_planner_type", "obstacle_avoidance_planner"
     )  # select from "obstacle_avoidance_planner", "path_sampler"
+    add_launch_arg("path_smoother_type", "elastic_band")  # select from "elastic_band", "none"
 
     add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication")
     add_launch_arg("use_multithread", "false", "use multithread")

launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml

@@ -27,11 +27,11 @@
       <set_remap from="~/input/trajectory" to="/planning/scenario_planning/scenario_selector/trajectory"/>
       <set_remap from="~/output/trajectory" to="/planning/scenario_planning/motion_velocity_smoother/trajectory"/>
       <include file="$(find-pkg-share motion_velocity_smoother)/launch/motion_velocity_smoother.launch.xml">
-        <arg name="smoother_type" value="$(var smoother_type)"/>
+        <arg name="velocity_smoother_type" value="$(var velocity_smoother_type)"/>
         <arg name="common_param_path" value="$(var common_param_path)"/>
         <arg name="nearest_search_param_path" value="$(var nearest_search_param_path)"/>
         <arg name="param_path" value="$(var motion_velocity_smoother_param_path)"/>
-        <arg name="smoother_param_path" value="$(var smoother_type_param_path)"/>
+        <arg name="velocity_smoother_param_path" value="$(var velocity_smoother_type_param_path)"/>
       </include>
     </group>
   </group>

planning/behavior_path_planner/config/behavior_path_planner.param.yaml

@@ -1,10 +1,6 @@
 /**:
   ros__parameters:
     verbose: false
-
-    groot_zmq_publisher_port: 1666
-    groot_zmq_server_port: 1667
-
     planning_hz: 10.0
     backward_path_length: 5.0
     forward_path_length: 300.0