Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(autoware_utils_debug): split package #44

60 changes: 0 additions & 60 deletions autoware_utils/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -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

Expand Down Expand Up @@ -91,33 +81,6 @@ int main() {

### Detailed Usage Examples

#### Logging Processing Times with ProcessingTimePublisher

```cpp
#include "autoware_utils/ros/processing_time_publisher.hpp"
#include <rclcpp/rclcpp.hpp>
#include <map>

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<std::string, double> 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
Expand Down Expand Up @@ -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 <rclcpp/rclcpp.hpp>

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<autoware_internal_debug_msgs::msg::Float32Stamped>("example", debug_data);

rclcpp::shutdown();
return 0;
}
```
65 changes: 8 additions & 57 deletions autoware_utils/include/autoware_utils/ros/debug_publisher.hpp
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -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 <rclcpp/publisher_base.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rosidl_runtime_cpp/traits.hpp>
#pragma message("#include <autoware_utils/ros/debug_publisher.hpp> is deprecated. Use #include <autoware_utils_debug/debug_publisher.hpp> instead.")
#include <autoware_utils_debug/debug_publisher.hpp>
namespace autoware_utils { using namespace autoware_utils_debug; }

#include <memory>
#include <string>
#include <unordered_map>

namespace autoware_utils
{
namespace debug_publisher
{
template <
class T_msg, class T,
std::enable_if_t<autoware_utils::debug_traits::is_debug_message<T_msg>::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<rosidl_generator_traits::is_message<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<T>(std::string(ns_) + "/" + name, qos);
}

std::dynamic_pointer_cast<rclcpp::Publisher<T>>(pub_map_.at(name))->publish(data);
}

template <
class T_msg, class T,
std::enable_if_t<!rosidl_generator_traits::is_message<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<T_msg>(data, node_->now()), qos);
}

private:
rclcpp::Node * node_;
const char * ns_;
std::unordered_map<std::string, std::shared_ptr<rclcpp::PublisherBase>> pub_map_;
};
} // namespace autoware_utils
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__ROS__DEBUG_PUBLISHER_HPP_
79 changes: 8 additions & 71 deletions autoware_utils/include/autoware_utils/ros/debug_traits.hpp
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -15,77 +15,14 @@
#ifndef AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_
#define AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_

#include <autoware_internal_debug_msgs/msg/bool_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/float32_multi_array_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/float32_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/float64_multi_array_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/float64_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/int32_multi_array_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/int32_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/int64_multi_array_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/int64_stamped.hpp>
#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

#include <type_traits>
#pragma message("#include <autoware_utils/ros/debug_traits.hpp> is deprecated. Use #include <autoware_utils_debug/debug_traits.hpp> instead.")
#include <autoware_utils_debug/debug_traits.hpp>
namespace autoware_utils { using namespace autoware_utils_debug; }

namespace autoware_utils::debug_traits
{
template <typename T>
struct is_debug_message : std::false_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::BoolStamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Float32MultiArrayStamped>
: std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Float32Stamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Float64MultiArrayStamped>
: std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Float64Stamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Int32MultiArrayStamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Int32Stamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Int64MultiArrayStamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::Int64Stamped> : std::true_type
{
};

template <>
struct is_debug_message<autoware_internal_debug_msgs::msg::StringStamped> : std::true_type
{
};
} // namespace autoware_utils::debug_traits
// clang-format on
// NOLINTEND

#endif // AUTOWARE_UTILS__ROS__DEBUG_TRAITS_HPP_
Original file line number Diff line number Diff line change
@@ -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.
Expand All @@ -15,53 +15,14 @@
#ifndef AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_
#define AUTOWARE_UTILS__ROS__PROCESSING_TIME_PUBLISHER_HPP_

#include <rclcpp/rclcpp.hpp>
// NOLINTBEGIN(build/namespaces, whitespace/line_length)
// clang-format off

#include <diagnostic_msgs/msg/diagnostic_status.hpp>
#pragma message("#include <autoware_utils/ros/processing_time_publisher.hpp> is deprecated. Use #include <autoware_utils_debug/processing_time_publisher.hpp> instead.")
#include <autoware_utils_debug/processing_time_publisher.hpp>
namespace autoware_utils { using namespace autoware_utils_debug; }

#include <map>
#include <sstream>
#include <string>

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<diagnostic_msgs::msg::DiagnosticStatus>(name, qos);
}

void publish(const std::map<std::string, double> & 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<diagnostic_msgs::msg::DiagnosticStatus>::SharedPtr pub_processing_time_;

template <class T>
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_
Loading