From 7b51a43ccf224c33a4f83021824d043824332bfc Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Fri, 24 May 2024 22:58:08 +0900 Subject: [PATCH] feat(default_ad_api): add heratbeat api (#6969) * feat(default_ad_api): add heratbeat api Signed-off-by: Takagi, Isamu * fix node dying Signed-off-by: Takagi, Isamu --------- Signed-off-by: Takagi, Isamu --- .../include/autoware_ad_api_specs/system.hpp | 36 ++++++++++++++++ system/default_ad_api/CMakeLists.txt | 2 + .../launch/default_ad_api.launch.py | 1 + system/default_ad_api/src/heartbeat.cpp | 42 +++++++++++++++++++ system/default_ad_api/src/heartbeat.hpp | 40 ++++++++++++++++++ 5 files changed, 121 insertions(+) create mode 100644 common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp create mode 100644 system/default_ad_api/src/heartbeat.cpp create mode 100644 system/default_ad_api/src/heartbeat.hpp diff --git a/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp new file mode 100644 index 0000000000000..09144c1d8ff50 --- /dev/null +++ b/common/autoware_ad_api_specs/include/autoware_ad_api_specs/system.hpp @@ -0,0 +1,36 @@ +// 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 AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ +#define AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ + +#include + +#include + +namespace autoware_ad_api::system +{ + +struct Heartbeat +{ + using Message = autoware_adapi_v1_msgs::msg::Heartbeat; + static constexpr char name[] = "/api/system/heartbeat"; + static constexpr size_t depth = 10; + static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT; + static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; +}; + +} // namespace autoware_ad_api::system + +#endif // AUTOWARE_AD_API_SPECS__SYSTEM_HPP_ diff --git a/system/default_ad_api/CMakeLists.txt b/system/default_ad_api/CMakeLists.txt index 982112189412e..4dacc0b893714 100644 --- a/system/default_ad_api/CMakeLists.txt +++ b/system/default_ad_api/CMakeLists.txt @@ -6,6 +6,7 @@ autoware_package() ament_auto_add_library(${PROJECT_NAME} SHARED src/fail_safe.cpp + src/heartbeat.cpp src/interface.cpp src/localization.cpp src/motion.cpp @@ -23,6 +24,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED rclcpp_components_register_nodes(${PROJECT_NAME} "default_ad_api::AutowareStateNode" "default_ad_api::FailSafeNode" + "default_ad_api::HeartbeatNode" "default_ad_api::InterfaceNode" "default_ad_api::LocalizationNode" "default_ad_api::MotionNode" diff --git a/system/default_ad_api/launch/default_ad_api.launch.py b/system/default_ad_api/launch/default_ad_api.launch.py index cf8fbe7d001bf..f4f3986c678e9 100644 --- a/system/default_ad_api/launch/default_ad_api.launch.py +++ b/system/default_ad_api/launch/default_ad_api.launch.py @@ -43,6 +43,7 @@ def generate_launch_description(): components = [ create_api_node("autoware_state", "AutowareStateNode"), create_api_node("fail_safe", "FailSafeNode"), + create_api_node("heartbeat", "HeartbeatNode"), create_api_node("interface", "InterfaceNode"), create_api_node("localization", "LocalizationNode"), create_api_node("motion", "MotionNode"), diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp new file mode 100644 index 0000000000000..4550302bb8bae --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,42 @@ +// 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 "heartbeat.hpp" + +#include + +namespace default_ad_api +{ + +HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) +{ + // Move this function so that the timer no longer holds it as a reference. + const auto on_timer = [this]() { + autoware_ad_api::system::Heartbeat::Message heartbeat; + heartbeat.stamp = now(); + heartbeat.seq = ++sequence_; // Wraps at 65535. + pub_->publish(heartbeat); + }; + + const auto adaptor = component_interface_utils::NodeAdaptor(this); + adaptor.init_pub(pub_); + + const auto period = rclcpp::Rate(10.0).period(); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); +} + +} // namespace default_ad_api + +#include +RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::HeartbeatNode) diff --git a/system/default_ad_api/src/heartbeat.hpp b/system/default_ad_api/src/heartbeat.hpp new file mode 100644 index 0000000000000..d922b88489639 --- /dev/null +++ b/system/default_ad_api/src/heartbeat.hpp @@ -0,0 +1,40 @@ +// 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 HEARTBEAT_HPP_ +#define HEARTBEAT_HPP_ + +#include +#include + +// This file should be included after messages. +#include "utils/types.hpp" + +namespace default_ad_api +{ + +class HeartbeatNode : public rclcpp::Node +{ +public: + explicit HeartbeatNode(const rclcpp::NodeOptions & options); + +private: + rclcpp::TimerBase::SharedPtr timer_; + Pub pub_; + uint16_t sequence_ = 0; +}; + +} // namespace default_ad_api + +#endif // HEARTBEAT_HPP_