diff --git a/autoware_utils/README.md b/autoware_utils/README.md index bb4236a..40259ee 100644 --- a/autoware_utils/README.md +++ b/autoware_utils/README.md @@ -33,20 +33,10 @@ The geometry module provides classes and functions for handling 2D and 3D points The ROS module provides utilities for working with ROS messages and nodes: -- **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps. - **`diagnostics_interface.hpp`**: An interface for publishing diagnostic messages. - **`msg_covariance.hpp`**: Indices for accessing covariance matrices in ROS messages. - **`msg_operation.hpp`**: Overloaded operators for quaternion messages. -- **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages. -- **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published. - **`self_pose_listener.hpp`**: Listens to the self-pose of the vehicle. -- **`debug_traits.hpp`**: Traits for identifying debug message types. - -#### System Module - -The system module provides low-level utilities for performance monitoring and error handling: - -- **`time_keeper.hpp`**: Tracks and reports the processing time of various functions. ## Usage @@ -91,33 +81,6 @@ int main() { ### Detailed Usage Examples -#### Logging Processing Times with ProcessingTimePublisher - -```cpp -#include "autoware_utils/ros/processing_time_publisher.hpp" -#include -#include - -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("processing_time_node"); - - // Initialize ProcessingTimePublisher - autoware_utils::ProcessingTimePublisher processing_time_pub(node.get(), "~/debug/processing_time_ms"); - - // Simulate some processing times - std::map processing_times = { - {"node1", 0.1}, {"node2", 0.2}, {"node3", 0.3} - }; - - // Publish processing times - processing_time_pub.publish(processing_times); - - rclcpp::shutdown(); - return 0; -} -``` - #### Manipulating Polygons with boost_polygon_utils.hpp ```cpp @@ -146,26 +109,3 @@ int main(int argc, char * argv[]) { return 0; } ``` - -#### Handling Debug Message Types with debug_traits.hpp - -```cpp -#include "autoware_utils/ros/debug_publisher.hpp" -#include "autoware_utils/ros/debug_traits.hpp" -#include - -int main(int argc, char * argv[]) { - rclcpp::init(argc, argv); - auto node = rclcpp::Node::make_shared("debug_node"); - - // Initialize DebugPublisher - autoware_utils::DebugPublisher debug_pub(node, "/debug"); - - // Publish a debug message with custom type - float debug_data = 42.0; - debug_pub.publish("example", debug_data); - - rclcpp::shutdown(); - return 0; -} -``` diff --git a/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp b/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp index 8afdb55..2698f91 100644 --- a/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp +++ b/autoware_utils/include/autoware_utils/ros/debug_publisher.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 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. @@ -15,63 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ #define AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ -#include "autoware_utils/ros/debug_traits.hpp" +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include -#include -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_debug; } -#include -#include -#include - -namespace autoware_utils -{ -namespace debug_publisher -{ -template < - class T_msg, class T, - std::enable_if_t::value, std::nullptr_t> = - nullptr> -T_msg to_debug_msg(const T & data, const rclcpp::Time & stamp) -{ - T_msg msg; - msg.stamp = stamp; - msg.data = data; - return msg; -} -} // namespace debug_publisher - -class DebugPublisher -{ -public: - explicit DebugPublisher(rclcpp::Node * node, const char * ns) : node_(node), ns_(ns) {} - - template < - class T, - std::enable_if_t::value, std::nullptr_t> = nullptr> - void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) - { - if (pub_map_.count(name) == 0) { - pub_map_[name] = node_->create_publisher(std::string(ns_) + "/" + name, qos); - } - - std::dynamic_pointer_cast>(pub_map_.at(name))->publish(data); - } - - template < - class T_msg, class T, - std::enable_if_t::value, std::nullptr_t> = nullptr> - void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) - { - publish(name, debug_publisher::to_debug_msg(data, node_->now()), qos); - } - -private: - rclcpp::Node * node_; - const char * ns_; - std::unordered_map> pub_map_; -}; -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/debug_traits.hpp b/autoware_utils/include/autoware_utils/ros/debug_traits.hpp index b82194c..43b7d1c 100644 --- a/autoware_utils/include/autoware_utils/ros/debug_traits.hpp +++ b/autoware_utils/include/autoware_utils/ros/debug_traits.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 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. @@ -15,77 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ #define AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_debug; } -namespace autoware_utils::debug_traits -{ -template -struct is_debug_message : std::false_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message -: std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message -: std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; - -template <> -struct is_debug_message : std::true_type -{ -}; -} // namespace autoware_utils::debug_traits +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp b/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp index 2da2fa5..9172b25 100644 --- a/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp +++ b/autoware_utils/include/autoware_utils/ros/processing_time_publisher.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2025 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. @@ -15,53 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ #define AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_debug; } -#include -#include -#include - -namespace autoware_utils -{ -class ProcessingTimePublisher -{ -public: - explicit ProcessingTimePublisher( - rclcpp::Node * node, const std::string & name = "~/debug/processing_time_ms", - const rclcpp::QoS & qos = rclcpp::QoS(1)) - { - pub_processing_time_ = - node->create_publisher(name, qos); - } - - void publish(const std::map & processing_time_map) - { - diagnostic_msgs::msg::DiagnosticStatus status; - - for (const auto & m : processing_time_map) { - diagnostic_msgs::msg::KeyValue key_value; - key_value.key = m.first; - key_value.value = to_string_with_precision(m.second, 3); - status.values.push_back(key_value); - } - - pub_processing_time_->publish(status); - } - -private: - rclcpp::Publisher::SharedPtr pub_processing_time_; - - template - std::string to_string_with_precision(const T & value, const int precision) - { - std::ostringstream oss; - oss.precision(precision); - oss << std::fixed << value; - return oss.str(); - } -}; -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/autoware_utils/include/autoware_utils/ros/published_time_publisher.hpp b/autoware_utils/include/autoware_utils/ros/published_time_publisher.hpp index faf09e8..da33ad3 100644 --- a/autoware_utils/include/autoware_utils/ros/published_time_publisher.hpp +++ b/autoware_utils/include/autoware_utils/ros/published_time_publisher.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 The Autoware Contributors +// Copyright 2025 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. @@ -15,100 +15,14 @@ #ifndef AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ #define AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ -#include +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -#include -#include +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_debug; } -#include -#include -#include -#include -#include - -namespace autoware_utils -{ - -class PublishedTimePublisher -{ -public: - explicit PublishedTimePublisher( - rclcpp::Node * node, std::string publisher_topic_suffix = "/debug/published_time", - const rclcpp::QoS & qos = rclcpp::QoS(1)) - : node_(node), publisher_topic_suffix_(std::move(publisher_topic_suffix)), qos_(qos) - { - } - - void publish_if_subscribed( - const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) - { - const auto & gid_key = publisher->get_gid(); - - // if the publisher is not in the map, create a new publisher for published time - ensure_publisher_exists(gid_key, publisher->get_topic_name()); - - const auto & pub_published_time = publishers_[gid_key]; - - // Check if there are any subscribers, otherwise don't do anything - if (pub_published_time->get_subscription_count() > 0) { - PublishedTime published_time; - - published_time.header.stamp = stamp; - published_time.published_stamp = rclcpp::Clock().now(); - - pub_published_time->publish(published_time); - } - } - - void publish_if_subscribed( - const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) - { - const auto & gid_key = publisher->get_gid(); - - // if the publisher is not in the map, create a new publisher for published time - ensure_publisher_exists(gid_key, publisher->get_topic_name()); - - const auto & pub_published_time = publishers_[gid_key]; - - // Check if there are any subscribers, otherwise don't do anything - if (pub_published_time->get_subscription_count() > 0) { - PublishedTime published_time; - - published_time.header = header; - published_time.published_stamp = rclcpp::Clock().now(); - - pub_published_time->publish(published_time); - } - } - -private: - rclcpp::Node * node_; - std::string publisher_topic_suffix_; - rclcpp::QoS qos_; - - using PublishedTime = autoware_internal_msgs::msg::PublishedTime; - - // Custom comparison struct for rmw_gid_t - struct GidCompare - { - bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const - { - return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; - } - }; - - // ensure that the publisher exists in publisher_ map, if not, create a new one - void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name) - { - if (publishers_.find(gid_key) == publishers_.end()) { - publishers_[gid_key] = - node_->create_publisher(topic_name + publisher_topic_suffix_, qos_); - } - } - - // store them for each different publisher of the node - std::map::SharedPtr, GidCompare> publishers_; -}; -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__ROS__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/autoware_utils/include/autoware_utils/system/time_keeper.hpp b/autoware_utils/include/autoware_utils/system/time_keeper.hpp index 4bf665f..e99269c 100644 --- a/autoware_utils/include/autoware_utils/system/time_keeper.hpp +++ b/autoware_utils/include/autoware_utils/system/time_keeper.hpp @@ -1,4 +1,4 @@ -// Copyright 2024 TIER IV, Inc. +// Copyright 2025 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. @@ -11,204 +11,18 @@ // 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_UTILS__SYSTEM__TIME_KEEPER_HPP_ #define AUTOWARE_UTILS__SYSTEM__TIME_KEEPER_HPP_ -#include -#include - -#include -#include -#include - -#include -#include -#include -#include -#include - -namespace autoware_utils -{ -/** - * @brief Class representing a node in the time tracking tree - */ -class ProcessingTimeNode : public std::enable_shared_from_this -{ -public: - /** - * @brief Construct a new ProcessingTimeNode object - * - * @param name Name of the node - */ - explicit ProcessingTimeNode(const std::string & name); - - /** - * @brief Add a child node - * - * @param name Name of the child node - * @return std::shared_ptr Shared pointer to the newly added child node - */ - std::shared_ptr add_child(const std::string & name); - - /** - * @brief Get the result string representing the node and its children in a tree structure - * - * @return std::string Result string representing the node and its children - */ - std::string to_string() const; - - /** - * @brief Construct a ProcessingTimeTree message from the node and its children - * - * @return autoware_internal_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree - * message - */ - autoware_internal_debug_msgs::msg::ProcessingTimeTree to_msg() const; - - /** - * @brief Get the parent node - * - * @return std::weak_ptr Weak pointer to the parent node - */ - std::weak_ptr get_parent_node() const; - - /** - * @brief Get the child nodes - * - * @return std::vector> Vector of shared pointers to the child - * nodes - */ - std::vector> get_child_nodes() const; - - /** - * @brief Set the processing time for the node - * - * @param processing_time Processing time to be set - */ - void set_time(const double processing_time); - - /** - * @brief Set the comment for the node - * - * @param comment Comment to be set - */ - void set_comment(const std::string & comment); - - /** - * @brief Get the name of the node - * - * @return std::string Name of the node - */ - std::string get_name() const; - -private: - const std::string name_; //!< Name of the node - double processing_time_{0.0}; //!< Processing time of the node - std::string comment_; //!< Comment for the node - std::weak_ptr parent_node_; //!< Weak pointer to the parent node - std::vector> - child_nodes_; //!< Vector of shared pointers to the child nodes -}; - -using ProcessingTimeDetail = - autoware_internal_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree - //!< message - -/** - * @brief Class for tracking and reporting the processing time of various functions - */ -class TimeKeeper -{ -public: - template - explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) - { - reporters_.reserve(sizeof...(Reporters)); - (add_reporter(reporters), ...); - } - - /** - * @brief Add a reporter to output processing times to an ostream - * - * @param os Pointer to the ostream object - */ - void add_reporter(std::ostream * os); - - /** - * @brief Add a reporter to publish processing times to an rclcpp publisher - * - * @param publisher Shared pointer to the rclcpp publisher - */ - void add_reporter(rclcpp::Publisher::SharedPtr publisher); - - /** - * @brief Start tracking the processing time of a function - * - * @param func_name Name of the function to be tracked - */ - void start_track(const std::string & func_name); - - /** - * @brief End tracking the processing time of a function - * - * @param func_name Name of the function to end tracking - */ - void end_track(const std::string & func_name); - - /** - * @brief Comment the current time node - * - * @param comment Comment to be added to the current time node - */ - void comment(const std::string & comment); - -private: - /** - * @brief Report the processing times to all registered reporters - */ - void report(); - - std::shared_ptr - current_time_node_; //!< Shared pointer to the current time node - std::shared_ptr root_node_; //!< Shared pointer to the root time node - std::thread::id root_node_thread_id_; //!< ID of the thread that started the tracking - autoware_utils_system::StopWatch< - std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> - stop_watch_; //!< StopWatch object for tracking the processing time - - std::vector &)>> - reporters_; //!< Vector of functions for reporting the processing times -}; - -/** - * @brief Class for automatically tracking the processing time of a function within a scope - */ -class ScopedTimeTrack -{ -public: - /** - * @brief Construct a new ScopedTimeTrack object - * - * @param func_name Name of the function to be tracked - * @param time_keeper Reference to the TimeKeeper object - */ - ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); - - ScopedTimeTrack(const ScopedTimeTrack &) = delete; - ScopedTimeTrack & operator=(const ScopedTimeTrack &) = delete; - ScopedTimeTrack(ScopedTimeTrack &&) = delete; - ScopedTimeTrack & operator=(ScopedTimeTrack &&) = delete; - - /** - * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function - */ - ~ScopedTimeTrack(); +// NOLINTBEGIN(build/namespaces, whitespace/line_length) +// clang-format off -private: - const std::string func_name_; //!< Name of the function being tracked - TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object -}; +#pragma message("#include is deprecated. Use #include instead.") +#include +namespace autoware_utils { using namespace autoware_utils_debug; } -} // namespace autoware_utils +// clang-format on +// NOLINTEND #endif // AUTOWARE_UTILS__SYSTEM__TIME_KEEPER_HPP_ diff --git a/autoware_utils/package.xml b/autoware_utils/package.xml index ca76554..a16c3f1 100644 --- a/autoware_utils/package.xml +++ b/autoware_utils/package.xml @@ -19,6 +19,7 @@ autoware_internal_planning_msgs autoware_perception_msgs autoware_planning_msgs + autoware_utils_debug autoware_utils_geometry autoware_utils_logging autoware_utils_math diff --git a/autoware_utils_debug/CMakeLists.txt b/autoware_utils_debug/CMakeLists.txt new file mode 100644 index 0000000..a823eda --- /dev/null +++ b/autoware_utils_debug/CMakeLists.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.14) +project(autoware_utils_debug) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +ament_auto_add_library(${PROJECT_NAME} SHARED + "src/time_keeper.cpp" +) + +if(BUILD_TESTING) + file(GLOB_RECURSE test_files test/*.cpp) + ament_add_ros_isolated_gtest(test_${PROJECT_NAME} ${test_files}) + target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME}) +endif() + +ament_auto_package() diff --git a/autoware_utils_debug/README.md b/autoware_utils_debug/README.md new file mode 100644 index 0000000..326c2ef --- /dev/null +++ b/autoware_utils_debug/README.md @@ -0,0 +1,67 @@ +# autoware_utils_debug + +## Overview + +The **autoware_utils** library is a comprehensive toolkit designed to facilitate the development of autonomous driving applications. +This package provides essential utilities for debug. +It is extensively used in the Autoware project to handle common tasks such as publishing debug data and measuring time. + +## Design + +- **`debug_publisher.hpp`**: A helper class for publishing debug messages with timestamps. +- **`debug_traits.hpp`**: Traits for identifying debug message types. +- **`processing_time_publisher.hpp`**: Publishes processing times as diagnostic messages. +- **`published_time_publisher.hpp`**: Tracks and publishes the time when messages are published. +- **`time_keeper.hpp`**: Tracks and reports the processing time of various functions. + +### Example Code Snippets + +### Handling Debug Message Types with debug_traits.hpp + +```cpp +#include +#include +#include + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("debug_node"); + + // Initialize DebugPublisher + autoware_utils_debug::DebugPublisher debug_pub(node, "/debug"); + + // Publish a debug message with custom type + float debug_data = 42.0; + debug_pub.publish("example", debug_data); + + rclcpp::shutdown(); + return 0; +} +``` + +### Logging Processing Times with ProcessingTimePublisher + +```cpp +#include +#include +#include + +int main(int argc, char * argv[]) { + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("processing_time_node"); + + // Initialize ProcessingTimePublisher + autoware_utils_debug::ProcessingTimePublisher processing_time_pub(node.get(), "~/debug/processing_time_ms"); + + // Simulate some processing times + std::map processing_times = { + {"node1", 0.1}, {"node2", 0.2}, {"node3", 0.3} + }; + + // Publish processing times + processing_time_pub.publish(processing_times); + + rclcpp::shutdown(); + return 0; +} +``` diff --git a/autoware_utils_debug/include/autoware_utils_debug/debug_publisher.hpp b/autoware_utils_debug/include/autoware_utils_debug/debug_publisher.hpp new file mode 100644 index 0000000..290de97 --- /dev/null +++ b/autoware_utils_debug/include/autoware_utils_debug/debug_publisher.hpp @@ -0,0 +1,77 @@ +// Copyright 2020 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 AUTOWARE_UTILS_DEBUG__DEBUG_PUBLISHER_HPP_ +#define AUTOWARE_UTILS_DEBUG__DEBUG_PUBLISHER_HPP_ + +#include "autoware_utils_debug/debug_traits.hpp" + +#include +#include +#include + +#include +#include +#include + +namespace autoware_utils_debug +{ +namespace debug_publisher +{ +template < + class T_msg, class T, + std::enable_if_t< + autoware_utils_debug::debug_traits::is_debug_message::value, std::nullptr_t> = nullptr> +T_msg to_debug_msg(const T & data, const rclcpp::Time & stamp) +{ + T_msg msg; + msg.stamp = stamp; + msg.data = data; + return msg; +} +} // namespace debug_publisher + +class DebugPublisher +{ +public: + explicit DebugPublisher(rclcpp::Node * node, const char * ns) : node_(node), ns_(ns) {} + + template < + class T, + std::enable_if_t::value, std::nullptr_t> = nullptr> + void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + if (pub_map_.count(name) == 0) { + pub_map_[name] = node_->create_publisher(std::string(ns_) + "/" + name, qos); + } + + std::dynamic_pointer_cast>(pub_map_.at(name))->publish(data); + } + + template < + class T_msg, class T, + std::enable_if_t::value, std::nullptr_t> = nullptr> + void publish(const std::string & name, const T & data, const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + publish(name, debug_publisher::to_debug_msg(data, node_->now()), qos); + } + +private: + rclcpp::Node * node_; + const char * ns_; + std::unordered_map> pub_map_; +}; +} // namespace autoware_utils_debug + +#endif // AUTOWARE_UTILS_DEBUG__DEBUG_PUBLISHER_HPP_ diff --git a/autoware_utils_debug/include/autoware_utils_debug/debug_traits.hpp b/autoware_utils_debug/include/autoware_utils_debug/debug_traits.hpp new file mode 100644 index 0000000..6a02cb6 --- /dev/null +++ b/autoware_utils_debug/include/autoware_utils_debug/debug_traits.hpp @@ -0,0 +1,93 @@ +// Copyright 2020 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 AUTOWARE_UTILS_DEBUG__DEBUG_TRAITS_HPP_ +#define AUTOWARE_UTILS_DEBUG__DEBUG_TRAITS_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace autoware_utils_debug::debug_traits +{ + +template +struct is_debug_message : std::false_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message +: std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +template <> +struct is_debug_message : std::true_type +{ +}; + +} // namespace autoware_utils_debug::debug_traits + +#endif // AUTOWARE_UTILS_DEBUG__DEBUG_TRAITS_HPP_ diff --git a/autoware_utils_debug/include/autoware_utils_debug/processing_time_publisher.hpp b/autoware_utils_debug/include/autoware_utils_debug/processing_time_publisher.hpp new file mode 100644 index 0000000..732d016 --- /dev/null +++ b/autoware_utils_debug/include/autoware_utils_debug/processing_time_publisher.hpp @@ -0,0 +1,67 @@ +// Copyright 2020 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 AUTOWARE_UTILS_DEBUG__PROCESSING_TIME_PUBLISHER_HPP_ +#define AUTOWARE_UTILS_DEBUG__PROCESSING_TIME_PUBLISHER_HPP_ + +#include + +#include + +#include +#include +#include + +namespace autoware_utils_debug +{ +class ProcessingTimePublisher +{ +public: + explicit ProcessingTimePublisher( + rclcpp::Node * node, const std::string & name = "~/debug/processing_time_ms", + const rclcpp::QoS & qos = rclcpp::QoS(1)) + { + pub_processing_time_ = + node->create_publisher(name, qos); + } + + void publish(const std::map & processing_time_map) + { + diagnostic_msgs::msg::DiagnosticStatus status; + + for (const auto & m : processing_time_map) { + diagnostic_msgs::msg::KeyValue key_value; + key_value.key = m.first; + key_value.value = to_string_with_precision(m.second, 3); + status.values.push_back(key_value); + } + + pub_processing_time_->publish(status); + } + +private: + rclcpp::Publisher::SharedPtr pub_processing_time_; + + template + std::string to_string_with_precision(const T & value, const int precision) + { + std::ostringstream oss; + oss.precision(precision); + oss << std::fixed << value; + return oss.str(); + } +}; +} // namespace autoware_utils_debug + +#endif // AUTOWARE_UTILS_DEBUG__PROCESSING_TIME_PUBLISHER_HPP_ diff --git a/autoware_utils_debug/include/autoware_utils_debug/published_time_publisher.hpp b/autoware_utils_debug/include/autoware_utils_debug/published_time_publisher.hpp new file mode 100644 index 0000000..c6702b7 --- /dev/null +++ b/autoware_utils_debug/include/autoware_utils_debug/published_time_publisher.hpp @@ -0,0 +1,114 @@ +// 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_UTILS_DEBUG__PUBLISHED_TIME_PUBLISHER_HPP_ +#define AUTOWARE_UTILS_DEBUG__PUBLISHED_TIME_PUBLISHER_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_utils_debug +{ + +class PublishedTimePublisher +{ +public: + explicit PublishedTimePublisher( + rclcpp::Node * node, std::string publisher_topic_suffix = "/debug/published_time", + const rclcpp::QoS & qos = rclcpp::QoS(1)) + : node_(node), publisher_topic_suffix_(std::move(publisher_topic_suffix)), qos_(qos) + { + } + + void publish_if_subscribed( + const rclcpp::PublisherBase::ConstSharedPtr & publisher, const rclcpp::Time & stamp) + { + const auto & gid_key = publisher->get_gid(); + + // if the publisher is not in the map, create a new publisher for published time + ensure_publisher_exists(gid_key, publisher->get_topic_name()); + + const auto & pub_published_time = publishers_[gid_key]; + + // Check if there are any subscribers, otherwise don't do anything + if (pub_published_time->get_subscription_count() > 0) { + PublishedTime published_time; + + published_time.header.stamp = stamp; + published_time.published_stamp = rclcpp::Clock().now(); + + pub_published_time->publish(published_time); + } + } + + void publish_if_subscribed( + const rclcpp::PublisherBase::ConstSharedPtr & publisher, const std_msgs::msg::Header & header) + { + const auto & gid_key = publisher->get_gid(); + + // if the publisher is not in the map, create a new publisher for published time + ensure_publisher_exists(gid_key, publisher->get_topic_name()); + + const auto & pub_published_time = publishers_[gid_key]; + + // Check if there are any subscribers, otherwise don't do anything + if (pub_published_time->get_subscription_count() > 0) { + PublishedTime published_time; + + published_time.header = header; + published_time.published_stamp = rclcpp::Clock().now(); + + pub_published_time->publish(published_time); + } + } + +private: + rclcpp::Node * node_; + std::string publisher_topic_suffix_; + rclcpp::QoS qos_; + + using PublishedTime = autoware_internal_msgs::msg::PublishedTime; + + // Custom comparison struct for rmw_gid_t + struct GidCompare + { + bool operator()(const rmw_gid_t & lhs, const rmw_gid_t & rhs) const + { + return std::memcmp(lhs.data, rhs.data, RMW_GID_STORAGE_SIZE) < 0; + } + }; + + // ensure that the publisher exists in publisher_ map, if not, create a new one + void ensure_publisher_exists(const rmw_gid_t & gid_key, const std::string & topic_name) + { + if (publishers_.find(gid_key) == publishers_.end()) { + publishers_[gid_key] = + node_->create_publisher(topic_name + publisher_topic_suffix_, qos_); + } + } + + // store them for each different publisher of the node + std::map::SharedPtr, GidCompare> publishers_; +}; +} // namespace autoware_utils_debug + +#endif // AUTOWARE_UTILS_DEBUG__PUBLISHED_TIME_PUBLISHER_HPP_ diff --git a/autoware_utils_debug/include/autoware_utils_debug/time_keeper.hpp b/autoware_utils_debug/include/autoware_utils_debug/time_keeper.hpp new file mode 100644 index 0000000..444e73f --- /dev/null +++ b/autoware_utils_debug/include/autoware_utils_debug/time_keeper.hpp @@ -0,0 +1,215 @@ +// 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 AUTOWARE_UTILS_DEBUG__TIME_KEEPER_HPP_ +#define AUTOWARE_UTILS_DEBUG__TIME_KEEPER_HPP_ + +#include +#include + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace autoware_utils_debug +{ +/** + * @brief Class representing a node in the time tracking tree + */ +class ProcessingTimeNode : public std::enable_shared_from_this +{ +public: + /** + * @brief Construct a new ProcessingTimeNode object + * + * @param name Name of the node + */ + explicit ProcessingTimeNode(const std::string & name); + + /** + * @brief Add a child node + * + * @param name Name of the child node + * @return std::shared_ptr Shared pointer to the newly added child node + */ + std::shared_ptr add_child(const std::string & name); + + /** + * @brief Get the result string representing the node and its children in a tree structure + * + * @return std::string Result string representing the node and its children + */ + std::string to_string() const; + + /** + * @brief Construct a ProcessingTimeTree message from the node and its children + * + * @return autoware_internal_debug_msgs::msg::ProcessingTimeTree Constructed ProcessingTimeTree + * message + */ + autoware_internal_debug_msgs::msg::ProcessingTimeTree to_msg() const; + + /** + * @brief Get the parent node + * + * @return std::weak_ptr Weak pointer to the parent node + */ + std::weak_ptr get_parent_node() const; + + /** + * @brief Get the child nodes + * + * @return std::vector> Vector of shared pointers to the child + * nodes + */ + std::vector> get_child_nodes() const; + + /** + * @brief Set the processing time for the node + * + * @param processing_time Processing time to be set + */ + void set_time(const double processing_time); + + /** + * @brief Set the comment for the node + * + * @param comment Comment to be set + */ + void set_comment(const std::string & comment); + + /** + * @brief Get the name of the node + * + * @return std::string Name of the node + */ + std::string get_name() const; + +private: + const std::string name_; //!< Name of the node + double processing_time_{0.0}; //!< Processing time of the node + std::string comment_; //!< Comment for the node + std::weak_ptr parent_node_; //!< Weak pointer to the parent node + std::vector> + child_nodes_; //!< Vector of shared pointers to the child nodes +}; + +using ProcessingTimeDetail = + autoware_internal_debug_msgs::msg::ProcessingTimeTree; //!< Alias for the ProcessingTimeTree + //!< message + +/** + * @brief Class for tracking and reporting the processing time of various functions + */ +class TimeKeeper +{ +public: + template + explicit TimeKeeper(Reporters... reporters) : current_time_node_(nullptr), root_node_(nullptr) + { + reporters_.reserve(sizeof...(Reporters)); + (add_reporter(reporters), ...); + } + + /** + * @brief Add a reporter to output processing times to an ostream + * + * @param os Pointer to the ostream object + */ + void add_reporter(std::ostream * os); + + /** + * @brief Add a reporter to publish processing times to an rclcpp publisher + * + * @param publisher Shared pointer to the rclcpp publisher + */ + void add_reporter(rclcpp::Publisher::SharedPtr publisher); + + /** + * @brief Start tracking the processing time of a function + * + * @param func_name Name of the function to be tracked + */ + void start_track(const std::string & func_name); + + /** + * @brief End tracking the processing time of a function + * + * @param func_name Name of the function to end tracking + */ + void end_track(const std::string & func_name); + + /** + * @brief Comment the current time node + * + * @param comment Comment to be added to the current time node + */ + void comment(const std::string & comment); + +private: + /** + * @brief Report the processing times to all registered reporters + */ + void report(); + + std::shared_ptr + current_time_node_; //!< Shared pointer to the current time node + std::shared_ptr root_node_; //!< Shared pointer to the root time node + std::thread::id root_node_thread_id_; //!< ID of the thread that started the tracking + autoware_utils_system::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; //!< StopWatch object for tracking the processing time + + std::vector &)>> + reporters_; //!< Vector of functions for reporting the processing times +}; + +/** + * @brief Class for automatically tracking the processing time of a function within a scope + */ +class ScopedTimeTrack +{ +public: + /** + * @brief Construct a new ScopedTimeTrack object + * + * @param func_name Name of the function to be tracked + * @param time_keeper Reference to the TimeKeeper object + */ + ScopedTimeTrack(const std::string & func_name, TimeKeeper & time_keeper); + + ScopedTimeTrack(const ScopedTimeTrack &) = delete; + ScopedTimeTrack & operator=(const ScopedTimeTrack &) = delete; + ScopedTimeTrack(ScopedTimeTrack &&) = delete; + ScopedTimeTrack & operator=(ScopedTimeTrack &&) = delete; + + /** + * @brief Destroy the ScopedTimeTrack object, ending the tracking of the function + */ + ~ScopedTimeTrack(); + +private: + const std::string func_name_; //!< Name of the function being tracked + TimeKeeper & time_keeper_; //!< Reference to the TimeKeeper object +}; + +} // namespace autoware_utils_debug + +#endif // AUTOWARE_UTILS_DEBUG__TIME_KEEPER_HPP_ diff --git a/autoware_utils_debug/package.xml b/autoware_utils_debug/package.xml new file mode 100644 index 0000000..2aeb0ed --- /dev/null +++ b/autoware_utils_debug/package.xml @@ -0,0 +1,30 @@ + + + + autoware_utils_debug + 1.1.0 + The autoware_utils_debug package + Jian Kang + Ryohsuke Mitsudome + Esteve Fernandez + Yutaka Kondo + Takagi, Isamu + Apache License 2.0 + + ament_cmake_auto + autoware_cmake + + autoware_internal_debug_msgs + autoware_internal_msgs + autoware_utils_system + diagnostic_msgs + rclcpp + + ament_cmake_ros + ament_lint_auto + autoware_lint_common + + + ament_cmake + + diff --git a/autoware_utils/src/system/time_keeper.cpp b/autoware_utils_debug/src/time_keeper.cpp similarity index 98% rename from autoware_utils/src/system/time_keeper.cpp rename to autoware_utils_debug/src/time_keeper.cpp index 78d5e0e..deb869a 100644 --- a/autoware_utils/src/system/time_keeper.cpp +++ b/autoware_utils_debug/src/time_keeper.cpp @@ -12,7 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "autoware_utils/system/time_keeper.hpp" +#include "autoware_utils_debug/time_keeper.hpp" #include #include @@ -24,7 +24,7 @@ #include #include -namespace autoware_utils +namespace autoware_utils_debug { ProcessingTimeNode::ProcessingTimeNode(const std::string & name) : name_(name) @@ -202,4 +202,4 @@ ScopedTimeTrack::~ScopedTimeTrack() // NOLINT time_keeper_.end_track(func_name_); } -} // namespace autoware_utils +} // namespace autoware_utils_debug diff --git a/autoware_utils_debug/test/cases/debug_publisher.cpp b/autoware_utils_debug/test/cases/debug_publisher.cpp new file mode 100644 index 0000000..b039425 --- /dev/null +++ b/autoware_utils_debug/test/cases/debug_publisher.cpp @@ -0,0 +1,27 @@ +// Copyright 2025 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 "autoware_utils_debug/debug_publisher.hpp" + +#include + +#include + +using autoware_utils_debug::DebugPublisher; + +TEST(TestDebugPublisher, Instantiation) +{ + const auto node = std::make_shared("test_node"); + DebugPublisher(node.get(), "namespace"); +} diff --git a/autoware_utils_debug/test/cases/debug_traits.cpp b/autoware_utils_debug/test/cases/debug_traits.cpp new file mode 100644 index 0000000..efd49b8 --- /dev/null +++ b/autoware_utils_debug/test/cases/debug_traits.cpp @@ -0,0 +1,47 @@ +// Copyright 2025 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 "autoware_utils_debug/debug_traits.hpp" + +#include + +#include + +TEST(TestDebugTraits, TrueCases) +{ + using autoware_utils_debug::debug_traits::is_debug_message; + + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); + EXPECT_TRUE(is_debug_message::value); +} + +TEST(TestDebugTraits, FalseCases) +{ + using autoware_utils_debug::debug_traits::is_debug_message; + + EXPECT_FALSE(is_debug_message::value); + EXPECT_FALSE(is_debug_message::value); + EXPECT_FALSE(is_debug_message::value); + EXPECT_FALSE(is_debug_message::value); + EXPECT_FALSE(is_debug_message::value); + EXPECT_FALSE(is_debug_message::value); +} diff --git a/autoware_utils_debug/test/cases/processing_time_publisher.cpp b/autoware_utils_debug/test/cases/processing_time_publisher.cpp new file mode 100644 index 0000000..70512a6 --- /dev/null +++ b/autoware_utils_debug/test/cases/processing_time_publisher.cpp @@ -0,0 +1,27 @@ +// Copyright 2025 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 "autoware_utils_debug/processing_time_publisher.hpp" + +#include + +#include + +using autoware_utils_debug::ProcessingTimePublisher; + +TEST(TestProcessingTimePublisher, Instantiation) +{ + const auto node = std::make_shared("test_node"); + ProcessingTimePublisher(node.get()); +} diff --git a/autoware_utils_debug/test/cases/published_time_publisher.cpp b/autoware_utils_debug/test/cases/published_time_publisher.cpp new file mode 100644 index 0000000..8ff02d5 --- /dev/null +++ b/autoware_utils_debug/test/cases/published_time_publisher.cpp @@ -0,0 +1,27 @@ +// Copyright 2025 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 "autoware_utils_debug/published_time_publisher.hpp" + +#include + +#include + +using autoware_utils_debug::PublishedTimePublisher; + +TEST(TestPublishedTimePublisher, Instantiation) +{ + const auto node = std::make_shared("test_node"); + PublishedTimePublisher(node.get()); +} diff --git a/autoware_utils_debug/test/cases/time_keeper.cpp b/autoware_utils_debug/test/cases/time_keeper.cpp new file mode 100644 index 0000000..24bdf72 --- /dev/null +++ b/autoware_utils_debug/test/cases/time_keeper.cpp @@ -0,0 +1,26 @@ +// Copyright 2025 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 "autoware_utils_debug/time_keeper.hpp" + +#include + +using autoware_utils_debug::ScopedTimeTrack; +using autoware_utils_debug::TimeKeeper; + +TEST(TestTimeKeeper, Instantiation) +{ + TimeKeeper time_keeper; + ScopedTimeTrack st("test", time_keeper); +} diff --git a/autoware_utils_debug/test/main.cpp b/autoware_utils_debug/test/main.cpp new file mode 100644 index 0000000..42f409f --- /dev/null +++ b/autoware_utils_debug/test/main.cpp @@ -0,0 +1,36 @@ +// Copyright 2025 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 + +#include + +class RclcppEnvironment : public testing::Environment +{ +public: + RclcppEnvironment(int argc, char ** argv) : argc(argc), argv(argv) {} + void SetUp() override { rclcpp::init(argc, argv); } + void TearDown() override { rclcpp::shutdown(); } + +private: + int argc; + char ** argv; +}; + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + testing::AddGlobalTestEnvironment(new RclcppEnvironment(argc, argv)); + return RUN_ALL_TESTS(); +}