From da6cebb0dfc4a916cb78fb828a16963ba9590da4 Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Thu, 4 Jul 2024 21:11:17 +0900 Subject: [PATCH 1/3] feat(control_cmd_switcher): mrm v0.6 add control cmd switcher based on x2 v3.0.0 (#1385) * feat: add control cmd switcher node Signed-off-by: TetsuKawa * fix: typo Signed-off-by: TetsuKawa * style(pre-commit): autofix --------- Signed-off-by: TetsuKawa Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- system/control_cmd_switcher/CMakeLists.txt | 25 ++++++ system/control_cmd_switcher/README.md | 1 + .../config/control_cmd_switcher.yaml | 4 + .../launch/control_cmd_switcher.launch.xml | 18 +++++ system/control_cmd_switcher/package.xml | 25 ++++++ .../control_cmd_switcher.cpp | 80 +++++++++++++++++++ .../control_cmd_switcher.hpp | 56 +++++++++++++ .../tool/relay_trajectory.py | 50 ++++++++++++ 8 files changed, 259 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 create mode 100755 system/control_cmd_switcher/tool/relay_trajectory.py 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() From 0e00824ff0d9b14a3af254d66141edea0ed2ce0f Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Thu, 4 Jul 2024 21:11:40 +0900 Subject: [PATCH 2/3] feat(dummy_operation_mode_publisher): add dummy operation mode publisher (#1386) * feat(dummy_operation_mode_publisher): add dummy operation mode publisher Signed-off-by: Makoto Kurihara * style(pre-commit): autofix --------- Signed-off-by: Makoto Kurihara Co-authored-by: Makoto Kurihara Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../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 | 56 +++++++++++++++++ 7 files changed, 197 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..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_ From 9ce3db61172860cea10ee85a40a8e7ea804a020d Mon Sep 17 00:00:00 2001 From: TetsuKawa <70682030+TetsuKawa@users.noreply.github.com> Date: Thu, 4 Jul 2024 21:12:10 +0900 Subject: [PATCH 3/3] feat(trajectory_repeater): mrm v0.6 add trajectory repeater based on x2 v3.0.0 (#1390) * feat(trajectory_repeater): add trajectory repeater Signed-off-by: Makoto Kurihara * chore(trajectory_repeater): change remap future work: parameterize Signed-off-by: Makoto Kurihara --------- Signed-off-by: Makoto Kurihara Co-authored-by: Makoto Kurihara --- planning/trajectory_repeater/CMakeLists.txt | 21 ++++++ planning/trajectory_repeater/README.md | 23 +++++++ .../config/trajectory_repeater.param.yaml | 2 + .../launch/trajectory_repeater.launch.xml | 8 +++ planning/trajectory_repeater/package.xml | 25 ++++++++ .../src/trajectory_repeater.cpp | 64 +++++++++++++++++++ .../src/trajectory_repeater.hpp | 60 +++++++++++++++++ 7 files changed, 203 insertions(+) create mode 100644 planning/trajectory_repeater/CMakeLists.txt create mode 100644 planning/trajectory_repeater/README.md create mode 100644 planning/trajectory_repeater/config/trajectory_repeater.param.yaml create mode 100644 planning/trajectory_repeater/launch/trajectory_repeater.launch.xml create mode 100644 planning/trajectory_repeater/package.xml create mode 100644 planning/trajectory_repeater/src/trajectory_repeater.cpp create mode 100644 planning/trajectory_repeater/src/trajectory_repeater.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