sync rc develop (#133)

* FIx backward fixing distance

* Disable lane departure temporarily

* Disable lidar temporarily

* Enable multi thread in control container (#126)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Feature/change acc param (#131)

* change acc param

* add space

* Enable avoidance without foa (#127)

* Enable avoidance without foa

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Update planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>

* Revert "Disable lidar temporarily"

This reverts commit 81359d412292b5396cef7aceb53909cb38cf1d95.

Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Fumiya Watanabe <rej55.g@gmail.com>
Co-authored-by: j4tfwm6z <proj-jpntaxi@tier4.jp>

autoware_launch/launch/autoware.launch.xml

@@ -60,6 +60,7 @@
   <include file="$(find-pkg-share control_launch)/launch/control.launch.py">
     <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
     <arg name="lateral_controller_mode" value="mpc_follower" />
+    <arg name="use_multithread" value="true" />
   </include>
 
   <!-- Autoware API -->

autoware_launch/launch/logging_simulator.launch.xml

@@ -88,6 +88,7 @@
     <include file="$(find-pkg-share control_launch)/launch/control.launch.py" if="$(var control)">
       <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
       <arg name="lateral_controller_mode" value="mpc_follower" />
+      <arg name="use_multithread" value="true" />
     </include>
   </group>
 

autoware_launch/launch/planning_simulator.launch.xml

@@ -63,6 +63,7 @@
   <include file="$(find-pkg-share control_launch)/launch/control.launch.py">
     <!-- options for lateral_controller_mode: mpc_follower, pure_pursuit -->
     <arg name="lateral_controller_mode" value="mpc_follower" />
+    <arg name="use_multithread" value="true" />
   </include>
 
   <!-- Autoware API -->

planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml

@@ -2,7 +2,7 @@
 /**:
   ros__parameters:
     avoidance:
-      threshold_distance_object_is_on_center: 0.0 # [m]
+      # threshold_distance_object_is_on_center: 0.0 # [m]
       threshold_speed_object_is_stopped: 1.0      # [m/s]
       object_check_forward_distance: 150.0        # [m]
       object_check_backward_distance: 2.0         # [m]

planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml

@@ -3,7 +3,7 @@
     # trajectory total/fixing length
     trajectory_length: 300 # total trajectory length[m]
     forward_fixing_distance: 5.0 # forward fixing length from base_link[m]
-    backward_fixing_distance: 3.0 # backward fixing length from base_link[m]
+    backward_fixing_distance: 10.0 # backward fixing length from base_link[m]
 
     # clearance(distance) when generating trajectory
     clearance_from_road: 0.2 # clearance from road boundary[m]

planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml

@@ -11,19 +11,19 @@
       obstacle_velocity_thresh_to_stop_acc: 3.0 # stop adaptive cruise control when the velocity of the forward obstacle falls below this value [m/s]
       emergency_stop_acceleration: -4.5 # supposed minimum acceleration (deceleration) in emergency stop [m/ss]
       emergency_stop_idling_time: 0.5 # supposed idling time to start emergency stop [s]
-      min_dist_stop: 4.0 # minimum distance of emergency stop [m]
+      min_dist_stop: 6.0 # minimum distance of emergency stop [m]
       max_standard_acceleration: 1.0 # supposed maximum acceleration in active cruise control [m/ss]
       min_standard_acceleration: -0.7 # supposed minimum acceleration (deceleration) in active cruise control
       standard_idling_time: 0.5 # supposed idling time to react object in active cruise control [s]
-      min_dist_standard: 4.0 # minimum distance in active cruise control [m]
-      obstacle_min_standard_acceleration: -1.5 # supposed minimum acceleration of forward obstacle [m/ss]
-      margin_rate_to_change_vel: 0.9 # margin to insert upper velocity [-]
+      min_dist_standard: 6.0 # minimum distance in active cruise control [m]
+      obstacle_min_standard_acceleration: -2.0 # supposed minimum acceleration of forward obstacle [m/ss]
+      margin_rate_to_change_vel: 0.7 # margin to insert upper velocity [-]
 
       # pid parameter for ACC
       p_coefficient_positive: 0.25 # coefficient P in PID control (used when target dist -current_dist >=0) [-]
       p_coefficient_negative: 0.3 # coefficient P in PID control (used when target dist -current_dist <0) [-]
       d_coefficient_positive: 0.15 # coefficient D in PID control (used when delta_dist >=0) [-]
-      d_coefficient_negative: 0.2 # coefficient D in PID control (used when delta_dist <0) [-]
+      d_coefficient_negative: 0.0 # coefficient D in PID control (used when delta_dist <0) [-]
 
       # parameter for object velocity estimation
       object_polygon_length_margin: 3.0 # The distance to extend the polygon length the object in pointcloud-object matching [m]

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

@@ -23,6 +23,7 @@
 from launch.conditions import UnlessCondition
 from launch.substitutions import LaunchConfiguration
 from launch_ros.actions import ComposableNodeContainer
+from launch_ros.actions import SetParameter
 from launch_ros.descriptions import ComposableNode
 from launch_ros.substitutions import FindPackageShare
 import yaml
@@ -122,9 +123,7 @@ def generate_launch_description():
                 LaunchConfiguration('disuse_foa'),
                 'lane_change.use_all_predicted_path': LaunchConfiguration('disuse_foa'),
                 'lane_change.enable_blocked_by_obstacle': LaunchConfiguration('disuse_foa'),
-                'bt_tree_config_path':
-                [FindPackageShare('behavior_path_planner'),
-                 '/config/behavior_path_planner_tree_lane_change_only.xml']
+                'bt_tree_config_path': LaunchConfiguration('bt_tree_config_path')
             }
         ],
         extra_arguments=[
@@ -274,6 +273,20 @@ def generate_launch_description():
         condition=IfCondition(LaunchConfiguration('use_multithread')),
     )
 
+    set_bt_tree_config_path_without_foa = SetLaunchConfiguration(
+        'bt_tree_config_path',
+        [FindPackageShare('behavior_path_planner'),
+         '/config/behavior_path_planner_tree.xml'],
+        condition=IfCondition(LaunchConfiguration('disuse_foa')),
+    )
+
+    set_bt_tree_config_path_with_foa = SetLaunchConfiguration(
+        'bt_tree_config_path',
+        [FindPackageShare('behavior_path_planner'),
+         '/config/behavior_path_planner_tree_lane_change_only.xml'],
+        condition=UnlessCondition(LaunchConfiguration('disuse_foa')),
+    )
+
     return launch.LaunchDescription([
         DeclareLaunchArgument(
             'input_route_topic_name',
@@ -292,6 +305,18 @@ def generate_launch_description():
         ),
         set_container_executable,
         set_container_mt_executable,
+        SetParameter(
+            name='avoidance.threshold_distance_object_is_on_center',
+            value=1.0,
+            condition=IfCondition(LaunchConfiguration('disuse_foa'))
+        ),
+        SetParameter(
+            name='avoidance.threshold_distance_object_is_on_center',
+            value=0.0,
+            condition=UnlessCondition(LaunchConfiguration('disuse_foa'))
+        ),
+        set_bt_tree_config_path_without_foa,
+        set_bt_tree_config_path_with_foa,
         container,
         ExecuteProcess(
             cmd=['ros2', 'topic', 'pub',

planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml

@@ -7,6 +7,9 @@
                     autoware_planning_msgs/msg/LaneChangeCommand 'command: true' -r 10" />
   </group>
 
+  <set_parameter name="avoidance.threshold_distance_object_is_on_center" value="0.5" if="$(var disuse_foa)" />
+  <set_parameter name="avoidance.threshold_distance_object_is_on_center" value="0.0" unless="$(var disuse_foa)" />
+
   <node pkg="behavior_path_planner" exec="behavior_path_planner" name="behavior_path_planner" output="screen">
     <remap from="~/input/route" to="$(var input_route_topic_name)" />
     <remap from="~/input/vector_map" to="$(var map_topic_name)" />

system_launch/config/autoware_error_monitor.param.yaml

@@ -17,7 +17,7 @@
       autonomous_driving:
         names: [
             "/autoware/control/autonomous_driving/alive_monitoring",
-            "/autoware/control/autonomous_driving/lane_departure",
+            # "/autoware/control/autonomous_driving/lane_departure",
             # '/autoware/control/autonomous_driving/trajectory_deviation', # Unstable
             "/autoware/control/command_gate/alive_monitoring",
             "/autoware/localization/alive_monitoring",