From 36d37c749986025ff3dce8d021cb929ae7178092 Mon Sep 17 00:00:00 2001 From: "autoware-iv-sync-ci[bot]" <87871706+autoware-iv-sync-ci[bot]@users.noreply.github.com> Date: Wed, 15 Sep 2021 18:35:20 +0900 Subject: [PATCH] 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 * Feature/change acc param (#131) * change acc param * add space * Enable avoidance without foa (#127) * Enable avoidance without foa Signed-off-by: wep21 * Update planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml Co-authored-by: Fumiya Watanabe Co-authored-by: Fumiya Watanabe * Revert "Disable lidar temporarily" This reverts commit 81359d412292b5396cef7aceb53909cb38cf1d95. Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: tkimura4 Co-authored-by: Fumiya Watanabe Co-authored-by: j4tfwm6z --- autoware_launch/launch/autoware.launch.xml | 1 + .../launch/logging_simulator.launch.xml | 1 + .../launch/planning_simulator.launch.xml | 1 + .../avoidance/avoidance.param.yaml | 2 +- .../obstacle_avoidance_planner.param.yaml | 2 +- .../adaptive_cruise_control.param.yaml | 10 +++--- .../behavior_planning.launch.py | 31 +++++++++++++++++-- .../behavior_planning.launch.xml | 3 ++ .../config/autoware_error_monitor.param.yaml | 2 +- 9 files changed, 42 insertions(+), 11 deletions(-) diff --git a/autoware_launch/launch/autoware.launch.xml b/autoware_launch/launch/autoware.launch.xml index 4eba82948..e69620620 100644 --- a/autoware_launch/launch/autoware.launch.xml +++ b/autoware_launch/launch/autoware.launch.xml @@ -60,6 +60,7 @@ + diff --git a/autoware_launch/launch/logging_simulator.launch.xml b/autoware_launch/launch/logging_simulator.launch.xml index bbb669435..0e3c36746 100644 --- a/autoware_launch/launch/logging_simulator.launch.xml +++ b/autoware_launch/launch/logging_simulator.launch.xml @@ -88,6 +88,7 @@ + diff --git a/autoware_launch/launch/planning_simulator.launch.xml b/autoware_launch/launch/planning_simulator.launch.xml index b8377dbde..1fe253ccc 100644 --- a/autoware_launch/launch/planning_simulator.launch.xml +++ b/autoware_launch/launch/planning_simulator.launch.xml @@ -63,6 +63,7 @@ + diff --git a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml b/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml index 7b5e6b96b..e8000aea3 100644 --- a/planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/avoidance/avoidance.param.yaml +++ b/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] diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml index 7a44aa592..d6b3eaf08 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/obstacle_avoidance_planner.param.yaml +++ b/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] diff --git a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml b/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml index 4aedc646c..654e291cd 100644 --- a/planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_stop_planner/adaptive_cruise_control.param.yaml +++ b/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] diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 578129bd3..f65684286 100644 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/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', diff --git a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml b/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml index 5e6265106..5b8b86861 100644 --- a/planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.xml +++ b/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" /> + + + diff --git a/system_launch/config/autoware_error_monitor.param.yaml b/system_launch/config/autoware_error_monitor.param.yaml index 9a2da14f9..ded3a1407 100644 --- a/system_launch/config/autoware_error_monitor.param.yaml +++ b/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",