diff --git a/dummy/dummy_operation_mode_publisher/CMakeLists.txt b/dummy/dummy_operation_mode_publisher/CMakeLists.txt new file mode 100644 index 0000000000000..d682cd8198e4e --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(dummy_operation_mode_publisher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(dummy_operation_mode_publisher SHARED + src/dummy_operation_mode_publisher.cpp +) +ament_target_dependencies(dummy_operation_mode_publisher) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "dummy_operation_mode_publisher::DummyOperationModePublisher" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/dummy/dummy_operation_mode_publisher/README.md b/dummy/dummy_operation_mode_publisher/README.md new file mode 100644 index 0000000000000..e67018a331dc5 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/README.md @@ -0,0 +1,23 @@ +# Dummy Operation Mode Publisher + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml b/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml new file mode 100644 index 0000000000000..ba1525699940b --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/dummy/dummy_operation_mode_publisher/package.xml b/dummy/dummy_operation_mode_publisher/package.xml new file mode 100644 index 0000000000000..5cc8fc63c70e8 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/package.xml @@ -0,0 +1,25 @@ + + + + dummy_operation_mode_publisher + 0.1.0 + The dummy_operation_mode_publisher package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_adapi_v1_msgs + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp new file mode 100644 index 0000000000000..b8840e0d3ebb7 --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp @@ -0,0 +1,63 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#include "dummy_operation_mode_publisher.hpp" + +namespace dummy_operation_mode_publisher +{ + +DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptions & node_options) +: Node("dummy_operation_mode_publisher", node_options) +{ + // Parameter + + // Subscriber + + // Publisher + pub_operation_mode_state_ = create_publisher( + "~/output/operation_mode_state", 10); + + // Service + + // Client + + // Timer + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); + + // State + + // Diagnostics +} + +void DummyOperationModePublisher::onTimer() +{ + autoware_adapi_v1_msgs::msg::OperationModeState msg; + msg.stamp = this->now(); + msg.mode = autoware_adapi_v1_msgs::msg::OperationModeState::AUTONOMOUS; + msg.is_autonomous_mode_available = true; + msg.is_in_transition = false; + msg.is_stop_mode_available = true; + msg.is_autonomous_mode_available = true; + msg.is_local_mode_available = true; + msg.is_remote_mode_available = true; + + pub_operation_mode_state_->publish(msg); +} + +} // namespace dummy_operation_mode_publisher + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(dummy_operation_mode_publisher::DummyOperationModePublisher) diff --git a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp new file mode 100644 index 0000000000000..43ee5b756451b --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp @@ -0,0 +1,56 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#define DUMMY_OPERATION_MODE_PUBLISHER_HPP_ + +// include +#include + +#include + +namespace dummy_operation_mode_publisher +{ + +class DummyOperationModePublisher : public rclcpp::Node +{ +public: + explicit DummyOperationModePublisher(const rclcpp::NodeOptions & node_options); + ~DummyOperationModePublisher() = default; + +private: + // Parameter + + // Subscriber + + // Publisher + rclcpp::Publisher::SharedPtr + pub_operation_mode_state_; + + // Service + + // Client + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + + // State + + // Diagnostics +}; +} // namespace dummy_operation_mode_publisher + +#endif // DUMMY_OPERATION_MODE_PUBLISHER_HPP_ 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..fac0c77c5d8ee --- /dev/null +++ b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml @@ -0,0 +1,190 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 789cf136f1152..d9cb839307c96 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,7 +12,9 @@ autoware_cmake component_state_monitor + control_cmd_switcher emergency_handler + leader_election_converter system_error_monitor system_monitor diff --git a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml index 7f4a4711e85c3..ebbde691c32d4 100644 --- a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml +++ b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml @@ -3,6 +3,6 @@ - + diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..aebea9d7edad8 --- /dev/null +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -0,0 +1,25 @@ +cmake_minimum_required(VERSION 3.14) +project(control_cmd_switcher) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/control_cmd_switcher/control_cmd_switcher.cpp +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "ControlCmdSwitcher" + EXECUTABLE ${PROJECT_NAME}_node +) + +install(PROGRAMS + tool/rely_trajectory.py + DESTINATION lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ +) + +ament_auto_package(INSTALL_TO_SHARE + launch + config +) diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md new file mode 100644 index 0000000000000..7c688a50919a6 --- /dev/null +++ b/system/control_cmd_switcher/README.md @@ -0,0 +1 @@ +# control_cmd_switcher diff --git a/system/control_cmd_switcher/config/control_cmd_switcher.yaml b/system/control_cmd_switcher/config/control_cmd_switcher.yaml new file mode 100644 index 0000000000000..b908163301d69 --- /dev/null +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -0,0 +1,4 @@ +# Default configuration for mrm handler +--- +/**: + ros__parameters: diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml new file mode 100644 index 0000000000000..b35bc2d726bc7 --- /dev/null +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml new file mode 100644 index 0000000000000..8ed70f43f51f1 --- /dev/null +++ b/system/control_cmd_switcher/package.xml @@ -0,0 +1,25 @@ + + + + control_cmd_switcher + 0.1.0 + The control_cmd_switcher ROS 2 package + + Tetsuhiro Kawaguchi + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_auto_control_msgs + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp new file mode 100644 index 0000000000000..7aae994e3ab9d --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -0,0 +1,73 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#include "control_cmd_switcher.hpp" + +#include +#include +#include +#include + +ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) +: Node("control_cmd_switcher", node_options) +{ + // Subscriber + sub_main_control_cmd_ = + create_subscription( + "~/input/main/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onMainControlCmd, this, std::placeholders::_1)); + + sub_sub_control_cmd_ = + create_subscription( + "~/input/sub/control_cmd", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onSubControlCmd, this, std::placeholders::_1)); + + sub_election_status = create_subscription( + "~/input/election/status", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); + // Publisher + pub_control_cmd_ = create_publisher( + "~/output/control_cmd", rclcpp::QoS{1}); + + // Initialize + use_main_control_cmd_ = true; +} + +void ControlCmdSwitcher::onMainControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + if (use_main_control_cmd_) { + pub_control_cmd_->publish(*msg); + } +} + +void ControlCmdSwitcher::onSubControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + if (!use_main_control_cmd_) { + pub_control_cmd_->publish(*msg); + } +} + +void ControlCmdSwitcher::onElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) +{ + if (((msg->path_info >> 3) & 0x01) == 1) { + use_main_control_cmd_ = true; + } else if (((msg->path_info >> 2) & 0x01) == 1) { + use_main_control_cmd_ = false; + } +} + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(ControlCmdSwitcher) diff --git a/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp new file mode 100644 index 0000000000000..1f17b63eaadc3 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp @@ -0,0 +1,55 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#ifndef CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ + +// Core +#include +#include +#include + +// Autoware +#include +#include + +// ROS 2 core +#include + +class ControlCmdSwitcher : public rclcpp::Node +{ +public: + explicit ControlCmdSwitcher(const rclcpp::NodeOptions & node_options); + +private: + // Subscribers + rclcpp::Subscription::SharedPtr + sub_main_control_cmd_; + rclcpp::Subscription::SharedPtr + sub_sub_control_cmd_; + rclcpp::Subscription::SharedPtr sub_election_status; + void onMainControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onSubControlCmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg); + void onElectionStatus(const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr + pub_control_cmd_; + + std::atomic use_main_control_cmd_; +}; + +#endif // CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ diff --git a/system/control_cmd_switcher/tool/rely_trajectory.py b/system/control_cmd_switcher/tool/rely_trajectory.py new file mode 100755 index 0000000000000..ef28badafdf70 --- /dev/null +++ b/system/control_cmd_switcher/tool/rely_trajectory.py @@ -0,0 +1,50 @@ +#!/usr/bin/env python3 + +import threading + +from autoware_auto_planning_msgs.msg import Trajectory +import rclpy +from rclpy.node import Node + + +class RelayTrajectoryNode(Node): + def __init__(self): + super().__init__("relay_trajectory") + self.subscription = self.create_subscription( + Trajectory, "/tmp/planning/scenario_planning/trajectory", self.listener_callback, 10 + ) + self.publisher = self.create_publisher( + Trajectory, "/planning/scenario_planning/trajectory", 10 + ) + self.running = True + + def listener_callback(self, msg): + if self.running: + self.publisher.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + node = RelayTrajectoryNode() + + def input_thread(): + nonlocal node + while True: + user_input = input("Enter 'y' to stop publishing: ") + if user_input.lower() == "y": + node.running = False + print("Publishing stopped.") + break + + thread = threading.Thread(target=input_thread) + thread.start() + + rclpy.spin(node) + + thread.join() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp index 286151b90fdf5..9a0b798331e4a 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -25,12 +25,13 @@ // Autoware #include -#include #include #include #include #include +#include #include +#include #include #include @@ -117,9 +118,9 @@ class MrmHandler : public rclcpp::Node void publishHazardCmd(); void publishGearCmd(); - rclcpp::Publisher::SharedPtr pub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_mrm_state_; - autoware_adapi_v1_msgs::msg::MrmState mrm_state_; + tier4_system_msgs::msg::MrmState mrm_state_; void publishMrmState(); // parameter callback @@ -136,7 +137,7 @@ class MrmHandler : public rclcpp::Node rclcpp::Client::SharedPtr client_mrm_emergency_stop_; bool requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior, RequestType request_type) const; void logMrmCallingResult( const tier4_system_msgs::srv::OperateMrm::Response & result, const std::string & behavior, @@ -164,7 +165,7 @@ class MrmHandler : public rclcpp::Node void updateMrmState(); void operateMrm(); void handleFailedRequest(); - autoware_adapi_v1_msgs::msg::MrmState::_behavior_type getCurrentMrmBehavior(); + tier4_system_msgs::msg::MrmBehavior::_type_type getCurrentMrmBehavior(); bool isStopped(); bool isEmergency() const; bool isArrivedAtGoal(); diff --git a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp index 9d31c68f161b5..700a2f1125b5d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -68,7 +68,7 @@ MrmHandler::MrmHandler() : Node("mrm_handler") pub_gear_cmd_ = create_publisher("~/output/gear", rclcpp::QoS{1}); pub_mrm_state_ = - create_publisher("~/output/mrm/state", rclcpp::QoS{1}); + create_publisher("~/output/mrm/state", rclcpp::QoS{1}); // Clients client_mrm_pull_over_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); @@ -95,11 +95,11 @@ MrmHandler::MrmHandler() : Node("mrm_handler") mrm_emergency_stop_status_ = std::make_shared(); operation_mode_state_ = std::make_shared(); mrm_state_.stamp = this->now(); - mrm_state_.state = autoware_adapi_v1_msgs::msg::MrmState::NORMAL; - mrm_state_.behavior = autoware_adapi_v1_msgs::msg::MrmState::NONE; + mrm_state_.state = tier4_system_msgs::msg::MrmState::NORMAL; + mrm_state_.behavior.type = tier4_system_msgs::msg::MrmBehavior::NONE; is_operation_mode_availability_timeout = false; - // Timer + // Timera const auto update_period_ns = rclcpp::Rate(param_.update_rate).period(); timer_ = rclcpp::create_timer( this, get_clock(), update_period_ns, std::bind(&MrmHandler::onTimer, this)); @@ -227,16 +227,17 @@ void MrmHandler::publishMrmState() void MrmHandler::operateMrm() { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; if (mrm_state_.state == MrmState::NORMAL) { // Cancel MRM behavior when returning to NORMAL state - const auto current_mrm_behavior = MrmState::NONE; - if (current_mrm_behavior == mrm_state_.behavior) { + const auto current_mrm_behavior = MrmBehavior::NONE; + if (current_mrm_behavior == mrm_state_.behavior.type) { return; } - if (requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { - mrm_state_.behavior = current_mrm_behavior; + if (requestMrmBehavior(mrm_state_.behavior.type, RequestType::CANCEL)) { + mrm_state_.behavior.type = current_mrm_behavior; } else { handleFailedRequest(); } @@ -244,13 +245,13 @@ void MrmHandler::operateMrm() } if (mrm_state_.state == MrmState::MRM_OPERATING) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior == mrm_state_.behavior) { + if (current_mrm_behavior == mrm_state_.behavior.type) { return; } - if (!requestMrmBehavior(mrm_state_.behavior, RequestType::CANCEL)) { + if (!requestMrmBehavior(mrm_state_.behavior.type, RequestType::CANCEL)) { handleFailedRequest(); } else if (requestMrmBehavior(current_mrm_behavior, RequestType::CALL)) { - mrm_state_.behavior = current_mrm_behavior; + mrm_state_.behavior.type = current_mrm_behavior; } else { handleFailedRequest(); } @@ -270,21 +271,23 @@ void MrmHandler::operateMrm() void MrmHandler::handleFailedRequest() { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; - if (requestMrmBehavior(MrmState::EMERGENCY_STOP, CALL)) { + if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) { if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); } else { transitionTo(MrmState::MRM_FAILED); } - mrm_state_.behavior = MrmState::EMERGENCY_STOP; + mrm_state_.behavior.type = MrmBehavior::EMERGENCY_STOP; } bool MrmHandler::requestMrmBehavior( - const autoware_adapi_v1_msgs::msg::MrmState::_behavior_type & mrm_behavior, + const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior, RequestType request_type) const { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; auto request = std::make_shared(); if (request_type == RequestType::CALL) { @@ -300,16 +303,16 @@ bool MrmHandler::requestMrmBehavior( std::shared_future> future; const auto behavior2string = [](const int behavior) { - if (behavior == MrmState::NONE) { + if (behavior == MrmBehavior::NONE) { return "NONE"; } - if (behavior == MrmState::PULL_OVER) { + if (behavior == MrmBehavior::PULL_OVER) { return "PULL_OVER"; } - if (behavior == MrmState::COMFORTABLE_STOP) { + if (behavior == MrmBehavior::COMFORTABLE_STOP) { return "COMFORTABLE_STOP"; } - if (behavior == MrmState::EMERGENCY_STOP) { + if (behavior == MrmBehavior::EMERGENCY_STOP) { return "EMERGENCY_STOP"; } const auto msg = "invalid behavior: " + std::to_string(behavior); @@ -317,18 +320,18 @@ bool MrmHandler::requestMrmBehavior( }; switch (mrm_behavior) { - case MrmState::NONE: + case MrmBehavior::NONE: RCLCPP_WARN(this->get_logger(), "MRM behavior is None. Do nothing."); return true; - case MrmState::PULL_OVER: { + case MrmBehavior::PULL_OVER: { future = client_mrm_pull_over_->async_send_request(request).future.share(); break; } - case MrmState::COMFORTABLE_STOP: { + case MrmBehavior::COMFORTABLE_STOP: { future = client_mrm_comfortable_stop_->async_send_request(request).future.share(); break; } - case MrmState::EMERGENCY_STOP: { + case MrmBehavior::EMERGENCY_STOP: { future = client_mrm_emergency_stop_->async_send_request(request).future.share(); break; } @@ -433,7 +436,7 @@ void MrmHandler::onTimer() void MrmHandler::transitionTo(const int new_state) { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmState; const auto state2string = [](const int state) { if (state == MrmState::NORMAL) { @@ -462,8 +465,9 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { - using autoware_adapi_v1_msgs::msg::MrmState; using autoware_auto_vehicle_msgs::msg::ControlModeReport; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; // Check emergency const bool is_emergency = isEmergency(); @@ -491,7 +495,7 @@ void MrmHandler::updateMrmState() if (mrm_state_.state == MrmState::MRM_OPERATING) { // TODO(TetsuKawa): Check MRC is accomplished - if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) { if (isStopped() && isArrivedAtGoal()) { transitionTo(MrmState::MRM_SUCCEEDED); return; @@ -504,7 +508,7 @@ void MrmHandler::updateMrmState() } } else if (mrm_state_.state == MrmState::MRM_SUCCEEDED) { const auto current_mrm_behavior = getCurrentMrmBehavior(); - if (current_mrm_behavior != mrm_state_.behavior) { + if (current_mrm_behavior != mrm_state_.behavior.type) { transitionTo(MrmState::MRM_OPERATING); } } else if (mrm_state_.state == MrmState::MRM_FAILED) { @@ -516,85 +520,86 @@ void MrmHandler::updateMrmState() } } -autoware_adapi_v1_msgs::msg::MrmState::_behavior_type MrmHandler::getCurrentMrmBehavior() +tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior() { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::OperationModeAvailability; // State machine - if (mrm_state_.behavior == MrmState::NONE) { + if (mrm_state_.behavior.type == MrmBehavior::NONE) { if (is_operation_mode_availability_timeout) { - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { - return MrmState::PULL_OVER; + return MrmBehavior::PULL_OVER; } } if (operation_mode_availability_->comfortable_stop) { if (param_.use_comfortable_stop) { - return MrmState::COMFORTABLE_STOP; + return MrmBehavior::COMFORTABLE_STOP; } } if (!operation_mode_availability_->emergency_stop) { RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); } - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } - if (mrm_state_.behavior == MrmState::PULL_OVER) { + if (mrm_state_.behavior.type == MrmBehavior::PULL_OVER) { if (is_operation_mode_availability_timeout) { - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } if (operation_mode_availability_->pull_over) { if (param_.use_pull_over) { - return MrmState::PULL_OVER; + return MrmBehavior::PULL_OVER; } } if (operation_mode_availability_->comfortable_stop) { if (param_.use_comfortable_stop) { - return MrmState::COMFORTABLE_STOP; + return MrmBehavior::COMFORTABLE_STOP; } } if (!operation_mode_availability_->emergency_stop) { RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); } - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } - if (mrm_state_.behavior == MrmState::COMFORTABLE_STOP) { + if (mrm_state_.behavior.type == MrmBehavior::COMFORTABLE_STOP) { if (is_operation_mode_availability_timeout) { - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over && param_.use_pull_over_after_stopped) { - return MrmState::PULL_OVER; + return MrmBehavior::PULL_OVER; } } if (operation_mode_availability_->comfortable_stop) { if (param_.use_comfortable_stop) { - return MrmState::COMFORTABLE_STOP; + return MrmBehavior::COMFORTABLE_STOP; } } if (!operation_mode_availability_->emergency_stop) { RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); } - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } - if (mrm_state_.behavior == MrmState::EMERGENCY_STOP) { + if (mrm_state_.behavior.type == MrmBehavior::EMERGENCY_STOP) { if (is_operation_mode_availability_timeout) { - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } if (isStopped() && operation_mode_availability_->pull_over) { if (param_.use_pull_over && param_.use_pull_over_after_stopped) { - return MrmState::PULL_OVER; + return MrmBehavior::PULL_OVER; } } if (!operation_mode_availability_->emergency_stop) { RCLCPP_WARN(this->get_logger(), "no mrm operation available: operate emergency_stop"); } - return MrmState::EMERGENCY_STOP; + return MrmBehavior::EMERGENCY_STOP; } - return mrm_state_.behavior; + return mrm_state_.behavior.type; } bool MrmHandler::isStopped() diff --git a/system/mrm_stop_operator/CMakeLists.txt b/system/mrm_stop_operator/CMakeLists.txt new file mode 100644 index 0000000000000..f66488c659806 --- /dev/null +++ b/system/mrm_stop_operator/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(mrm_stop_operator) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(mrm_stop_operator SHARED + src/mrm_stop_operator.cpp +) +ament_target_dependencies(mrm_stop_operator) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "mrm_stop_operator::MrmStopOperator" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/system/mrm_stop_operator/README.md b/system/mrm_stop_operator/README.md new file mode 100644 index 0000000000000..4407161d510bd --- /dev/null +++ b/system/mrm_stop_operator/README.md @@ -0,0 +1,23 @@ +# Mrm Stop Operator + +## Purpose + +## Inner-workings / Algorithms + +## Inputs / Outputs + +### Input + +### Output + +## Parameters + +## Assumptions / Known limits + +## (Optional) Error detection and handling + +## (Optional) Performance characterization + +## (Optional) References/External links + +## (Optional) Future extensions / Unimplemented parts diff --git a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml new file mode 100644 index 0000000000000..7043b61596f76 --- /dev/null +++ b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml @@ -0,0 +1,5 @@ +/**: + ros__parameters: + min_acceleration: -4.0 # min acceleration for sub ecu mrm stop [m/s^2] + max_jerk: 5.0 # max jerk for sub ecu mrm stop [m/s^3] + min_jerk: -5.0 # min jerk for sub ecu mrm stop [m/s^3] diff --git a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml new file mode 100644 index 0000000000000..58305c509c70c --- /dev/null +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -0,0 +1,11 @@ + + + + + + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml new file mode 100644 index 0000000000000..f3822268496b0 --- /dev/null +++ b/system/mrm_stop_operator/package.xml @@ -0,0 +1,26 @@ + + + + mrm_stop_operator + 0.1.0 + The mrm_stop_operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + rclcpp + rclcpp_components + tier4_planning_msgs + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp new file mode 100644 index 0000000000000..7ffc8aa276326 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -0,0 +1,81 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#include "mrm_stop_operator.hpp" + +namespace mrm_stop_operator +{ + +MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) +: Node("mrm_stop_operator", node_options) +{ + // Parameter + params_.min_acceleration = declare_parameter("min_acceleration", -4.0); + params_.max_jerk = declare_parameter("max_jerk", 5.0); + params_.min_jerk = declare_parameter("min_jerk", -5.0); + + // Subscriber + sub_mrm_request_ = create_subscription( + "~/input/mrm_request", 10, + std::bind(&MrmStopOperator::onMrmRequest, this, std::placeholders::_1)); + + // Publisher + pub_velocity_limit_ = create_publisher( + "~/output/velocity_limit", rclcpp::QoS{10}.transient_local()); + pub_velocity_limit_clear_command_ = + create_publisher( + "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local()); + + // Timer + + // Service + + // Client + + // Timer + + // State + initState(); + + // Diagnostics +} + +void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg) +{ + if ( + msg->type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP && + last_mrm_request_.type != tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) { + tier4_planning_msgs::msg::VelocityLimit velocity_limit; + velocity_limit.stamp = this->now(); + velocity_limit.max_velocity = 0.0; + velocity_limit.use_constraints = true; + velocity_limit.constraints.min_acceleration = params_.min_acceleration; + velocity_limit.constraints.max_jerk = params_.max_jerk; + velocity_limit.constraints.min_jerk = params_.min_jerk; + velocity_limit.sender = "mrm_stop_operator"; + pub_velocity_limit_->publish(velocity_limit); + + last_mrm_request_ = *msg; + } +} + +void MrmStopOperator::initState() +{ + last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE; +} + +} // namespace mrm_stop_operator + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(mrm_stop_operator::MrmStopOperator) diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp new file mode 100644 index 0000000000000..a44b2ed4f1988 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -0,0 +1,72 @@ +// Copyright 2024 TIER IV, Inc. +// +// 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. + +#ifndef MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR_HPP_ + +// include +#include + +#include +#include +#include + +namespace mrm_stop_operator +{ + +struct Parameters +{ + double min_acceleration; + double max_jerk; + double min_jerk; +}; + +class MrmStopOperator : public rclcpp::Node +{ +public: + explicit MrmStopOperator(const rclcpp::NodeOptions & node_options); + ~MrmStopOperator() = default; + +private: + // Parameter + Parameters params_; + + // Subscriber + rclcpp::Subscription::SharedPtr sub_mrm_request_; + + void onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::ConstSharedPtr msg); + + // Service + + // Publisher + rclcpp::Publisher::SharedPtr pub_velocity_limit_; + rclcpp::Publisher::SharedPtr + pub_velocity_limit_clear_command_; + + // Service + + // Client + + // Timer + + // State + tier4_system_msgs::msg::MrmBehavior last_mrm_request_; + + void initState(); + + // Diagnostics +}; +} // namespace mrm_stop_operator + +#endif // MRM_STOP_OPERATOR_HPP_