From eb60bb153e8903a0661d0e875a4a80a5e2e2e8b7 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 8 May 2024 21:02:25 +0900 Subject: [PATCH 1/2] feat(default_ad_api): add heratbeat api 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 | 41 +++++++++++++++++++ system/default_ad_api/src/heartbeat.hpp | 40 ++++++++++++++++++ 5 files changed, 120 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 000000000000..09144c1d8ff5 --- /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 982112189412..4dacc0b89371 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 cf8fbe7d001b..f4f3986c678e 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 000000000000..74766e24940c --- /dev/null +++ b/system/default_ad_api/src/heartbeat.cpp @@ -0,0 +1,41 @@ +// 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) +{ + 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, 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 000000000000..d922b8848963 --- /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_ From b3161cbc1526c57a8340fbb073502214901b1997 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" Date: Wed, 15 May 2024 18:40:48 +0900 Subject: [PATCH 2/2] fix node dying Signed-off-by: Takagi, Isamu --- system/default_ad_api/src/heartbeat.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/system/default_ad_api/src/heartbeat.cpp b/system/default_ad_api/src/heartbeat.cpp index 74766e24940c..4550302bb8ba 100644 --- a/system/default_ad_api/src/heartbeat.cpp +++ b/system/default_ad_api/src/heartbeat.cpp @@ -21,7 +21,8 @@ namespace default_ad_api HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartbeat", options) { - auto on_timer = [this]() { + // 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. @@ -32,7 +33,7 @@ HeartbeatNode::HeartbeatNode(const rclcpp::NodeOptions & options) : Node("heartb adaptor.init_pub(pub_); const auto period = rclcpp::Rate(10.0).period(); - timer_ = rclcpp::create_timer(this, get_clock(), period, on_timer); + timer_ = rclcpp::create_timer(this, get_clock(), period, std::move(on_timer)); } } // namespace default_ad_api