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/planning/trajectory_repeater/CMakeLists.txt b/planning/trajectory_repeater/CMakeLists.txt new file mode 100644 index 0000000000000..41185c3495afe --- /dev/null +++ b/planning/trajectory_repeater/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.14) +project(trajectory_repeater) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(trajectory_repeater SHARED + src/trajectory_repeater.cpp +) +ament_target_dependencies(trajectory_repeater) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "trajectory_repeater::TrajectoryRepeater" + EXECUTABLE ${PROJECT_NAME}_node +) + +ament_auto_package( + INSTALL_TO_SHARE + launch + config +) diff --git a/planning/trajectory_repeater/README.md b/planning/trajectory_repeater/README.md new file mode 100644 index 0000000000000..b484c80639437 --- /dev/null +++ b/planning/trajectory_repeater/README.md @@ -0,0 +1,23 @@ +# Trajectory Repeater + +## 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/planning/trajectory_repeater/config/trajectory_repeater.param.yaml b/planning/trajectory_repeater/config/trajectory_repeater.param.yaml new file mode 100644 index 0000000000000..a20dbd7ffd3d9 --- /dev/null +++ b/planning/trajectory_repeater/config/trajectory_repeater.param.yaml @@ -0,0 +1,2 @@ +/**: + ros__parameters: diff --git a/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml new file mode 100644 index 0000000000000..ebbde691c32d4 --- /dev/null +++ b/planning/trajectory_repeater/launch/trajectory_repeater.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/planning/trajectory_repeater/package.xml b/planning/trajectory_repeater/package.xml new file mode 100644 index 0000000000000..eb2bcf24a2aaf --- /dev/null +++ b/planning/trajectory_repeater/package.xml @@ -0,0 +1,25 @@ + + + + trajectory_repeater + 0.1.0 + The trajectory_repeater package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_auto_planning_msgs + rclcpp + rclcpp_components + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/planning/trajectory_repeater/src/trajectory_repeater.cpp b/planning/trajectory_repeater/src/trajectory_repeater.cpp new file mode 100644 index 0000000000000..af6852c70398a --- /dev/null +++ b/planning/trajectory_repeater/src/trajectory_repeater.cpp @@ -0,0 +1,64 @@ +// 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 "trajectory_repeater.hpp" + +namespace trajectory_repeater +{ + +TrajectoryRepeater::TrajectoryRepeater(const rclcpp::NodeOptions & node_options) +: Node("trajectory_repeater", node_options) +{ + // Parameter + + // Subscriber + sub_trajectory_ = create_subscription( + "~/input/trajectory", 10, std::bind(&TrajectoryRepeater::onTrajectory, this, std::placeholders::_1)); + + // Publisher + pub_trajectory_ = create_publisher("~/output/trajectory", 10); + + // Service + + // Client + + // Timer + using namespace std::literals::chrono_literals; + timer_ = rclcpp::create_timer(this, get_clock(), 100ms, std::bind(&TrajectoryRepeater::onTimer, this)); + + // State + + // Diagnostics + +} + +void TrajectoryRepeater::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + last_trajectory_ = msg; +} + +void TrajectoryRepeater::onTimer() +{ + if (!last_trajectory_) { + RCLCPP_DEBUG(get_logger(), "No trajectory received"); + return; + } + + pub_trajectory_->publish(*last_trajectory_); +} + +} // namespace trajectory_repeater + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(trajectory_repeater::TrajectoryRepeater) diff --git a/planning/trajectory_repeater/src/trajectory_repeater.hpp b/planning/trajectory_repeater/src/trajectory_repeater.hpp new file mode 100644 index 0000000000000..4b6d47823818b --- /dev/null +++ b/planning/trajectory_repeater/src/trajectory_repeater.hpp @@ -0,0 +1,60 @@ +// 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 TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ +#define TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ + +// include +#include + +#include + +namespace trajectory_repeater +{ + +class TrajectoryRepeater : public rclcpp::Node +{ +public: + explicit TrajectoryRepeater(const rclcpp::NodeOptions & node_options); + ~TrajectoryRepeater() = default; + +private: + // Parameter + + // Subscriber + rclcpp::Subscription::SharedPtr sub_trajectory_; + + void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + // Publisher + rclcpp::Publisher::SharedPtr pub_trajectory_; + + // Service + + // Client + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + void onTimer(); + + // State + autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr last_trajectory_; + + // Diagnostics + +}; +} // namespace trajectory_repeater + +#endif // TRAJECTORY_REPEATER__TRAJECTORY_REPEATER_HPP_ \ No newline at end of file diff --git a/system/control_cmd_switcher/CMakeLists.txt b/system/control_cmd_switcher/CMakeLists.txt new file mode 100644 index 0000000000000..2bd58dbeedaaa --- /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/relay_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..c19abb1c69af4 --- /dev/null +++ b/system/control_cmd_switcher/launch/control_cmd_switcher.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + 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..ddec94716c376 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.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 "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_main_ = create_subscription( + "~/input/election/status/main", rclcpp::QoS{10}, + std::bind(&ControlCmdSwitcher::onElectionStatus, this, std::placeholders::_1)); + + sub_election_status_sub_ = create_subscription( + "~/input/election/status/sub", 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->election_start_count <= 0) return; + if (msg->in_election) return; + 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..446979ca1c2c4 --- /dev/null +++ b/system/control_cmd_switcher/src/control_cmd_switcher/control_cmd_switcher.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 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_main_; + rclcpp::Subscription::SharedPtr sub_election_status_sub_; + 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/relay_trajectory.py b/system/control_cmd_switcher/tool/relay_trajectory.py new file mode 100755 index 0000000000000..ef28badafdf70 --- /dev/null +++ b/system/control_cmd_switcher/tool/relay_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/leader_election_converter/CMakeLists.txt b/system/leader_election_converter/CMakeLists.txt new file mode 100644 index 0000000000000..697b9a3b74d13 --- /dev/null +++ b/system/leader_election_converter/CMakeLists.txt @@ -0,0 +1,30 @@ +cmake_minimum_required(VERSION 3.14) +project(leader_election_converter) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(common_converter SHARED + src/common/converter/availability_converter.cpp + src/common/converter/mrm_converter.cpp + src/common/converter/log_converter.cpp +) + +target_include_directories(common_converter PRIVATE + src/common/converter +) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/node/leader_election_converter.cpp +) +target_include_directories(${PROJECT_NAME} PRIVATE src/common/converter) + +target_link_libraries(${PROJECT_NAME} common_converter) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "leader_election_converter::LeaderElectionConverter" + EXECUTABLE ${PROJECT_NAME}_node + EXECUTOR MultiThreadedExecutor +) + +ament_auto_package(INSTALL_TO_SHARE config launch) diff --git a/system/leader_election_converter/README.md b/system/leader_election_converter/README.md new file mode 100644 index 0000000000000..af98223bfaf39 --- /dev/null +++ b/system/leader_election_converter/README.md @@ -0,0 +1,50 @@ +# leader_election_converter + +## Overview + +The leader election converter node is responsible for relaying UDP packets and ROS2 topics between the leader_election invoked by systemd and Autoware executed on ROS2. + +## availability converter + +The availability converter subscribes `/system/operation_mode/availability` and `/vehicle/status/mrm_state`, adds them together into a structure called `Availability` and sends it as a udp packet. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------------- | -------------------------------------------------- | ----------------------------- | +| subscriber | `/system/operation_mode/availability` | `tier4_system_msgs/msg/OperationModeAvailability` | Usable behavior of the ego. | +| subscriber | `/vehicle/status/mrm_state` | `autoware_auto_vehicle_msgs/msg/ControlModeReport` | Ego control mode. | +| udp sender | none | `struct Availability` | Combination of the above two. | + +## mrm converter + +The mrm converter subscribes `/system/fail_safe/mrm_state` into a structure called `MrmState` and sends it as a UDP packet. +In addition, it receives a udp packet`MrmState` and publish `/system/mrm_request`. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | ------------------------------ | ----------------------------------- | ------------------------ | +| subscriber | `/system/fail_safe/mrm_state` | `tier4_system_msgs/msg/MrmState` | MRM status of each ECU. | +| udp sender | none | `struct MrmState` | Same as above. | +| publisher | `/system/election/mrm_request` | `tier4_system_msgs/msg/MrmBehavior` | Request of MRM behavior. | +| udp receiver | none | `struct MrmRequest` | Same as above. | + +## log converter + +The log　converter receive udp packets into a structure called `ElectionCommunication` and `ElectionStatus`, and publish `/system/election/communication`, +`/system/election/status`, and `/system/fail_safe/over_all/mrm_state`. + +### Interface + +| Interface Type | Interface Name | Data Type | Description | +| -------------- | -------------------------------------- | --------------------------------------------- | ------------------------------ | +| udp receiver | none | `struct ElectionCommunication` | messages among election nodes. | +| udp receiver | none | `struct ElectionStatus` | Leader Election status. | +| publisher | `/system/election/communication` | `tier4_system_msgs/msg/ElectionCommunication` | messages among election nodes. | +| publisher | `/system/election/status` | `tier4_system_msgs/msg/MrmState` | Leader Election status. | +| publisher | `/system/fail_safe/over_all/mrm_state` | `autoware_adapi_v1_msgs/msg/mrm_state` | System-wide MRM status. | + +## Parameters + +{{ json_to_markdown("system/leader_election_converter/schema/leader_election_converter.schema.json") }} diff --git a/system/leader_election_converter/config/leader_election_converter.param.yaml b/system/leader_election_converter/config/leader_election_converter.param.yaml new file mode 100644 index 0000000000000..5269a558cd107 --- /dev/null +++ b/system/leader_election_converter/config/leader_election_converter.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + availability_dest_ip: "127.0.0.1" + availability_dest_port: "9000" + mrm_state_dest_ip: "127.0.0.1" + mrm_state_dest_port: "9001" + mrm_request_src_ip: "127.0.0.1" + mrm_request_src_port: "9002" + election_communication_src_ip: "127.0.0.1" + election_communication_src_port: "9003" + election_status_src_ip: "127.0.0.1" + election_status_src_port: "9004" diff --git a/system/leader_election_converter/launch/leader_election_converter.launch.xml b/system/leader_election_converter/launch/leader_election_converter.launch.xml new file mode 100644 index 0000000000000..2078fc7fd7d42 --- /dev/null +++ b/system/leader_election_converter/launch/leader_election_converter.launch.xml @@ -0,0 +1,20 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/system/leader_election_converter/package.xml b/system/leader_election_converter/package.xml new file mode 100644 index 0000000000000..d9341c8fe2df3 --- /dev/null +++ b/system/leader_election_converter/package.xml @@ -0,0 +1,25 @@ + + + + leader_election_converter + 0.1.0 + The leader election converter package + TetsuKawa + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_adapi_v1_msgs + autoware_auto_vehicle_msgs + rclcpp + rclcpp_components + tier4_system_msgs + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/system/leader_election_converter/schema/leader_election_converter.schema.json b/system/leader_election_converter/schema/leader_election_converter.schema.json new file mode 100644 index 0000000000000..c54fdf2210221 --- /dev/null +++ b/system/leader_election_converter/schema/leader_election_converter.schema.json @@ -0,0 +1,89 @@ +{ + "$schema": "http://json-schema.org/draft-07/schema#", + "title": "Parameters for leader election converter", + "type": "object", + "definitions": { + "leader_election_converter": { + "type": "object", + "properties": { + "availability_dest_ip": { + "type": "string", + "description": "IP address of the destination of availability", + "default": "127.0.0.1" + }, + "availability_dest_port": { + "type": "string", + "description": "Port of the destination of availability", + "default": "9000" + }, + "mrm_state_dest_ip": { + "type": "string", + "description": "IP address of the destination of mrm_state", + "default": "127.0.0.1" + }, + "mrm_state_dest_port": { + "type": "string", + "description": "Port of the destination of mrm_state", + "default": "9001" + }, + "mrm_request_src_ip": { + "type": "string", + "description": "IP address of the source of mrm_request", + "default": "127.0.0.1" + }, + "mrm_request_src_port": { + "type": "string", + "description": "Port of the source of mrm_request", + "default": "9002" + }, + "election_communication_src_ip": { + "type": "string", + "description": "IP address of the source of election_communication", + "default": "127.0.0.1" + }, + "election_communication_src_port": { + "type": "string", + "description": "Port of the source of election_communication", + "default": "9003" + }, + "election_status_src_ip": { + "type": "string", + "description": "IP address of the source of election_status", + "default": "127.0.0.1" + }, + "election_status_src_port": { + "type": "string", + "description": "Port of the source of election_status", + "default": "9004" + } + }, + "required": [ + "availability_dest_ip", + "availability_dest_port", + "mrm_state_dest_ip", + "mrm_state_dest_port", + "mrm_request_src_ip", + "mrm_request_src_port", + "election_communication_src_ip", + "election_communication_src_port", + "election_status_src_ip", + "election_status_src_port" + ], + "additionalProperties": false + } + }, + "properties": { + "/**": { + "type": "object", + "properties": { + "ros__parameters": { + "$ref": "#/definitions/leader_election_converter" + } + }, + "required": ["ros__parameters"], + "additionalProperties": false + } + }, + "required": ["/**"], + "additionalProperties": false +} diff --git a/system/leader_election_converter/src/common/converter/availability_converter.cpp b/system/leader_election_converter/src/common/converter/availability_converter.cpp new file mode 100644 index 0000000000000..24eae8f98d986 --- /dev/null +++ b/system/leader_election_converter/src/common/converter/availability_converter.cpp @@ -0,0 +1,83 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "availability_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace leader_election_converter +{ + +AvailabilityConverter::AvailabilityConverter(rclcpp::Node * node) : node_(node) +{ +} + +void AvailabilityConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) +{ + udp_availability_sender_ = std::make_unique>(dest_ip, dest_port); +} + +void AvailabilityConverter::setSubscriber() +{ + const auto qos = rclcpp::QoS(1); + availability_callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions availability_options = rclcpp::SubscriptionOptions(); + availability_options.callback_group = availability_callback_group_; + + control_mode_callback_group_ = + node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions control_mode_options = rclcpp::SubscriptionOptions(); + control_mode_options.callback_group = control_mode_callback_group_; + auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg:: + ControlModeReport::ConstSharedPtr msg) {}; + + sub_operation_mode_availability_ = + node_->create_subscription( + "~/input/operation_mode_availability", qos, + std::bind(&AvailabilityConverter::convertToUdp, this, std::placeholders::_1), + availability_options); + + sub_control_mode_ = + node_->create_subscription( + "~/input/control_mode", qos, not_executed_callback, control_mode_options); +} + +void AvailabilityConverter::convertToUdp( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg) +{ + auto control_mode_report = std::make_shared(); + rclcpp::MessageInfo message_info; + const bool success = sub_control_mode_->take(*control_mode_report, message_info); + if (success) { + Availability availability; + availability.mode = control_mode_report->mode; + availability.stop = availability_msg->stop; + availability.autonomous = availability_msg->autonomous; + availability.local = availability_msg->local; + availability.remote = availability_msg->remote; + availability.emergency_stop = availability_msg->emergency_stop; + availability.comfortable_stop = availability_msg->comfortable_stop; + 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"); + } +} + +} // namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/availability_converter.hpp b/system/leader_election_converter/src/common/converter/availability_converter.hpp new file mode 100644 index 0000000000000..a0dd5ebb422bb --- /dev/null +++ b/system/leader_election_converter/src/common/converter/availability_converter.hpp @@ -0,0 +1,67 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ +#define COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ + +#include "udp_sender.hpp" + +#include + +#include +#include + +#include +#include + +namespace leader_election_converter +{ + +struct Availability +{ + autoware_auto_vehicle_msgs::msg::ControlModeReport::_mode_type mode; + tier4_system_msgs::msg::OperationModeAvailability::_stop_type stop; + tier4_system_msgs::msg::OperationModeAvailability::_autonomous_type autonomous; + tier4_system_msgs::msg::OperationModeAvailability::_local_type local; + tier4_system_msgs::msg::OperationModeAvailability::_remote_type remote; + tier4_system_msgs::msg::OperationModeAvailability::_emergency_stop_type emergency_stop; + tier4_system_msgs::msg::OperationModeAvailability::_comfortable_stop_type comfortable_stop; + tier4_system_msgs::msg::OperationModeAvailability::_pull_over_type pull_over; + // tier4_system_msgs::msg::OperationModeAvailability::_stop_next_bus_stop_type stop_next_bus_stop; +}; + +class AvailabilityConverter +{ +public: + explicit AvailabilityConverter(rclcpp::Node * node); + + void setUdpSender(const std::string & dest_ip, const std::string & dest_port); + void setSubscriber(); + void convertToUdp( + const tier4_system_msgs::msg::OperationModeAvailability::ConstSharedPtr availability_msg); + +private: + rclcpp::Node * node_; + std::unique_ptr> udp_availability_sender_; + rclcpp::CallbackGroup::SharedPtr availability_callback_group_; + rclcpp::CallbackGroup::SharedPtr control_mode_callback_group_; + rclcpp::Subscription::SharedPtr + sub_control_mode_; + rclcpp::Subscription::SharedPtr + sub_operation_mode_availability_; +}; + +} // namespace leader_election_converter + +#endif // COMMON__CONVERTER__AVAILABILITY_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/log_converter.cpp b/system/leader_election_converter/src/common/converter/log_converter.cpp new file mode 100644 index 0000000000000..a1e9ee6cd89dd --- /dev/null +++ b/system/leader_election_converter/src/common/converter/log_converter.cpp @@ -0,0 +1,139 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "log_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace leader_election_converter +{ + +LogConverter::LogConverter(rclcpp::Node * node) +: node_(node), is_election_communication_running_(true), is_election_status_running_(true) +{ +} + +LogConverter::~LogConverter() +{ + is_election_communication_running_ = false; + udp_election_communication_receiver_->~UdpReceiver(); + is_election_status_running_ = false; + udp_election_status_receiver_->~UdpReceiver(); + if (udp_election_communication_thread_.joinable()) { + udp_election_communication_thread_.join(); + } + if (udp_election_status_thread_.joinable()) { + udp_election_status_thread_.join(); + } +} + +void LogConverter::setUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port) +{ + udp_election_communication_thread_ = + std::thread(&LogConverter::startUdpElectionCommunicationReceiver, this, src_ip, src_port); +} + +void LogConverter::startUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port) +{ + try { + udp_election_communication_receiver_ = std::make_unique>( + src_ip, src_port, + std::bind(&LogConverter::convertElectionCommunicationToTopic, this, std::placeholders::_1)); + while (is_election_communication_running_) { + udp_election_communication_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void LogConverter::setUdpElectionStatusReceiver( + const std::string & src_ip, const std::string & src_port) +{ + udp_election_status_thread_ = + std::thread(&LogConverter::startUdpElectionStatusReceiver, this, src_ip, src_port); +} + +void LogConverter::startUdpElectionStatusReceiver( + const std::string & src_ip, const std::string & src_port) +{ + try { + udp_election_status_receiver_ = std::make_unique>( + src_ip, src_port, + std::bind(&LogConverter::convertElectionStatusToTopic, this, std::placeholders::_1)); + while (is_election_status_running_) { + udp_election_status_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void LogConverter::setPublisher() +{ + pub_election_communication_ = + node_->create_publisher( + "~/output/election_communication", rclcpp::QoS{1}); + pub_election_status_ = node_->create_publisher( + "~/output/election_status", rclcpp::QoS{1}); + pub_over_all_mrm_state_ = node_->create_publisher( + "~/output/over_all_mrm_state", rclcpp::QoS{1}); +} + +void LogConverter::convertElectionCommunicationToTopic(const ElectionCommunication & node_msg) +{ + tier4_system_msgs::msg::ElectionCommunication msg; + msg.stamp = node_->now(); + msg.node_id = (node_msg.msg >> 8) & 0xFF; + msg.type = node_msg.msg & 0xFF; + msg.term = (node_msg.msg >> 16) & 0xFF; + msg.link = (node_msg.msg >> 24) & 0xFF; + msg.heartbeat = (node_msg.msg >> 56) & 0x0F; + msg.checksum = (node_msg.msg >> 60) & 0x0F; + pub_election_communication_->publish(msg); +} + +void LogConverter::convertElectionStatusToTopic(const ElectionStatus & status) +{ + tier4_system_msgs::msg::ElectionStatus election_status; + autoware_adapi_v1_msgs::msg::MrmState mrm_status; + + election_status.stamp = node_->now(); + election_status.leader_id = status.leader_id; + election_status.path_info = status.path_info; + election_status.mrm_state.state = status.state; + election_status.mrm_state.behavior.type = status.behavior; + election_status.election_start_count = status.election_start_count; + election_status.in_election = status.in_election; + election_status.has_received_availability = status.has_received_availability; + election_status.has_received_mrm_state = status.has_received_mrm_state; + election_status.is_autoware_emergency = status.is_autoware_emergency; + election_status.is_main_ecu_connected = status.is_main_ecu_connected; + election_status.is_sub_ecu_connected = status.is_sub_ecu_connected; + election_status.is_main_vcu_connected = status.is_main_vcu_connected; + election_status.is_sub_vcu_connected = status.is_sub_vcu_connected; + pub_election_status_->publish(election_status); + + mrm_status.stamp = node_->now(); + mrm_status.state = status.state; + mrm_status.behavior = status.behavior; + pub_over_all_mrm_state_->publish(mrm_status); +} + +} // namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/log_converter.hpp b/system/leader_election_converter/src/common/converter/log_converter.hpp new file mode 100644 index 0000000000000..af0113544bf7f --- /dev/null +++ b/system/leader_election_converter/src/common/converter/log_converter.hpp @@ -0,0 +1,92 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 COMMON__CONVERTER__LOG_CONVERTER_HPP_ +#define COMMON__CONVERTER__LOG_CONVERTER_HPP_ + +#include "udp_receiver.hpp" +#include "udp_sender.hpp" + +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace leader_election_converter +{ + +typedef struct ElectionCommunication +{ + uint64_t msg; +} ElectionCommunication; + +typedef struct ElectionStatus +{ + tier4_system_msgs::msg::ElectionStatus::_leader_id_type leader_id; + tier4_system_msgs::msg::ElectionStatus::_path_info_type path_info; + tier4_system_msgs::msg::MrmState::_state_type state; + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; + tier4_system_msgs::msg::ElectionStatus::_election_start_count_type election_start_count; + tier4_system_msgs::msg::ElectionStatus::_in_election_type in_election; + tier4_system_msgs::msg::ElectionStatus::_has_received_availability_type has_received_availability; + tier4_system_msgs::msg::ElectionStatus::_has_received_mrm_state_type has_received_mrm_state; + tier4_system_msgs::msg::ElectionStatus::_is_autoware_emergency_type is_autoware_emergency; + tier4_system_msgs::msg::ElectionStatus::_is_main_ecu_connected_type is_main_ecu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_sub_ecu_connected_type is_sub_ecu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_main_vcu_connected_type is_main_vcu_connected; + tier4_system_msgs::msg::ElectionStatus::_is_sub_vcu_connected_type is_sub_vcu_connected; +} ElectionStatus; + +class LogConverter +{ +public: + explicit LogConverter(rclcpp::Node * node); + ~LogConverter(); + + void setUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port); + void setUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port); + void setPublisher(); + +private: + void startUdpElectionCommunicationReceiver( + const std::string & src_ip, const std::string & src_port); + void startUdpElectionStatusReceiver(const std::string & src_ip, const std::string & src_port); + void convertElectionCommunicationToTopic(const ElectionCommunication & node_msg); + void convertElectionStatusToTopic(const ElectionStatus & status); + + rclcpp::Node * node_; + std::unique_ptr> udp_election_communication_receiver_; + std::unique_ptr> udp_election_status_receiver_; + rclcpp::Publisher::SharedPtr + pub_election_communication_; + rclcpp::Publisher::SharedPtr pub_election_status_; + rclcpp::Publisher::SharedPtr pub_over_all_mrm_state_; + + std::thread udp_election_communication_thread_; + std::thread udp_election_status_thread_; + std::atomic is_election_communication_running_; + std::atomic is_election_status_running_; +}; + +} // namespace leader_election_converter + +#endif // COMMON__CONVERTER__LOG_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.cpp b/system/leader_election_converter/src/common/converter/mrm_converter.cpp new file mode 100644 index 0000000000000..9fe6c58a04ab8 --- /dev/null +++ b/system/leader_election_converter/src/common/converter/mrm_converter.cpp @@ -0,0 +1,98 @@ +// Copyright 2024 The Autoware Contributors +// +// 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_converter.hpp" + +#include "rclcpp/rclcpp.hpp" + +#include +#include + +namespace leader_election_converter +{ + +MrmConverter::MrmConverter(rclcpp::Node * node) : node_(node), is_udp_receiver_running_(true) +{ +} + +MrmConverter::~MrmConverter() +{ + is_udp_receiver_running_ = false; + udp_mrm_request_receiver_->~UdpReceiver(); + if (udp_receiver_thread_.joinable()) { + udp_receiver_thread_.join(); + } +} + +void MrmConverter::setUdpSender(const std::string & dest_ip, const std::string & dest_port) +{ + udp_mrm_state_sender_ = std::make_unique>(dest_ip, dest_port); +} + +void MrmConverter::setUdpReceiver(const std::string & src_ip, const std::string & src_port) +{ + udp_receiver_thread_ = std::thread(&MrmConverter::startUdpReceiver, this, src_ip, src_port); +} + +void MrmConverter::startUdpReceiver(const std::string & src_ip, const std::string & src_port) +{ + try { + udp_mrm_request_receiver_ = std::make_unique>( + src_ip, src_port, std::bind(&MrmConverter::convertToTopic, this, std::placeholders::_1)); + while (is_udp_receiver_running_) { + udp_mrm_request_receiver_->receive(); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(node_->get_logger(), "Error in UDP receiver thread: %s", e.what()); + } +} + +void MrmConverter::setSubscriber() +{ + const auto qos = rclcpp::QoS(1); + callback_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + rclcpp::SubscriptionOptions options; + options.callback_group = callback_group_; + + sub_mrm_state_ = node_->create_subscription( + "~/input/mrm_state", qos, std::bind(&MrmConverter::convertToUdp, this, std::placeholders::_1), + options); +} + +void MrmConverter::setPublisher() +{ + pub_mrm_request_ = node_->create_publisher( + "~/output/mrm_request", rclcpp::QoS{1}); +} + +void MrmConverter::convertToUdp( + const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg) +{ + MrmState mrm_state; + mrm_state.state = mrm_state_msg->state; + mrm_state.behavior = mrm_state_msg->behavior.type; + + udp_mrm_state_sender_->send(mrm_state); +} + +void MrmConverter::convertToTopic(const MrmRequest & mrm_request) +{ + tier4_system_msgs::msg::MrmBehavior mrm_request_msg; + mrm_request_msg.stamp = node_->now(); + mrm_request_msg.type = mrm_request.behavior; + + pub_mrm_request_->publish(mrm_request_msg); +} + +} // namespace leader_election_converter diff --git a/system/leader_election_converter/src/common/converter/mrm_converter.hpp b/system/leader_election_converter/src/common/converter/mrm_converter.hpp new file mode 100644 index 0000000000000..347a816029ecc --- /dev/null +++ b/system/leader_election_converter/src/common/converter/mrm_converter.hpp @@ -0,0 +1,74 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 COMMON__CONVERTER__MRM_CONVERTER_HPP_ +#define COMMON__CONVERTER__MRM_CONVERTER_HPP_ + +#include "udp_receiver.hpp" +#include "udp_sender.hpp" + +#include + +#include +#include + +#include +#include +#include +#include + +namespace leader_election_converter +{ + +typedef struct MrmState +{ + tier4_system_msgs::msg::MrmState::_state_type state; + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; +} MrmState; + +typedef struct MrmRequest +{ + tier4_system_msgs::msg::MrmBehavior::_type_type behavior; +} MrmRequest; + +class MrmConverter +{ +public: + explicit MrmConverter(rclcpp::Node * node); + ~MrmConverter(); + + void setUdpSender(const std::string & dest_ip, const std::string & dest_port); + void setUdpReceiver(const std::string & src_ip, const std::string & src_port); + void setSubscriber(); + void setPublisher(); + +private: + void startUdpReceiver(const std::string & src_ip, const std::string & src_port); + void convertToUdp(const tier4_system_msgs::msg::MrmState::ConstSharedPtr mrm_state_msg); + void convertToTopic(const MrmRequest & mrm_request); + + rclcpp::Node * node_; + std::unique_ptr> udp_mrm_state_sender_; + std::unique_ptr> udp_mrm_request_receiver_; + rclcpp::CallbackGroup::SharedPtr callback_group_; + rclcpp::Subscription::SharedPtr sub_mrm_state_; + rclcpp::Publisher::SharedPtr pub_mrm_request_; + + std::thread udp_receiver_thread_; + std::atomic is_udp_receiver_running_; +}; + +} // namespace leader_election_converter + +#endif // COMMON__CONVERTER__MRM_CONVERTER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_receiver.hpp b/system/leader_election_converter/src/common/converter/udp_receiver.hpp new file mode 100644 index 0000000000000..3a3e9f7075004 --- /dev/null +++ b/system/leader_election_converter/src/common/converter/udp_receiver.hpp @@ -0,0 +1,151 @@ + +// Copyright 2024 The Autoware Contributors +// +// 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 COMMON__CONVERTER__UDP_RECEIVER_HPP_ +#define COMMON__CONVERTER__UDP_RECEIVER_HPP_ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace leader_election_converter +{ + +template +class UdpReceiver +{ +public: + using CallbackType = std::function; + UdpReceiver(const std::string & ip, const std::string & port); + UdpReceiver(const std::string & ip, const std::string & port, CallbackType callback); + UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking); + UdpReceiver( + const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback); + ~UdpReceiver(); + + bool receive(T & data); // for non callback + void receive(); // for callback + +private: + int socketfd_; + struct addrinfo * res_; + CallbackType callback_; + + void setCallback(CallbackType callback); +}; + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port) +{ + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + + if (getaddrinfo(ip.c_str(), port.c_str(), &hints, &res_) != 0) { + throw std::runtime_error("getaddrinfo failed"); + } + + socketfd_ = socket(res_->ai_family, res_->ai_socktype, res_->ai_protocol); + if (socketfd_ < 0) { + freeaddrinfo(res_); + throw std::runtime_error("socket failed"); + } + + if (bind(socketfd_, res_->ai_addr, res_->ai_addrlen) < 0) { + freeaddrinfo(res_); + close(socketfd_); + throw std::runtime_error("bind failed"); + } +} + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, CallbackType callback) +: UdpReceiver(ip, port) +{ + setCallback(callback); +} + +template +UdpReceiver::UdpReceiver(const std::string & ip, const std::string & port, bool is_non_blocking) +: UdpReceiver(ip, port) +{ + if (is_non_blocking) { + if (fcntl(socketfd_, F_SETFL, O_NONBLOCK) < 0) { + freeaddrinfo(res_); + close(socketfd_); + throw std::runtime_error("fcntl failed"); + } + } +} + +template +UdpReceiver::UdpReceiver( + const std::string & ip, const std::string & port, bool is_non_blocking, CallbackType callback) +: UdpReceiver(ip, port, is_non_blocking) +{ + setCallback(callback); +} + +template +UdpReceiver::~UdpReceiver() +{ + shutdown(socketfd_, SHUT_RDWR); + freeaddrinfo(res_); + close(socketfd_); +} + +template +void UdpReceiver::setCallback(CallbackType callback) +{ + callback_ = callback; +} + +template +bool UdpReceiver::receive(T & data) +{ + struct sockaddr_storage addr; + socklen_t addr_len = sizeof(addr); + ssize_t recv_size = recvfrom(socketfd_, &data, sizeof(T), 0, (struct sockaddr *)&addr, &addr_len); + if (recv_size < 0) { + if (errno == EAGAIN || errno == EWOULDBLOCK) { + return false; + } + throw std::runtime_error("recvfrom failed"); + } + return true; +} + +template +void UdpReceiver::receive() +{ + T data; + if (receive(data) && callback_) { + callback_(data); + } +} + +} // namespace leader_election_converter + +#endif // COMMON__CONVERTER__UDP_RECEIVER_HPP_ diff --git a/system/leader_election_converter/src/common/converter/udp_sender.hpp b/system/leader_election_converter/src/common/converter/udp_sender.hpp new file mode 100644 index 0000000000000..15cb16774a24e --- /dev/null +++ b/system/leader_election_converter/src/common/converter/udp_sender.hpp @@ -0,0 +1,82 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 COMMON__CONVERTER__UDP_SENDER_HPP_ +#define COMMON__CONVERTER__UDP_SENDER_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace leader_election_converter +{ + +template +class UdpSender +{ +public: + UdpSender(const std::string & ip, const std::string & port); + ~UdpSender(); + + void send(const T & data); + +private: + int socketfd_; + struct addrinfo * res_; +}; + +template +UdpSender::UdpSender(const std::string & ip, const std::string & port) +{ + struct addrinfo hints; + memset(&hints, 0, sizeof(hints)); + hints.ai_family = AF_UNSPEC; + hints.ai_socktype = SOCK_DGRAM; + + if (getaddrinfo(ip.c_str(), port.c_str(), &hints, &res_) != 0) { + throw std::runtime_error("getaddrinfo failed"); + } + + socketfd_ = socket(res_->ai_family, res_->ai_socktype, res_->ai_protocol); + if (socketfd_ < 0) { + freeaddrinfo(res_); + throw std::runtime_error("socket failed"); + } +} + +template +UdpSender::~UdpSender() +{ + shutdown(socketfd_, SHUT_RDWR); + freeaddrinfo(res_); + close(socketfd_); +} + +template +void UdpSender::send(const T & data) +{ + if (sendto(socketfd_, &data, sizeof(T), 0, res_->ai_addr, res_->ai_addrlen) < 0) { + throw std::runtime_error("sendto failed"); + } +} + +} // namespace leader_election_converter + +#endif // COMMON__CONVERTER__UDP_SENDER_HPP_ diff --git a/system/leader_election_converter/src/node/leader_election_converter.cpp b/system/leader_election_converter/src/node/leader_election_converter.cpp new file mode 100644 index 0000000000000..abd136fdea39c --- /dev/null +++ b/system/leader_election_converter/src/node/leader_election_converter.cpp @@ -0,0 +1,62 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 "leader_election_converter.hpp" + +#include + +namespace leader_election_converter +{ + +LeaderElectionConverter::LeaderElectionConverter(const rclcpp::NodeOptions & node_options) +: Node("leader_election_converter", node_options), + availability_converter_(this), + mrm_converter_(this), + log_converter_(this) +{ + availability_dest_ip_ = declare_parameter("availability_dest_ip"); + availability_dest_port_ = declare_parameter("availability_dest_port"); + mrm_state_dest_ip_ = declare_parameter("mrm_state_dest_ip"); + mrm_state_dest_port_ = declare_parameter("mrm_state_dest_port"); + mrm_request_src_ip_ = declare_parameter("mrm_request_src_ip"); + mrm_request_src_port_ = declare_parameter("mrm_request_src_port"); + election_communication_src_ip_ = declare_parameter("election_communication_src_ip"); + election_communication_src_port_ = + declare_parameter("election_communication_src_port"); + election_status_src_ip_ = declare_parameter("election_status_src_ip"); + election_status_src_port_ = declare_parameter("election_status_src_port"); + + // convert udp packets of availability to topics + availability_converter_.setUdpSender(availability_dest_ip_, availability_dest_port_); + availability_converter_.setSubscriber(); + + // convert topics of mrm state to udp packets + mrm_converter_.setUdpSender(mrm_state_dest_ip_, mrm_state_dest_port_); + mrm_converter_.setSubscriber(); + + // convert udp packets of mrm request to topics + mrm_converter_.setPublisher(); + mrm_converter_.setUdpReceiver(mrm_request_src_ip_, mrm_request_src_port_); + + // convert udp packets of election info to topics + log_converter_.setPublisher(); + log_converter_.setUdpElectionCommunicationReceiver( + election_communication_src_ip_, election_communication_src_port_); + log_converter_.setUdpElectionStatusReceiver(election_status_src_ip_, election_status_src_port_); +} + +} // namespace leader_election_converter + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(leader_election_converter::LeaderElectionConverter) diff --git a/system/leader_election_converter/src/node/leader_election_converter.hpp b/system/leader_election_converter/src/node/leader_election_converter.hpp new file mode 100644 index 0000000000000..87381b2968097 --- /dev/null +++ b/system/leader_election_converter/src/node/leader_election_converter.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 The Autoware Contributors +// +// 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 NODE__LEADER_ELECTION_CONVERTER_HPP_ +#define NODE__LEADER_ELECTION_CONVERTER_HPP_ + +#include "availability_converter.hpp" +#include "log_converter.hpp" +#include "mrm_converter.hpp" + +#include + +#include +#include + +namespace leader_election_converter +{ + +class LeaderElectionConverter : public rclcpp::Node +{ +public: + explicit LeaderElectionConverter(const rclcpp::NodeOptions & node_options); + +private: + std::string availability_dest_ip_; + std::string availability_dest_port_; + std::string mrm_state_dest_ip_; + std::string mrm_state_dest_port_; + std::string mrm_request_src_ip_; + std::string mrm_request_src_port_; + std::string election_communication_src_ip_; + std::string election_communication_src_port_; + std::string election_status_src_ip_; + std::string election_status_src_port_; + + AvailabilityConverter availability_converter_; + MrmConverter mrm_converter_; + LogConverter log_converter_; +}; + +} // namespace leader_election_converter + +#endif // NODE__LEADER_ELECTION_CONVERTER_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..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..52538f184c313 --- /dev/null +++ b/system/mrm_stop_operator/launch/mrm_stop_operator.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/system/mrm_stop_operator/package.xml b/system/mrm_stop_operator/package.xml new file mode 100644 index 0000000000000..42f5268757afc --- /dev/null +++ b/system/mrm_stop_operator/package.xml @@ -0,0 +1,27 @@ + + + + mrm_stop_operator + 0.1.0 + The mrm_stop_operator package + Makoto Kurihara + Apache License 2.0 + + ament_cmake_auto + + autoware_cmake + + + autoware_auto_vehicle_msgs + 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..f7f39accaf262 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.cpp @@ -0,0 +1,129 @@ +// 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)); + + sub_velocity_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive, false); + rclcpp::SubscriptionOptions velocity_options = rclcpp::SubscriptionOptions(); + velocity_options.callback_group = sub_velocity_group_; + auto not_executed_callback = []([[maybe_unused]] const typename autoware_auto_vehicle_msgs::msg:: + VelocityReport::ConstSharedPtr msg) {}; + sub_velocity_ = create_subscription( + "~/input/velocity", 10, not_executed_callback, velocity_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_mrm_state_ = + create_publisher("~/output/mrm_state", rclcpp::QoS{1}); + + // Timer + const auto update_period_ns = rclcpp::Rate(10).period(); + timer_ = rclcpp::create_timer( + this, get_clock(), update_period_ns, std::bind(&MrmStopOperator::onTimer, this)); + + // 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; + current_mrm_state_.behavior = *msg; + current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_OPERATING; + } +} + +void MrmStopOperator::initState() +{ + last_mrm_request_.type = tier4_system_msgs::msg::MrmBehavior::NONE; + current_mrm_state_.state = tier4_system_msgs::msg::MrmState::NORMAL; + current_mrm_state_.behavior.type = tier4_system_msgs::msg::MrmBehavior::NONE; +} + +void MrmStopOperator::onTimer() +{ + if (current_mrm_state_.state == tier4_system_msgs::msg::MrmState::MRM_OPERATING) { + if (current_mrm_state_.behavior.type == tier4_system_msgs::msg::MrmBehavior::COMFORTABLE_STOP) { + if (isStopped()) { + current_mrm_state_.state = tier4_system_msgs::msg::MrmState::MRM_SUCCEEDED; + } else { + // nothing to do + } + } else { + // TODO + } + } + current_mrm_state_.stamp = this->now(); + pub_mrm_state_->publish(current_mrm_state_); +} + +bool MrmStopOperator::isStopped() +{ + constexpr auto th_stopped_velocity = 0.001; + auto current_velocity = std::make_shared(); + rclcpp::MessageInfo message_info; + + const bool success = sub_velocity_->take(*current_velocity, message_info); + if (success) { + return current_velocity->longitudinal_velocity < th_stopped_velocity; + } else { + return false; + } +} + +} // 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..2a28b66472a54 --- /dev/null +++ b/system/mrm_stop_operator/src/mrm_stop_operator.hpp @@ -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. + +#ifndef MRM_STOP_OPERATOR_HPP_ +#define MRM_STOP_OPERATOR_HPP_ + +// include +#include + +#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_; + rclcpp::Subscription::SharedPtr sub_velocity_; + + 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_; + rclcpp::Publisher::SharedPtr pub_mrm_state_; + // Service + + // Client + + // Timer + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::CallbackGroup::SharedPtr sub_velocity_group_; + + // State + tier4_system_msgs::msg::MrmBehavior last_mrm_request_; + tier4_system_msgs::msg::MrmState current_mrm_state_; + + void initState(); + void onTimer(); + bool isStopped(); + + // Diagnostics +}; +} // namespace mrm_stop_operator + +#endif // MRM_STOP_OPERATOR_HPP_