From da201e44257b4cbeed52d23169f818d09ffd0d5e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Thu, 27 Jun 2024 21:02:55 +0900 Subject: [PATCH 01/13] feat: add control_cmd_switcher Signed-off-by: TetsuKawa --- system/control_cmd_switcher/CMakeLists.txt | 19 ++++++ system/control_cmd_switcher/README.md | 1 + .../config/control_cmd_switcher.yaml | 5 ++ .../launch/control_cmd_switcher.launch.xml | 17 +++++ system/control_cmd_switcher/package.xml | 25 +++++++ .../control_cmd_switcher.cpp | 65 +++++++++++++++++++ .../control_cmd_switcher.hpp | 55 ++++++++++++++++ 7 files changed, 187 insertions(+) create mode 100644 system/control_cmd_switcher/CMakeLists.txt create mode 100644 system/control_cmd_switcher/README.md create mode 100644 system/control_cmd_switcher/config/control_cmd_switcher.yaml create mode 100644 system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml create mode 100644 system/control_cmd_switcher/package.xml create mode 100644 system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp create mode 100644 system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.hpp diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..42ec827f855cc --- /dev/null +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -0,0 +1,19 @@ +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 +) + +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..f1e018734e2d1 --- /dev/null +++ b/system/control_cmd_switcher/README.md @@ -0,0 +1 @@ +# control_cmd_switcher \ No newline at end of file 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..e083b38a5ebe9 --- /dev/null +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -0,0 +1,5 @@ +# 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..9717a57970a3e --- /dev/null +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml new file mode 100644 index 0000000000000..7e0fa2bf2e97b --- /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 + tier4_system_msgs + rclcpp_components + + 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..b038264a7af99 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.cpp @@ -0,0 +1,65 @@ +// 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..1ebdf0b51551d --- /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_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_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_SWITCHER__CONTROL_SWITCHER_HPP_ From bfe14ff4755129b88be067f663a5973ac3b4e31a Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 01:03:33 +0900 Subject: [PATCH 02/13] feat: add some launch for one host test Signed-off-by: TetsuKawa --- .../control.main.mrm-v0.6.tmp.launch.py | 431 ++++++++++++++++++ .../system.main.mrm-v0.6.tmp.launch.xml | 187 ++++++++ launch/tier4_system_launch/package.xml | 1 + 3 files changed, 619 insertions(+) create mode 100644 launch/tier4_control_launch/launch/control.main.mrm-v0.6.tmp.launch.py create mode 100644 launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml 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_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..32d93bc92511f --- /dev/null +++ b/launch/tier4_system_launch/launch/system.main.mrm-v0.6.tmp.launch.xml @@ -0,0 +1,187 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 789cf136f1152..42a7fce7a7e2a 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -15,6 +15,7 @@ emergency_handler system_error_monitor system_monitor + control_cmd_switcher ament_lint_auto autoware_lint_common From b0adbf87c4df651b265bf20e828e4b6f969fb581 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 01:05:49 +0900 Subject: [PATCH 03/13] modify: launch file mistakes Signed-off-by: TetsuKawa --- .../launch/control_cmd_switcher.launch.xml | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml index 9717a57970a3e..b35bc2d726bc7 100644 --- a/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -7,11 +7,10 @@ - + - From df86641e1ec2a6cf609cdca21eb841d092fae51e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 01:12:54 +0900 Subject: [PATCH 04/13] feat: add leader_election_converter to launch Signed-off-by: TetsuKawa --- .../launch/system.main.mrm-v0.6.tmp.launch.xml | 5 +++++ launch/tier4_system_launch/package.xml | 1 + 2 files changed, 6 insertions(+) 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 index 32d93bc92511f..fbd38dccd80ec 100644 --- 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 @@ -160,6 +160,11 @@ + + + + + diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 42a7fce7a7e2a..42c57a7b4a22b 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -16,6 +16,7 @@ system_error_monitor system_monitor control_cmd_switcher + leader_election_converter ament_lint_auto autoware_lint_common From 67eb555ebe4941784f170b68a2d2c7fd11d2c7a8 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 10:37:24 +0900 Subject: [PATCH 05/13] feat: add rely script Signed-off-by: TetsuKawa --- .../simple_planning_simulator/CMakeLists.txt | 6 +++ .../tool/rely_trajectry.py | 47 +++++++++++++++++++ 2 files changed, 53 insertions(+) create mode 100755 system/control_cmd_switcher/tool/rely_trajectry.py diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 87d0f7e5fef0b..5531ad8f2f754 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -29,6 +29,12 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) +install(PROGRAMS + tool/rely_trajectry.py + DESTINATION lib/${PROJECT_NAME} + PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ +) + if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp diff --git a/system/control_cmd_switcher/tool/rely_trajectry.py b/system/control_cmd_switcher/tool/rely_trajectry.py new file mode 100755 index 0000000000000..b2dbef5ba4965 --- /dev/null +++ b/system/control_cmd_switcher/tool/rely_trajectry.py @@ -0,0 +1,47 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from autoware_auto_planning_msgs.msg import Trajectory +import threading + +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() From 586265199f2e1db93efd8ae985b05555eb576464 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Fri, 28 Jun 2024 10:40:14 +0900 Subject: [PATCH 06/13] feat: add launch for remapping trajectry Signed-off-by: TetsuKawa --- .../planning.main.mrm-v0.6.tmp.launch.xml | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) create mode 100644 launch/tier4_planning_launch/launch/planning.main.mrm-v0.6.tmp.launch.xml 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 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c8885e5b71bc70e11a12b1b16171d289c854a72f Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 28 Jun 2024 13:46:38 +0900 Subject: [PATCH 07/13] feat(dummy_operation_mode_publisher): add dummy operation mode publisher Signed-off-by: Makoto Kurihara --- .../CMakeLists.txt | 21 +++++++ .../dummy_operation_mode_publisher/README.md | 23 +++++++ .../dummy_operation_mode_publisher.param.yaml | 2 + .../dummy_operation_mode_publisher.launch.xml | 7 +++ .../package.xml | 25 ++++++++ .../src/dummy_operation_mode_publisher.cpp | 63 +++++++++++++++++++ .../src/dummy_operation_mode_publisher.hpp | 57 +++++++++++++++++ 7 files changed, 198 insertions(+) create mode 100644 dummy/dummy_operation_mode_publisher/CMakeLists.txt create mode 100644 dummy/dummy_operation_mode_publisher/README.md create mode 100644 dummy/dummy_operation_mode_publisher/config/dummy_operation_mode_publisher.param.yaml create mode 100644 dummy/dummy_operation_mode_publisher/launch/dummy_operation_mode_publisher.launch.xml create mode 100644 dummy/dummy_operation_mode_publisher/package.xml create mode 100644 dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp create mode 100644 dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp 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..bb5c5ba377083 --- /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..70020d88a762e --- /dev/null +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp @@ -0,0 +1,57 @@ +// 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__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#define DUMMY_OPERATION_MODE_PUBLISHER__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__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ \ No newline at end of file From ce6dc8735acf8aec358d91bf4b4beca21402c83a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Fri, 28 Jun 2024 13:47:00 +0900 Subject: [PATCH 08/13] chore(trajectory_repeater): change remap future work: parameterize Signed-off-by: Makoto Kurihara --- .../trajectory_repeater/launch/trajectory_repeater.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 @@ - + From 1805916eed51b1a82610c4c386030dc2156c5819 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 1 Jul 2024 12:52:13 +0900 Subject: [PATCH 09/13] feat: change MrmState.msg to internal one Signed-off-by: TetsuKawa --- .../include/mrm_handler/mrm_handler_core.hpp | 11 +- .../src/mrm_handler/mrm_handler_core.cpp | 107 +++++++++--------- 2 files changed, 62 insertions(+), 56 deletions(-) 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..a7aa44d702371 100644 --- a/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp +++ b/system/mrm_handler/include/mrm_handler/mrm_handler_core.hpp @@ -25,7 +25,8 @@ // Autoware #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..7c05ef1f18998 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::MrmState; + using tier4_system_msgs::msg::MrmBehavior; 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::MrmState; + using tier4_system_msgs::msg::MrmBehavior; - 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::MrmState; + using tier4_system_msgs::msg::MrmBehavior; 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,7 +465,8 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { - using autoware_adapi_v1_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmState; + using tier4_system_msgs::msg::MrmBehavior; using autoware_auto_vehicle_msgs::msg::ControlModeReport; // Check emergency @@ -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::MrmState; + using tier4_system_msgs::msg::MrmBehavior; 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() From 70daa6de7c110cbed120c54fb562898907333ecc Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Mon, 1 Jul 2024 16:22:36 +0900 Subject: [PATCH 10/13] feat(mrm_stop_operator): add mrm stop operator Signed-off-by: Makoto Kurihara --- system/mrm_stop_operator/CMakeLists.txt | 21 +++++ system/mrm_stop_operator/README.md | 23 ++++++ .../config/mrm_stop_operator.param.yaml | 6 ++ .../launch/mrm_stop_operator.launch.xml | 11 +++ system/mrm_stop_operator/package.xml | 26 ++++++ .../src/mrm_stop_operator.cpp | 80 +++++++++++++++++++ .../src/mrm_stop_operator.hpp | 72 +++++++++++++++++ 7 files changed, 239 insertions(+) create mode 100644 system/mrm_stop_operator/CMakeLists.txt create mode 100644 system/mrm_stop_operator/README.md create mode 100644 system/mrm_stop_operator/config/mrm_stop_operator.param.yaml create mode 100644 system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml create mode 100644 system/mrm_stop_operator/package.xml create mode 100644 system/mrm_stop_operator/src/mrm_stop_operator.cpp create mode 100644 system/mrm_stop_operator/src/mrm_stop_operator.hpp 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..5dfbb2956d668 --- /dev/null +++ b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml @@ -0,0 +1,6 @@ +/**: + 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..9d5226cf99f19 --- /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..d403e886ed7a1 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -0,0 +1,80 @@ +// 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..750c343f13062 --- /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__MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR__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__MRM_STOP_OPERATOR_HPP_ \ No newline at end of file From 8fa519d1bed087c42a7e7b0d0a213adfd11164e7 Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 1 Jul 2024 19:14:33 +0900 Subject: [PATCH 11/13] modify: remove relay_trajectory from psim Signed-off-by: TetsuKawa --- simulator/simple_planning_simulator/CMakeLists.txt | 6 ------ 1 file changed, 6 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 5531ad8f2f754..87d0f7e5fef0b 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -29,12 +29,6 @@ rclcpp_components_register_node(${PROJECT_NAME} EXECUTABLE ${PROJECT_NAME}_exe ) -install(PROGRAMS - tool/rely_trajectry.py - DESTINATION lib/${PROJECT_NAME} - PERMISSIONS OWNER_EXECUTE OWNER_WRITE OWNER_READ GROUP_EXECUTE GROUP_READ WORLD_EXECUTE WORLD_READ -) - if(BUILD_TESTING) ament_add_ros_isolated_gtest(test_simple_planning_simulator test/test_simple_planning_simulator.cpp From dc1c9a532a599ef37764a7dab112294fc442ef9e Mon Sep 17 00:00:00 2001 From: TetsuKawa Date: Mon, 1 Jul 2024 20:34:07 +0900 Subject: [PATCH 12/13] fix: fix CMakefile Signed-off-by: TetsuKawa --- system/control_cmd_switcher/CMakeLists.txt | 6 ++++++ .../tool/{rely_trajectry.py => rely_trajectory.py} | 0 2 files changed, 6 insertions(+) rename system/control_cmd_switcher/tool/{rely_trajectry.py => rely_trajectory.py} (100%) diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt index 42ec827f855cc..aebea9d7edad8 100644 --- a/system/control_cmd_switcher/CMakeLists.txt +++ b/system/control_cmd_switcher/CMakeLists.txt @@ -13,6 +13,12 @@ rclcpp_components_register_node(${PROJECT_NAME} 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/tool/rely_trajectry.py b/system/control_cmd_switcher/tool/rely_trajectory.py similarity index 100% rename from system/control_cmd_switcher/tool/rely_trajectry.py rename to system/control_cmd_switcher/tool/rely_trajectory.py From 3555551131ada52b32c0f5f548b8fc1c52213a5a Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 1 Jul 2024 12:40:32 +0000 Subject: [PATCH 13/13] style(pre-commit): autofix --- .../src/dummy_operation_mode_publisher.cpp | 4 +-- .../src/dummy_operation_mode_publisher.hpp | 11 ++++---- .../system.main.mrm-v0.6.tmp.launch.xml | 4 +-- launch/tier4_system_launch/package.xml | 4 +-- system/control_cmd_switcher/README.md | 2 +- .../config/control_cmd_switcher.yaml | 1 - system/control_cmd_switcher/package.xml | 2 +- .../control_cmd_switcher.cpp | 28 ++++++++++++------- .../control_cmd_switcher.hpp | 28 +++++++++---------- .../tool/rely_trajectory.py | 25 +++++++++-------- .../converter/availability_converter.cpp | 3 +- .../include/mrm_handler/mrm_handler_core.hpp | 4 +-- .../src/mrm_handler/mrm_handler_core.cpp | 12 ++++---- .../config/mrm_stop_operator.param.yaml | 1 - .../launch/mrm_stop_operator.launch.xml | 2 +- .../src/mrm_stop_operator.cpp | 13 +++++---- .../src/mrm_stop_operator.hpp | 12 ++++---- 17 files changed, 82 insertions(+), 74 deletions(-) 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 index bb5c5ba377083..b8840e0d3ebb7 100644 --- a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.cpp @@ -34,12 +34,12 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio // Timer using namespace std::literals::chrono_literals; - timer_ = rclcpp::create_timer(this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); + timer_ = rclcpp::create_timer( + this, get_clock(), 1s, std::bind(&DummyOperationModePublisher::onTimer, this)); // State // Diagnostics - } void DummyOperationModePublisher::onTimer() 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 index 70020d88a762e..43ee5b756451b 100644 --- a/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp +++ b/dummy/dummy_operation_mode_publisher/src/dummy_operation_mode_publisher.hpp @@ -12,15 +12,14 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ -#define DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#ifndef DUMMY_OPERATION_MODE_PUBLISHER_HPP_ +#define DUMMY_OPERATION_MODE_PUBLISHER_HPP_ // include #include #include - namespace dummy_operation_mode_publisher { @@ -36,7 +35,8 @@ class DummyOperationModePublisher : public rclcpp::Node // Subscriber // Publisher - rclcpp::Publisher::SharedPtr pub_operation_mode_state_; + rclcpp::Publisher::SharedPtr + pub_operation_mode_state_; // Service @@ -50,8 +50,7 @@ class DummyOperationModePublisher : public rclcpp::Node // State // Diagnostics - }; } // namespace dummy_operation_mode_publisher -#endif // DUMMY_OPERATION_MODE_PUBLISHER__DUMMY_OPERATION_MODE_PUBLISHER_HPP_ \ No newline at end of file +#endif // DUMMY_OPERATION_MODE_PUBLISHER_HPP_ 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 index fbd38dccd80ec..fac0c77c5d8ee 100644 --- 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 @@ -160,7 +160,7 @@ - + @@ -187,6 +187,4 @@ --> - - diff --git a/launch/tier4_system_launch/package.xml b/launch/tier4_system_launch/package.xml index 42c57a7b4a22b..d9cb839307c96 100644 --- a/launch/tier4_system_launch/package.xml +++ b/launch/tier4_system_launch/package.xml @@ -12,11 +12,11 @@ autoware_cmake component_state_monitor + control_cmd_switcher emergency_handler + leader_election_converter system_error_monitor system_monitor - control_cmd_switcher - leader_election_converter ament_lint_auto autoware_lint_common diff --git a/system/control_cmd_switcher/README.md b/system/control_cmd_switcher/README.md index f1e018734e2d1..7c688a50919a6 100644 --- a/system/control_cmd_switcher/README.md +++ b/system/control_cmd_switcher/README.md @@ -1 +1 @@ -# control_cmd_switcher \ No newline at end of file +# 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 index e083b38a5ebe9..b908163301d69 100644 --- a/system/control_cmd_switcher/config/control_cmd_switcher.yaml +++ b/system/control_cmd_switcher/config/control_cmd_switcher.yaml @@ -2,4 +2,3 @@ --- /**: ros__parameters: - diff --git a/system/control_cmd_switcher/package.xml b/system/control_cmd_switcher/package.xml index 7e0fa2bf2e97b..8ed70f43f51f1 100644 --- a/system/control_cmd_switcher/package.xml +++ b/system/control_cmd_switcher/package.xml @@ -13,8 +13,8 @@ autoware_auto_control_msgs rclcpp + rclcpp_components tier4_system_msgs - rclcpp_components ament_lint_auto autoware_lint_common 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 index b038264a7af99..7aae994e3ab9d 100644 --- 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 @@ -18,17 +18,23 @@ #include #include -ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) : Node("control_cmd_switcher", node_options) +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_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_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)); + "~/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}); @@ -37,21 +43,24 @@ ControlCmdSwitcher::ControlCmdSwitcher(const rclcpp::NodeOptions & node_options) use_main_control_cmd_ = true; } -void ControlCmdSwitcher::onMainControlCmd(const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +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) +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) +void ControlCmdSwitcher::onElectionStatus( + const tier4_system_msgs::msg::ElectionStatus::ConstSharedPtr msg) { if (((msg->path_info >> 3) & 0x01) == 1) { use_main_control_cmd_ = true; @@ -62,4 +71,3 @@ void ControlCmdSwitcher::onElectionStatus(const tier4_system_msgs::msg::Election #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 index 1ebdf0b51551d..1f17b63eaadc3 100644 --- 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 @@ -12,14 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ -#define CONTROL_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#ifndef CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ +#define CONTROL_CMD_SWITCHER__CONTROL_CMD_SWITCHER_HPP_ // Core +#include #include #include -#include - // Autoware #include @@ -28,28 +27,29 @@ // 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_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 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_; - + rclcpp::Publisher::SharedPtr + pub_control_cmd_; std::atomic use_main_control_cmd_; }; -#endif // CONTROL_SWITCHER__CONTROL_SWITCHER_HPP_ +#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 index b2dbef5ba4965..ef28badafdf70 100755 --- a/system/control_cmd_switcher/tool/rely_trajectory.py +++ b/system/control_cmd_switcher/tool/rely_trajectory.py @@ -1,26 +1,28 @@ #!/usr/bin/env python3 +import threading + +from autoware_auto_planning_msgs.msg import Trajectory import rclpy from rclpy.node import Node -from autoware_auto_planning_msgs.msg import Trajectory -import threading -class RelayTrajectoryNode(Node): +class RelayTrajectoryNode(Node): def __init__(self): - super().__init__('relay_trajectory') + 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) + 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() @@ -29,7 +31,7 @@ def input_thread(): nonlocal node while True: user_input = input("Enter 'y' to stop publishing: ") - if user_input.lower() == 'y': + if user_input.lower() == "y": node.running = False print("Publishing stopped.") break @@ -43,5 +45,6 @@ def input_thread(): node.destroy_node() rclpy.shutdown() -if __name__ == '__main__': + +if __name__ == "__main__": main() diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp index 24fe28efc1b63..f36203c66b4d6 100644 --- a/system/leader_election_converter/src/common/converter/availability_converter.cpp +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -75,7 +75,8 @@ void AvailabilityConverter::convertToUdp( availability.pull_over = availability_msg->pull_over; udp_availability_sender_->send(availability); } else { - RCLCPP_ERROR_THROTTLE(node_->get_logger(), *node->get_clock(), 5000, "Failed to take control mode report"); + RCLCPP_ERROR_THROTTLE( + node_->get_logger(), *node->get_clock(), 5000, "Failed to take control mode report"); } } 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 a7aa44d702371..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,13 +25,13 @@ // Autoware #include -#include -#include #include #include #include #include +#include #include +#include #include #include 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 7c05ef1f18998..700a2f1125b5d 100644 --- a/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp +++ b/system/mrm_handler/src/mrm_handler/mrm_handler_core.cpp @@ -227,8 +227,8 @@ void MrmHandler::publishMrmState() void MrmHandler::operateMrm() { - using tier4_system_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 @@ -271,8 +271,8 @@ void MrmHandler::operateMrm() void MrmHandler::handleFailedRequest() { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; if (requestMrmBehavior(MrmBehavior::EMERGENCY_STOP, CALL)) { if (mrm_state_.state != MrmState::MRM_OPERATING) transitionTo(MrmState::MRM_OPERATING); @@ -286,8 +286,8 @@ bool MrmHandler::requestMrmBehavior( const tier4_system_msgs::msg::MrmBehavior::_type_type & mrm_behavior, RequestType request_type) const { - using tier4_system_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) { @@ -465,9 +465,9 @@ void MrmHandler::transitionTo(const int new_state) void MrmHandler::updateMrmState() { - using tier4_system_msgs::msg::MrmState; - using tier4_system_msgs::msg::MrmBehavior; 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(); @@ -522,8 +522,8 @@ void MrmHandler::updateMrmState() tier4_system_msgs::msg::MrmBehavior::_type_type MrmHandler::getCurrentMrmBehavior() { - using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::MrmBehavior; + using tier4_system_msgs::msg::MrmState; using tier4_system_msgs::msg::OperationModeAvailability; // State machine diff --git a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml index 5dfbb2956d668..7043b61596f76 100644 --- a/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml +++ b/system/mrm_stop_operator/config/mrm_stop_operator.param.yaml @@ -3,4 +3,3 @@ 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 index 9d5226cf99f19..58305c509c70c 100644 --- a/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -3,7 +3,7 @@ - + diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.cpp b/system/mrm_stop_operator/src/mrm_stop_operator.cpp index d403e886ed7a1..7ffc8aa276326 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.cpp +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -33,8 +33,9 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) // 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()); + pub_velocity_limit_clear_command_ = + create_publisher( + "~/output/velocity_limit_clear_command", rclcpp::QoS{10}.transient_local()); // Timer @@ -48,13 +49,13 @@ MrmStopOperator::MrmStopOperator(const rclcpp::NodeOptions & node_options) 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) { + 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; @@ -64,7 +65,7 @@ void MrmStopOperator::onMrmRequest(const tier4_system_msgs::msg::MrmBehavior::Co velocity_limit.constraints.min_jerk = params_.min_jerk; velocity_limit.sender = "mrm_stop_operator"; pub_velocity_limit_->publish(velocity_limit); - + last_mrm_request_ = *msg; } } diff --git a/system/mrm_stop_operator/src/mrm_stop_operator.hpp b/system/mrm_stop_operator/src/mrm_stop_operator.hpp index 750c343f13062..a44b2ed4f1988 100644 --- a/system/mrm_stop_operator/src/mrm_stop_operator.hpp +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ -#define MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ +#ifndef MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR_HPP_ // include #include @@ -51,7 +51,8 @@ class MrmStopOperator : public rclcpp::Node // Publisher rclcpp::Publisher::SharedPtr pub_velocity_limit_; - rclcpp::Publisher::SharedPtr pub_velocity_limit_clear_command_; + rclcpp::Publisher::SharedPtr + pub_velocity_limit_clear_command_; // Service @@ -61,12 +62,11 @@ class MrmStopOperator : public rclcpp::Node // State tier4_system_msgs::msg::MrmBehavior last_mrm_request_; - + void initState(); // Diagnostics - }; } // namespace mrm_stop_operator -#endif // MRM_STOP_OPERATOR__MRM_STOP_OPERATOR_HPP_ \ No newline at end of file +#endif // MRM_STOP_OPERATOR_HPP_