diff --git a/launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py b/launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py new file mode 100644 index 0000000000000..47c27244d9cb7 --- /dev/null +++ b/launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py @@ -0,0 +1,431 @@ +# Copyright 2020 Tier IV, Inc. All rights reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +import launch +from launch.actions import DeclareLaunchArgument +from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription +from launch.actions import OpaqueFunction +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.actions import PushRosNamespace +from launch_ros.descriptions import ComposableNode +from launch_ros.substitutions import FindPackageShare +import yaml + + +def launch_setup(context, *args, **kwargs): + with open(LaunchConfiguration("vehicle_param_file").perform(context), "r") as f: + vehicle_info_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + with open(LaunchConfiguration("nearest_search_param_path").perform(context), "r") as f: + nearest_search_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + with open( + LaunchConfiguration("trajectory_follower_node_param_path").perform(context), "r" + ) as f: + trajectory_follower_node_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lat_controller_param_path").perform(context), "r") as f: + lat_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lon_controller_param_path").perform(context), "r") as f: + lon_controller_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("vehicle_cmd_gate_param_path").perform(context), "r") as f: + vehicle_cmd_gate_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("lane_departure_checker_param_path").perform(context), "r") as f: + lane_departure_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("control_validator_param_path").perform(context), "r") as f: + control_validator_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open( + LaunchConfiguration("operation_mode_transition_manager_param_path").perform(context), "r" + ) as f: + operation_mode_transition_manager_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("shift_decider_param_path").perform(context), "r") as f: + shift_decider_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open( + LaunchConfiguration("obstacle_collision_checker_param_path").perform(context), "r" + ) as f: + obstacle_collision_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("aeb_param_path").perform(context), "r") as f: + aeb_param = yaml.safe_load(f)["/**"]["ros__parameters"] + with open(LaunchConfiguration("predicted_path_checker_param_path").perform(context), "r") as f: + predicted_path_checker_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + controller_component = ComposableNode( + package="trajectory_follower_node", + plugin="autoware::motion::control::trajectory_follower_node::Controller", + name="controller_node_exe", + namespace="trajectory_follower", + remappings=[ + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_odometry", "/localization/kinematic_state"), + ("~/input/current_steering", "/vehicle/status/steering_status"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/current_operation_mode", "/system/operation_mode/state"), + ("~/output/predicted_trajectory", "lateral/predicted_trajectory"), + ("~/output/lateral_diagnostic", "lateral/diagnostic"), + ("~/output/slope_angle", "longitudinal/slope_angle"), + ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"), + ("~/output/control_cmd", "control_cmd"), + ], + parameters=[ + { + "lateral_controller_mode": LaunchConfiguration("lateral_controller_mode"), + "longitudinal_controller_mode": LaunchConfiguration("longitudinal_controller_mode"), + }, + nearest_search_param, + trajectory_follower_node_param, + lon_controller_param, + lat_controller_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # lane departure checker + lane_departure_component = ComposableNode( + package="lane_departure_checker", + plugin="lane_departure_checker::LaneDepartureCheckerNode", + name="lane_departure_checker_node", + namespace="trajectory_follower", + remappings=[ + ("~/input/odometry", "/localization/kinematic_state"), + ("~/input/lanelet_map_bin", "/map/vector_map"), + ("~/input/route", "/planning/mission_planning/route"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[nearest_search_param, lane_departure_checker_param, vehicle_info_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + # control validator checker + control_validator_component = ComposableNode( + package="control_validator", + plugin="control_validator::ControlValidator", + name="control_validator", + remappings=[ + ("~/input/kinematics", "/localization/kinematic_state"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ("~/output/validation_status", "~/validation_status"), + ], + parameters=[control_validator_param], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # shift decider + shift_decider_component = ComposableNode( + package="shift_decider", + plugin="ShiftDecider", + name="shift_decider", + remappings=[ + ("input/control_cmd", "/control/trajectory_follower/control_cmd"), + ("input/state", "/autoware/state"), + ("input/current_gear", "/vehicle/status/gear_status"), + ("output/gear_cmd", "/control/shift_decider/gear_cmd"), + ], + parameters=[ + shift_decider_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # autonomous emergency braking + autonomous_emergency_braking = ComposableNode( + package="autonomous_emergency_braking", + plugin="autoware::motion::control::autonomous_emergency_braking::AEB", + name="autonomous_emergency_braking", + remappings=[ + ("~/input/pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("~/input/velocity", "/vehicle/status/velocity_status"), + ("~/input/imu", "/sensing/imu/imu_data"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + aeb_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + autonomous_emergency_braking_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_autonomous_emergency_braking")), + composable_node_descriptions=[autonomous_emergency_braking], + target_container="/control/control_container", + ) + + # autonomous emergency braking + predicted_path_checker = ComposableNode( + package="predicted_path_checker", + plugin="autoware::motion::control::predicted_path_checker::PredictedPathCheckerNode", + name="predicted_path_checker", + remappings=[ + ("~/input/objects", "/perception/object_recognition/objects"), + ("~/input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ("~/input/current_accel", "/localization/acceleration"), + ("~/input/odometry", "/localization/kinematic_state"), + ( + "~/input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ], + parameters=[ + vehicle_info_param, + predicted_path_checker_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + predicted_path_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_predicted_path_checker")), + composable_node_descriptions=[predicted_path_checker], + target_container="/control/control_container", + ) + + # vehicle cmd gate + vehicle_cmd_gate_component = ComposableNode( + package="vehicle_cmd_gate", + plugin="vehicle_cmd_gate::VehicleCmdGate", + name="vehicle_cmd_gate", + remappings=[ + ("input/steering", "/vehicle/status/steering_status"), + ("input/operation_mode", "/system/operation_mode/state"), + ("input/auto/control_cmd", "/control/trajectory_follower/control_cmd"), + ("input/auto/turn_indicators_cmd", "/planning/turn_indicators_cmd"), + ("input/auto/hazard_lights_cmd", "/planning/hazard_lights_cmd"), + ("input/auto/gear_cmd", "/control/shift_decider/gear_cmd"), + ("input/external/control_cmd", "/external/selected/control_cmd"), + ("input/external/turn_indicators_cmd", "/external/selected/turn_indicators_cmd"), + ("input/external/hazard_lights_cmd", "/external/selected/hazard_lights_cmd"), + ("input/external/gear_cmd", "/external/selected/gear_cmd"), + ("input/external_emergency_stop_heartbeat", "/external/selected/heartbeat"), + ("input/gate_mode", "/control/gate_mode_cmd"), + ("input/emergency/control_cmd", "/system/emergency/control_cmd"), + ("input/emergency/hazard_lights_cmd", "/system/emergency/hazard_lights_cmd"), + ("input/emergency/gear_cmd", "/system/emergency/gear_cmd"), + ("input/mrm_state", "/system/fail_safe/mrm_state"), + ("input/kinematics", "/localization/kinematic_state"), + ("input/acceleration", "/localization/acceleration"), + ("output/vehicle_cmd_emergency", "/control/command/emergency_cmd"), + ("output/control_cmd", "/main/control/command/control_cmd"), + ("output/gear_cmd", "/control/command/gear_cmd"), + ("output/turn_indicators_cmd", "/control/command/turn_indicators_cmd"), + ("output/hazard_lights_cmd", "/control/command/hazard_lights_cmd"), + ("output/gate_mode", "/control/current_gate_mode"), + ("output/engage", "/api/autoware/get/engage"), + ("output/external_emergency", "/api/autoware/get/emergency"), + ("output/operation_mode", "/control/vehicle_cmd_gate/operation_mode"), + ("~/service/engage", "/api/autoware/set/engage"), + ("~/service/external_emergency", "/api/autoware/set/emergency"), + # TODO(Takagi, Isamu): deprecated + ("input/engage", "/autoware/engage"), + ("~/service/external_emergency_stop", "~/external_emergency_stop"), + ("~/service/clear_external_emergency_stop", "~/clear_external_emergency_stop"), + ], + parameters=[ + vehicle_cmd_gate_param, + vehicle_info_param, + { + "check_external_emergency_heartbeat": LaunchConfiguration( + "check_external_emergency_heartbeat" + ), + }, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + # operation mode transition manager + operation_mode_transition_manager_component = ComposableNode( + package="operation_mode_transition_manager", + plugin="operation_mode_transition_manager::OperationModeTransitionManager", + name="operation_mode_transition_manager", + remappings=[ + # input + ("kinematics", "/localization/kinematic_state"), + ("steering", "/vehicle/status/steering_status"), + ("trajectory", "/planning/scenario_planning/trajectory"), + ("control_cmd", "/control/command/control_cmd"), + ("trajectory_follower_control_cmd", "/control/trajectory_follower/control_cmd"), + ("control_mode_report", "/vehicle/status/control_mode"), + ("gate_operation_mode", "/control/vehicle_cmd_gate/operation_mode"), + # output + ("is_autonomous_available", "/control/is_autonomous_available"), + ("control_mode_request", "/control/control_mode_request"), + ], + parameters=[ + nearest_search_param, + operation_mode_transition_manager_param, + vehicle_info_param, + ], + ) + + # external cmd selector + external_cmd_selector_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_selector"), "/launch/external_cmd_selector.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ( + "external_cmd_selector_param_path", + LaunchConfiguration("external_cmd_selector_param_path"), + ), + ], + ) + + # external cmd converter + external_cmd_converter_loader = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [FindPackageShare("external_cmd_converter"), "/launch/external_cmd_converter.launch.py"] + ), + launch_arguments=[ + ("use_intra_process", LaunchConfiguration("use_intra_process")), + ("target_container", "/control/control_container"), + ], + ) + + # obstacle collision checker + obstacle_collision_checker_component = ComposableNode( + package="obstacle_collision_checker", + plugin="obstacle_collision_checker::ObstacleCollisionCheckerNode", + name="obstacle_collision_checker", + remappings=[ + ("input/lanelet_map_bin", "/map/vector_map"), + ("input/obstacle_pointcloud", "/perception/obstacle_segmentation/pointcloud"), + ("input/reference_trajectory", "/planning/scenario_planning/trajectory"), + ( + "input/predicted_trajectory", + "/control/trajectory_follower/lateral/predicted_trajectory", + ), + ("input/odometry", "/localization/kinematic_state"), + ], + parameters=[ + obstacle_collision_checker_param, + vehicle_info_param, + ], + extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], + ) + + obstacle_collision_checker_loader = LoadComposableNodes( + condition=IfCondition(LaunchConfiguration("enable_obstacle_collision_checker")), + composable_node_descriptions=[obstacle_collision_checker_component], + target_container="/control/control_container", + ) + + glog_component = ComposableNode( + package="glog_component", + plugin="GlogComponent", + name="glog_component", + ) + + # set container to run all required components in the same process + container = ComposableNodeContainer( + name="control_container", + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=[ + controller_component, + control_validator_component, + lane_departure_component, + shift_decider_component, + vehicle_cmd_gate_component, + operation_mode_transition_manager_component, + glog_component, + ], + ) + + group = GroupAction( + [ + PushRosNamespace("control"), + container, + external_cmd_selector_loader, + external_cmd_converter_loader, + obstacle_collision_checker_loader, + autonomous_emergency_braking_loader, + predicted_path_checker_loader, + ] + ) + + return [group] + + +def generate_launch_description(): + launch_arguments = [] + + def add_launch_arg(name: str, default_value=None, description=None): + launch_arguments.append( + DeclareLaunchArgument(name, default_value=default_value, description=description) + ) + + # option + add_launch_arg("vehicle_param_file") + add_launch_arg("vehicle_id") + add_launch_arg("enable_obstacle_collision_checker") + add_launch_arg("lateral_controller_mode") + add_launch_arg("longitudinal_controller_mode") + # common param path + add_launch_arg("nearest_search_param_path") + # package param path + add_launch_arg("trajectory_follower_node_param_path") + add_launch_arg("lat_controller_param_path") + add_launch_arg("lon_controller_param_path") + add_launch_arg("vehicle_cmd_gate_param_path") + add_launch_arg("lane_departure_checker_param_path") + add_launch_arg("control_validator_param_path") + add_launch_arg("operation_mode_transition_manager_param_path") + add_launch_arg("shift_decider_param_path") + add_launch_arg("obstacle_collision_checker_param_path") + add_launch_arg("external_cmd_selector_param_path") + add_launch_arg("aeb_param_path") + add_launch_arg("predicted_path_checker_param_path") + add_launch_arg("enable_predicted_path_checker") + add_launch_arg("enable_autonomous_emergency_braking") + add_launch_arg("check_external_emergency_heartbeat") + + # component + add_launch_arg("use_intra_process", "false", "use ROS 2 component container communication") + add_launch_arg("use_multithread", "true", "use multithread") + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return launch.LaunchDescription( + launch_arguments + + [ + set_container_executable, + set_container_mt_executable, + ] + + [OpaqueFunction(function=launch_setup)] + ) diff --git a/launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml b/launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml new file mode 100644 index 0000000000000..120c55f0cae8d --- /dev/null +++ b/launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml new file mode 100644 index 0000000000000..fbd38dccd80ec --- /dev/null +++ b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml @@ -0,0 +1,192 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 789cf136f1152..42c57a7b4a22b 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -15,6 +15,8 @@ emergency_handler system_error_monitor system_monitor + control_cmd_switcher + leader_election_converter ament_lint_auto autoware_lint_common