From 1bde107824bfc21cd8af8d2574ebef9b91808628 Mon Sep 17 00:00:00 2001 From: Max SCHMELLER Date: Tue, 8 Oct 2024 18:55:30 +0900 Subject: [PATCH] feat: remove MtQueue, add debug publishers for decode latency Signed-off-by: Max SCHMELLER --- .../hesai_hw_interface.hpp | 2 +- nebula_ros/CMakeLists.txt | 3 + .../include/nebula_ros/common/counters.hpp | 65 ++++++++++++++++++ .../nebula_ros/common/debug_publisher.hpp | 68 +++++++++++++++++++ .../nebula_ros/hesai/decoder_wrapper.hpp | 4 +- .../nebula_ros/hesai/hesai_ros_wrapper.hpp | 9 ++- nebula_ros/package.xml | 1 + nebula_ros/src/hesai/decoder_wrapper.cpp | 25 ++++++- nebula_ros/src/hesai/hesai_ros_wrapper.cpp | 23 ++++--- proc_net_udp_to_csv.py | 57 ++++++++++++++++ tier4_debug_msgs/CMakeLists.txt | 43 ++++++++++++ tier4_debug_msgs/msg/BoolStamped.msg | 2 + .../msg/Float32MultiArrayStamped.msg | 3 + tier4_debug_msgs/msg/Float32Stamped.msg | 2 + .../msg/Float64MultiArrayStamped.msg | 3 + tier4_debug_msgs/msg/Float64Stamped.msg | 2 + .../msg/Int32MultiArrayStamped.msg | 3 + tier4_debug_msgs/msg/Int32Stamped.msg | 2 + .../msg/Int64MultiArrayStamped.msg | 3 + tier4_debug_msgs/msg/Int64Stamped.msg | 2 + tier4_debug_msgs/msg/MultiArrayDimension.msg | 11 +++ tier4_debug_msgs/msg/MultiArrayLayout.msg | 34 ++++++++++ tier4_debug_msgs/msg/ProcessingTimeNode.msg | 10 +++ tier4_debug_msgs/msg/ProcessingTimeTree.msg | 2 + tier4_debug_msgs/msg/StringStamped.msg | 2 + tier4_debug_msgs/msg/SystemUsage.msg | 4 ++ tier4_debug_msgs/msg/SystemUsageArray.msg | 3 + tier4_debug_msgs/package.xml | 26 +++++++ 28 files changed, 393 insertions(+), 21 deletions(-) create mode 100644 nebula_ros/include/nebula_ros/common/counters.hpp create mode 100644 nebula_ros/include/nebula_ros/common/debug_publisher.hpp create mode 100755 proc_net_udp_to_csv.py create mode 100644 tier4_debug_msgs/CMakeLists.txt create mode 100644 tier4_debug_msgs/msg/BoolStamped.msg create mode 100644 tier4_debug_msgs/msg/Float32MultiArrayStamped.msg create mode 100644 tier4_debug_msgs/msg/Float32Stamped.msg create mode 100644 tier4_debug_msgs/msg/Float64MultiArrayStamped.msg create mode 100644 tier4_debug_msgs/msg/Float64Stamped.msg create mode 100644 tier4_debug_msgs/msg/Int32MultiArrayStamped.msg create mode 100644 tier4_debug_msgs/msg/Int32Stamped.msg create mode 100644 tier4_debug_msgs/msg/Int64MultiArrayStamped.msg create mode 100644 tier4_debug_msgs/msg/Int64Stamped.msg create mode 100644 tier4_debug_msgs/msg/MultiArrayDimension.msg create mode 100644 tier4_debug_msgs/msg/MultiArrayLayout.msg create mode 100644 tier4_debug_msgs/msg/ProcessingTimeNode.msg create mode 100644 tier4_debug_msgs/msg/ProcessingTimeTree.msg create mode 100644 tier4_debug_msgs/msg/StringStamped.msg create mode 100644 tier4_debug_msgs/msg/SystemUsage.msg create mode 100644 tier4_debug_msgs/msg/SystemUsageArray.msg create mode 100644 tier4_debug_msgs/package.xml diff --git a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp index e3031fcf4..4e7247cff 100644 --- a/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp +++ b/nebula_hw_interfaces/include/nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp @@ -107,7 +107,7 @@ const uint16_t MTU_SIZE = 1500; /// small to bridge scheduling and processing delays, packets will be dropped. This corresponds to /// the net.core.rmem_default setting in Linux. The current value is hardcoded to accommodate one /// pointcloud worth of OT128 packets (currently the highest data rate sensor supported). -const size_t UDP_SOCKET_BUFFER_SIZE = MTU_SIZE * 3600; +const size_t UDP_SOCKET_BUFFER_SIZE = 0x10000000; // 250 MiB // Time interval between Announce messages, in units of log seconds (default: 1) const int PTP_LOG_ANNOUNCE_INTERVAL = 1; diff --git a/nebula_ros/CMakeLists.txt b/nebula_ros/CMakeLists.txt index 709de2025..8f2dae062 100644 --- a/nebula_ros/CMakeLists.txt +++ b/nebula_ros/CMakeLists.txt @@ -28,6 +28,7 @@ find_package(robosense_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(visualization_msgs REQUIRED) find_package(yaml-cpp REQUIRED) +find_package(tier4_debug_msgs REQUIRED) include_directories( include @@ -36,12 +37,14 @@ include_directories( ${YAML_CPP_INCLUDE_DIRS} ${PCL_INCLUDE_DIRS} ${rclcpp_components_INCLUDE_DIRS} + ${tier4_debug_msgs_INCLUDE_DIRS} ) link_libraries( ${nebula_common_TARGETS} ${YAML_CPP_LIBRARIES} ${PCL_LIBRARIES} + ${tier4_debug_msgs_TARGETS} ) ## Hesai diff --git a/nebula_ros/include/nebula_ros/common/counters.hpp b/nebula_ros/include/nebula_ros/common/counters.hpp new file mode 100644 index 000000000..991d753a1 --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/counters.hpp @@ -0,0 +1,65 @@ +// 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. + +#pragma once + +#include "nebula_ros/common/debug_publisher.hpp" + +#include + +#include +#include +#include +#include +namespace nebula::ros +{ + +class Counter +{ +public: + using clock = std::chrono::steady_clock; + + explicit Counter(rclcpp::Node * node) : debug_pub_(node, node->get_fully_qualified_name()) {} + + void contribute(const std::string & key, clock::duration duration) + { + measurements_.try_emplace(key, 0); + measurements_[key] += duration; + } + + bool try_publish(const std::string & key) + { + if (measurements_.find(key) == measurements_.end()) { + return false; + } + + debug_pub_.publish(key, measurements_[key].count()); + measurements_.erase(key); + return true; + } + + void publish_all() + { + for (const auto & [key, value] : measurements_) { + debug_pub_.publish(key, value.count()); + } + measurements_.clear(); + } + +private: + autoware::universe_utils::DebugPublisher debug_pub_; + std::map measurements_; +}; + +} // namespace nebula::ros diff --git a/nebula_ros/include/nebula_ros/common/debug_publisher.hpp b/nebula_ros/include/nebula_ros/common/debug_publisher.hpp new file mode 100644 index 000000000..4c1bd6baa --- /dev/null +++ b/nebula_ros/include/nebula_ros/common/debug_publisher.hpp @@ -0,0 +1,68 @@ +// 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__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ +#define AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ + +#include +#include +#include + +#include +#include +#include + +namespace autoware::universe_utils +{ +namespace debug_publisher +{ +template +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 + 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 + 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::universe_utils + +#endif // AUTOWARE__UNIVERSE_UTILS__ROS__DEBUG_PUBLISHER_HPP_ diff --git a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp index edd3b642e..15df3b431 100644 --- a/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/decoder_wrapper.hpp @@ -16,6 +16,7 @@ #include "nebula_decoders/nebula_decoders_hesai/hesai_driver.hpp" #include "nebula_hw_interfaces/nebula_hw_interfaces_hesai/hesai_hw_interface.hpp" +#include "nebula_ros/common/counters.hpp" #include "nebula_ros/common/watchdog_timer.hpp" #include @@ -39,7 +40,8 @@ class HesaiDecoderWrapper const std::shared_ptr & calibration, bool publish_packets); - void process_cloud_packet(std::unique_ptr packet_msg); + bool process_cloud_packet( + std::unique_ptr packet_msg, Counter & counter); void on_config_change( const std::shared_ptr & new_config); diff --git a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp index f8d221c3f..75f54919c 100644 --- a/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp +++ b/nebula_ros/include/nebula_ros/hesai/hesai_ros_wrapper.hpp @@ -17,6 +17,8 @@ #include "nebula_common/hesai/hesai_common.hpp" #include "nebula_common/nebula_common.hpp" #include "nebula_common/nebula_status.hpp" +#include "nebula_ros/common/counters.hpp" +#include "nebula_ros/common/debug_publisher.hpp" #include "nebula_ros/common/mt_queue.hpp" #include "nebula_ros/hesai/decoder_wrapper.hpp" #include "nebula_ros/hesai/hw_interface_wrapper.hpp" @@ -94,15 +96,12 @@ class HesaiRosWrapper final : public rclcpp::Node get_calibration_result_t get_calibration_data( const std::string & calibration_file_path, bool ignore_others = false); + Counter counter_; + Status wrapper_status_; std::shared_ptr sensor_cfg_ptr_{}; - /// @brief Stores received packets that have not been processed yet by the decoder thread - MtQueue> packet_queue_; - /// @brief Thread to isolate decoding from receiving - std::thread decoder_thread_; - rclcpp::Subscription::SharedPtr packets_sub_{}; bool launch_hw_; diff --git a/nebula_ros/package.xml b/nebula_ros/package.xml index 3a97db1f6..a936aa68a 100644 --- a/nebula_ros/package.xml +++ b/nebula_ros/package.xml @@ -28,6 +28,7 @@ rclcpp_components robosense_msgs tf2_ros + tier4_debug_msgs velodyne_msgs visualization_msgs yaml-cpp diff --git a/nebula_ros/src/hesai/decoder_wrapper.cpp b/nebula_ros/src/hesai/decoder_wrapper.cpp index e3d726fd9..c5f0da49e 100644 --- a/nebula_ros/src/hesai/decoder_wrapper.cpp +++ b/nebula_ros/src/hesai/decoder_wrapper.cpp @@ -2,10 +2,13 @@ #include "nebula_ros/hesai/decoder_wrapper.hpp" +#include "nebula_ros/common/counters.hpp" + #include #include #include +#include #include #pragma clang diagnostic ignored "-Wbitwise-instead-of-logical" @@ -92,9 +95,11 @@ void HesaiDecoderWrapper::on_calibration_change( calibration_cfg_ptr_ = new_calibration; } -void HesaiDecoderWrapper::process_cloud_packet( - std::unique_ptr packet_msg) +bool HesaiDecoderWrapper::process_cloud_packet( + std::unique_ptr packet_msg, Counter & counter) { + auto t1 = std::chrono::steady_clock::now(); + // Accumulate packets for recording only if someone is subscribed to the topic (for performance) if ( packets_pub_ && (packets_pub_->get_subscription_count() > 0 || @@ -110,6 +115,9 @@ void HesaiDecoderWrapper::process_cloud_packet( current_scan_msg_->packets.emplace_back(std::move(pandar_packet_msg)); } + auto t2 = std::chrono::steady_clock::now(); + counter.contribute("copy_into_scanmsg_ns", t2 - t1); + std::tuple pointcloud_ts{}; nebula::drivers::NebulaPointCloudPtr pointcloud = nullptr; { @@ -118,13 +126,16 @@ void HesaiDecoderWrapper::process_cloud_packet( pointcloud = std::get<0>(pointcloud_ts); } + auto t3 = std::chrono::steady_clock::now(); + counter.contribute("decode_ns", t3 - t2); + // A pointcloud is only emitted when a scan completes (e.g. 3599 packets do not emit, the 3600th // emits one) if (pointcloud == nullptr) { // Since this ends the function early, the `cloud_watchdog_` will not be updated. // Thus, if pointclouds are not emitted for too long (e.g. when decoder settings are wrong or no // packets come in), the watchdog will log a warning automatically - return; + return false; } cloud_watchdog_->update(); @@ -135,6 +146,9 @@ void HesaiDecoderWrapper::process_cloud_packet( current_scan_msg_ = std::make_unique(); } + auto t4 = std::chrono::steady_clock::now(); + counter.contribute("publish_packets_ns", t4 - t3); + if ( nebula_points_pub_->get_subscription_count() > 0 || nebula_points_pub_->get_intra_process_subscription_count() > 0) { @@ -166,6 +180,11 @@ void HesaiDecoderWrapper::process_cloud_packet( rclcpp::Time(seconds_to_chrono_nano_seconds(std::get<1>(pointcloud_ts)).count()); publish_cloud(std::move(ros_pc_msg_ptr), aw_points_ex_pub_); } + + auto t5 = std::chrono::steady_clock::now(); + counter.contribute("publish_points_ns", t5 - t4); + + return true; } void HesaiDecoderWrapper::publish_cloud( diff --git a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp index 756e7dd44..d8586d0e1 100644 --- a/nebula_ros/src/hesai/hesai_ros_wrapper.cpp +++ b/nebula_ros/src/hesai/hesai_ros_wrapper.cpp @@ -2,12 +2,15 @@ #include "nebula_ros/hesai/hesai_ros_wrapper.hpp" +#include "nebula_ros/common/counters.hpp" +#include "nebula_ros/common/debug_publisher.hpp" #include "nebula_ros/common/parameter_descriptors.hpp" #include #include #include +#include #include #include #include @@ -21,9 +24,9 @@ namespace nebula::ros { HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) : rclcpp::Node("hesai_ros_wrapper", rclcpp::NodeOptions(options).use_intra_process_comms(true)), + counter_(this), wrapper_status_(Status::NOT_INITIALIZED), sensor_cfg_ptr_(nullptr), - packet_queue_(3000), hw_interface_wrapper_(), hw_monitor_wrapper_(), decoder_wrapper_() @@ -67,12 +70,6 @@ HesaiRosWrapper::HesaiRosWrapper(const rclcpp::NodeOptions & options) RCLCPP_DEBUG(get_logger(), "Starting stream"); - decoder_thread_ = std::thread([this]() { - while (true) { - decoder_wrapper_->process_cloud_packet(packet_queue_.pop()); - } - }); - if (launch_hw_) { hw_interface_wrapper_->hw_interface()->RegisterScanCallback( std::bind(&HesaiRosWrapper::receive_cloud_packet_callback, this, std::placeholders::_1)); @@ -273,8 +270,7 @@ void HesaiRosWrapper::receive_scan_message_callback( auto nebula_pkt_ptr = std::make_unique(); nebula_pkt_ptr->stamp = pkt.stamp; std::copy(pkt.data.begin(), pkt.data.end(), std::back_inserter(nebula_pkt_ptr->data)); - - packet_queue_.push(std::move(nebula_pkt_ptr)); + decoder_wrapper_->process_cloud_packet(std::move(nebula_pkt_ptr), counter_); } } @@ -400,6 +396,8 @@ rcl_interfaces::msg::SetParametersResult HesaiRosWrapper::on_parameter_change( void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packet) { + auto perf_start = std::chrono::steady_clock::now(); + if (!decoder_wrapper_ || decoder_wrapper_->status() != Status::OK) { return; } @@ -412,9 +410,12 @@ void HesaiRosWrapper::receive_cloud_packet_callback(std::vector & packe msg_ptr->stamp.sec = static_cast(timestamp_ns / 1'000'000'000); msg_ptr->stamp.nanosec = static_cast(timestamp_ns % 1'000'000'000); msg_ptr->data.swap(packet); + bool published = decoder_wrapper_->process_cloud_packet(std::move(msg_ptr), counter_); - if (!packet_queue_.try_push(std::move(msg_ptr))) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 500, "Packet(s) dropped"); + auto perf_stop = std::chrono::steady_clock::now(); + counter_.contribute("total_ns", perf_stop - perf_start); + if (published) { + counter_.publish_all(); } } diff --git a/proc_net_udp_to_csv.py b/proc_net_udp_to_csv.py new file mode 100755 index 000000000..0be3bbb00 --- /dev/null +++ b/proc_net_udp_to_csv.py @@ -0,0 +1,57 @@ +#!/usr/bin/python3 + +import os +import re +import sys + +COLUMNS = [ + "sl", + "local_address", + "local_port", + "rem_address", + "rem_port", + "st", + "tx_queue", + "rx_queue", + "tr", + "tm->when", + "retrnsmt", + "uid", + "timeout", + "inode", + "ref", + "pointer", + "drops", +] + + +def convert_line(line: str): + split = re.split(r"[:\s]+", line.strip()) + if len(split) != len(COLUMNS): + raise ValueError("Length mismatch") + split = [str(int(value, 16)) for value in split] + return split + + +def convert(src_filename: str, dst_filename: str): + with open(src_filename, "r") as f: + lines = f.readlines() + + rows = [convert_line(line) for line in lines] + + with open(dst_filename, "w") as f: + f.write(",".join(COLUMNS) + "\n") + for row in rows: + f.write(",".join(row) + "\n") + + +if __name__ == "__main__": + args = sys.argv + if len(args) == 1: + print(f"Usage: {args[0]} [file1 [file2 ...]]") + sys.exit(1) + + files_to_process = args[1:] + for filename in files_to_process: + out_filename = os.path.splitext(filename)[0] + ".csv" + convert(filename, out_filename) diff --git a/tier4_debug_msgs/CMakeLists.txt b/tier4_debug_msgs/CMakeLists.txt new file mode 100644 index 000000000..dff0fc613 --- /dev/null +++ b/tier4_debug_msgs/CMakeLists.txt @@ -0,0 +1,43 @@ +cmake_minimum_required(VERSION 3.5) +project(tier4_debug_msgs) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD_REQUIRED ON) + set(CMAKE_CXX_EXTENSIONS OFF) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/MultiArrayDimension.msg" + "msg/MultiArrayLayout.msg" + "msg/BoolStamped.msg" + "msg/Float32Stamped.msg" + "msg/Float32MultiArrayStamped.msg" + "msg/Float64Stamped.msg" + "msg/Float64MultiArrayStamped.msg" + "msg/Int32Stamped.msg" + "msg/Int32MultiArrayStamped.msg" + "msg/Int64Stamped.msg" + "msg/Int64MultiArrayStamped.msg" + "msg/StringStamped.msg" + "msg/ProcessingTimeNode.msg" + "msg/ProcessingTimeTree.msg" + "msg/SystemUsage.msg" + "msg/SystemUsageArray.msg" + DEPENDENCIES + builtin_interfaces +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/tier4_debug_msgs/msg/BoolStamped.msg b/tier4_debug_msgs/msg/BoolStamped.msg new file mode 100644 index 000000000..edb7a413e --- /dev/null +++ b/tier4_debug_msgs/msg/BoolStamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +bool data diff --git a/tier4_debug_msgs/msg/Float32MultiArrayStamped.msg b/tier4_debug_msgs/msg/Float32MultiArrayStamped.msg new file mode 100644 index 000000000..fee16cc47 --- /dev/null +++ b/tier4_debug_msgs/msg/Float32MultiArrayStamped.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +tier4_debug_msgs/MultiArrayLayout layout +float32[] data diff --git a/tier4_debug_msgs/msg/Float32Stamped.msg b/tier4_debug_msgs/msg/Float32Stamped.msg new file mode 100644 index 000000000..8d02ab528 --- /dev/null +++ b/tier4_debug_msgs/msg/Float32Stamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +float32 data diff --git a/tier4_debug_msgs/msg/Float64MultiArrayStamped.msg b/tier4_debug_msgs/msg/Float64MultiArrayStamped.msg new file mode 100644 index 000000000..101959369 --- /dev/null +++ b/tier4_debug_msgs/msg/Float64MultiArrayStamped.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +tier4_debug_msgs/MultiArrayLayout layout +float64[] data diff --git a/tier4_debug_msgs/msg/Float64Stamped.msg b/tier4_debug_msgs/msg/Float64Stamped.msg new file mode 100644 index 000000000..97a9447ee --- /dev/null +++ b/tier4_debug_msgs/msg/Float64Stamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +float64 data diff --git a/tier4_debug_msgs/msg/Int32MultiArrayStamped.msg b/tier4_debug_msgs/msg/Int32MultiArrayStamped.msg new file mode 100644 index 000000000..ce74accf6 --- /dev/null +++ b/tier4_debug_msgs/msg/Int32MultiArrayStamped.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +tier4_debug_msgs/MultiArrayLayout layout +int32[] data diff --git a/tier4_debug_msgs/msg/Int32Stamped.msg b/tier4_debug_msgs/msg/Int32Stamped.msg new file mode 100644 index 000000000..0d548772e --- /dev/null +++ b/tier4_debug_msgs/msg/Int32Stamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +int32 data diff --git a/tier4_debug_msgs/msg/Int64MultiArrayStamped.msg b/tier4_debug_msgs/msg/Int64MultiArrayStamped.msg new file mode 100644 index 000000000..9e83ab9a7 --- /dev/null +++ b/tier4_debug_msgs/msg/Int64MultiArrayStamped.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp +tier4_debug_msgs/MultiArrayLayout layout +int64[] data diff --git a/tier4_debug_msgs/msg/Int64Stamped.msg b/tier4_debug_msgs/msg/Int64Stamped.msg new file mode 100644 index 000000000..917edfff7 --- /dev/null +++ b/tier4_debug_msgs/msg/Int64Stamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +int64 data diff --git a/tier4_debug_msgs/msg/MultiArrayDimension.msg b/tier4_debug_msgs/msg/MultiArrayDimension.msg new file mode 100644 index 000000000..05bb4d867 --- /dev/null +++ b/tier4_debug_msgs/msg/MultiArrayDimension.msg @@ -0,0 +1,11 @@ +# This is from ros2 common_interfaces. +# https://github.com/ros2/common_interfaces/blob/master/std_msgs/msg/MultiArrayDimension.msg + +# This was originally provided as an example message. +# It is deprecated as of Foxy +# It is recommended to create your own semantically meaningful message. +# However if you would like to continue using this please use the equivalent in example_msgs. + +string label # label of given dimension +uint32 size # size of given dimension (in type units) +uint32 stride # stride of given dimension diff --git a/tier4_debug_msgs/msg/MultiArrayLayout.msg b/tier4_debug_msgs/msg/MultiArrayLayout.msg new file mode 100644 index 000000000..a2d8afb9f --- /dev/null +++ b/tier4_debug_msgs/msg/MultiArrayLayout.msg @@ -0,0 +1,34 @@ +# This is from ros2 common_interfaces. +# https://github.com/ros2/common_interfaces/blob/master/std_msgs/msg/MultiArrayLayout.msg + +# This was originally provided as an example message. +# It is deprecated as of Foxy +# It is recommended to create your own semantically meaningful message. +# However if you would like to continue using this please use the equivalent in example_msgs. + +# The multiarray declares a generic multi-dimensional array of a +# particular data type. Dimensions are ordered from outer most +# to inner most. +# +# Accessors should ALWAYS be written in terms of dimension stride +# and specified outer-most dimension first. +# +# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k] +# +# A standard, 3-channel 640x480 image with interleaved color channels +# would be specified as: +# +# dim[0].label = "height" +# dim[0].size = 480 +# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image) +# dim[1].label = "width" +# dim[1].size = 640 +# dim[1].stride = 3*640 = 1920 +# dim[2].label = "channel" +# dim[2].size = 3 +# dim[2].stride = 3 +# +# multiarray(i,j,k) refers to the ith row, jth column, and kth channel. + +tier4_debug_msgs/MultiArrayDimension[] dim # Array of dimension properties +uint32 data_offset # padding bytes at front of data diff --git a/tier4_debug_msgs/msg/ProcessingTimeNode.msg b/tier4_debug_msgs/msg/ProcessingTimeNode.msg new file mode 100644 index 000000000..e7d44098a --- /dev/null +++ b/tier4_debug_msgs/msg/ProcessingTimeNode.msg @@ -0,0 +1,10 @@ +# Unique ID of the node +int32 id +# Name of the node +string name +# Processing time of the node +float64 processing_time +# ID of the parent node, 0 if no parent +int32 parent_id +# Comment +string comment diff --git a/tier4_debug_msgs/msg/ProcessingTimeTree.msg b/tier4_debug_msgs/msg/ProcessingTimeTree.msg new file mode 100644 index 000000000..0e7be75dc --- /dev/null +++ b/tier4_debug_msgs/msg/ProcessingTimeTree.msg @@ -0,0 +1,2 @@ +# Array of all time nodes +ProcessingTimeNode[] nodes diff --git a/tier4_debug_msgs/msg/StringStamped.msg b/tier4_debug_msgs/msg/StringStamped.msg new file mode 100644 index 000000000..a48c85b2c --- /dev/null +++ b/tier4_debug_msgs/msg/StringStamped.msg @@ -0,0 +1,2 @@ +builtin_interfaces/Time stamp +string data diff --git a/tier4_debug_msgs/msg/SystemUsage.msg b/tier4_debug_msgs/msg/SystemUsage.msg new file mode 100644 index 000000000..34d8c060f --- /dev/null +++ b/tier4_debug_msgs/msg/SystemUsage.msg @@ -0,0 +1,4 @@ +string name # unique name to denote the process +int64 pid # process id +float64 cpu_usage # [%] +float64 memory_usage # [byte] diff --git a/tier4_debug_msgs/msg/SystemUsageArray.msg b/tier4_debug_msgs/msg/SystemUsageArray.msg new file mode 100644 index 000000000..1380dcbff --- /dev/null +++ b/tier4_debug_msgs/msg/SystemUsageArray.msg @@ -0,0 +1,3 @@ +builtin_interfaces/Time stamp + +SystemUsage[] system_usage diff --git a/tier4_debug_msgs/package.xml b/tier4_debug_msgs/package.xml new file mode 100644 index 000000000..509b7f2ed --- /dev/null +++ b/tier4_debug_msgs/package.xml @@ -0,0 +1,26 @@ + + + + tier4_debug_msgs + 0.1.0 + The tier4_debug_msgs package + Takamasa Horibe + Apache License 2.0 + + ament_cmake_auto + + rosidl_default_generators + + builtin_interfaces + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + +