feat(behavior_velocity): add external velocity limit to behavior_velo…

…city (#238)

* feat(behavior_velocity): add external velocity limit to behavior_velocity

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* feat(behavior_velocity): add motion velocity smoother param to behavior velocity

Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp>

* fix(planning_launch): fix common param

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp>

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py

@@ -178,6 +178,39 @@ def generate_launch_description():
         extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}],
     )
 
+    # smoother param
+    common_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "common.param.yaml",
+    )
+    with open(common_param_path, "r") as f:
+        common_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    base_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "motion_velocity_smoother",
+        "motion_velocity_smoother.param.yaml",
+    )
+    with open(base_param_path, "r") as f:
+        base_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
+    smoother_param_path = os.path.join(
+        get_package_share_directory("planning_launch"),
+        "config",
+        "scenario_planning",
+        "common",
+        "motion_velocity_smoother",
+        "Analytical.param.yaml",
+    )
+    with open(smoother_param_path, "r") as f:
+        smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"]
+
     # behavior velocity planner
     blind_spot_param_path = os.path.join(
         get_package_share_directory("behavior_velocity_planner"),
@@ -273,6 +306,10 @@ def generate_launch_description():
                 "~/input/external_traffic_signals",
                 "/external/traffic_light_recognition/traffic_signals",
             ),
+            (
+                "~/input/external_velocity_limit_mps",
+                "/planning/scenario_planning/max_velocity_default",
+            ),
             ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"),
             (
                 "~/input/occupancy_grid",
@@ -302,6 +339,9 @@ def generate_launch_description():
                 "max_accel": -2.8,
                 "delay_response_time": 1.3,
             },
+            common_param,
+            base_param,
+            smoother_param,
             blind_spot_param,
             crosswalk_param,
             detection_area_param,