diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp index 29350744..d63f19a6 100644 --- a/src/tilde/src/tilde_node.cpp +++ b/src/tilde/src/tilde_node.cpp @@ -14,6 +14,7 @@ #include "tilde/tilde_node.hpp" +#include #include using tilde::TildeNode; @@ -38,6 +39,7 @@ void TildeNode::init() this->declare_parameter("enable_tilde", true); this->get_parameter("enable_tilde", enable_tilde_); + std::cout << "enable_tilde: " << enable_tilde_ << std::endl; param_callback_handle_ = this->add_on_set_parameters_callback([this](const std::vector & parameters) { diff --git a/src/tilde_early_deadline_detector/CMakeLists.txt b/src/tilde_early_deadline_detector/CMakeLists.txt new file mode 100644 index 00000000..c8b8740c --- /dev/null +++ b/src/tilde_early_deadline_detector/CMakeLists.txt @@ -0,0 +1,45 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_early_deadline_detector) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + + +set(INCLUDE_DIR + include + ${PROJECT_SOURCE_DIR}/include +) + +include_directories("${INCLUDE_DIR}") +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(tilde_msg REQUIRED) +find_package(tilde_deadline_detector REQUIRED) +find_package(rclcpp_components REQUIRED) + +add_library(tilde_early_deadline_detector_node SHARED + src/forward_estimator.cpp + src/tilde_early_deadline_detector_node.cpp) +ament_target_dependencies(tilde_early_deadline_detector_node + rclcpp + rclcpp_components + tilde_deadline_detector + tilde_msg) + +rclcpp_components_register_node(tilde_early_deadline_detector_node + PLUGIN "tilde_early_deadline_detector::TildeEarlyDeadlineDetectorNode" + EXECUTABLE tilde_early_deadline_detector_node_exe) + + + + +install(TARGETS + tilde_early_deadline_detector_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/src/tilde_early_deadline_detector/README.md b/src/tilde_early_deadline_detector/README.md new file mode 100644 index 00000000..998b13ac --- /dev/null +++ b/src/tilde_early_deadline_detector/README.md @@ -0,0 +1,74 @@ +# tilde_early_deadline_detector + +## Description + +early_deadline_detector is for early deadline detection on the specified path. + +early_deadline_detector can be built in [TILDE](https://github.com/tier4/TILDE/tree/master/doc) package. + +## Requirement + +- ROS 2 humble +- [TILDE](https://github.com/tier4/TILDE/tree/master/doc) +- TILDE enabled application +- The Messages should have the `header.stamp` field. + +## The default path for early deadline detection + +The default path is shown below. + +![default_path](./default_path.svg) + +## Code to be changed + +If you want to change the path for early deadline detection, here is a set of the parts that should be changed in tilde_early_deadline_detector.cpp. + +- line 66~: All pairs of topic names included in the path and accumulated execution time must be registered. The accumulated execution time means the sum of the execution time of topics and nodes from the topic to the end of the path. The accumulated execution time can be measured using [CARET](https://github.com/tier4/caret). +- line 233~: All topic names and their MessageTrackingTags (topic) must be registered. +- line 374: The end of the path (topic) must be registered to measure end-to-end latency. + +## Build + +early_deadline_detector must be built in [TILDE/src](https://github.com/tier4/TILDE/tree/master/src). + +Do `colcon build`. We recommend the "Release" build for performance. + +```bash +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` + +## Run + +Use `ros2 run` as below. + +```bash +$ source /path/to/ros/humble/setup.bash +$ source /path/to/TILDE/install/setup.bash +$ source install/setup.bash +$ ros2 run tilde_early_deadline_detector tilde_early_deadline_detector_node_exe \ + --ros-args --params-file src/tilde_early_deadline_detector/autoware_sensors.yaml +``` + +Here is a set of parameters. + +| category | name | about | +| ----------------- | ------------------------ | ---------------------------------------------------------------------------------------------- | +| system input | `sensor_topics` | regard nodes as sensors if MessageTrackingTag has no input_infos or the topic is in this list. | +| ignore | `ignore_topics` | don't subscribe these topics | +| skip | `skips_main_in` | skip setting: input main topics | +| | `skips_main_out` | skip setting: output main topic, in `skips_main_in` order | +| target & deadline | `target_topics` | the all topics included in the specified path. | +| | `deadline_ms` | list of deadline [ms] in `target_topics` order. | +| maintenance | `expire_ms` | internal data lifetime | +| | `cleanup_ms` | timer period to cleanup internal data | +| miscellaneous | `clock_work_around` | set true when your bag file does not have `/clock` | +| debug print | `show_performance` | set true to show performance report | +| | `print_report` | whether to print internal data | +| | `print_pending_messages` | whether to print pending messages | + +See [autoware_sensors.yaml](autoware_sensors.yaml) for a sample parameter yaml file. + +## Notification + +If the target topic overruns, `deadline_notification` topic is published. +The message type is [DeadlineNotification.msg](../tilde_msg/msg/DeadlineNotification.msg). diff --git a/src/tilde_early_deadline_detector/autoware_sensors.yaml b/src/tilde_early_deadline_detector/autoware_sensors.yaml new file mode 100644 index 00000000..7d8af5c9 --- /dev/null +++ b/src/tilde_early_deadline_detector/autoware_sensors.yaml @@ -0,0 +1,28 @@ +tilde_early_deadline_detector_node: + ros__parameters: + # input of e2e + sensor_topics: ["/sensing/lidar/top/self_cropped/pointcloud_ex"] + # early deadline detection points (not the output of e2e) + target_topics: [ + # "/sensing/lidar/top/self_cropped/pointcloud_ex", + # "/sensing/lidar/top/mirror_cropped/pointcloud_ex", + # "/sensing/lidar/top/rectified/pointcloud_ex", + # "/sensing/lidar/top/outlier_filtered/pointcloud", + "/localization/util/measurement_range/pointcloud", + "/localization/util/voxel_grid_downsample/pointcloud", + "/localization/util/downsample/pointcloud", + "/localization/pose_estimator/pose_with_covariance", + "/localization/pose_twist_fusion_filter/kinematic_state", + "/localization/kinematic_state", + "/planning/scenario_planning/scenario_selector/trajectory", + "/planning/scenario_planning/trajectory", + "/control/trajectory_follower/control_cmd", + ] + # specify deadline ms for topics in target_topics order. + # 0 means no deadline, and negative values are replaced by 0 + # deadline_ms corresponds to target_topics + deadline_ms: [553, 553, 553, 553, 553, 553, 553, 553, 553] + # parameters of debug messages + print_report: true + show_performance: true + print_pending_messages: false diff --git a/src/tilde_early_deadline_detector/default_path.svg b/src/tilde_early_deadline_detector/default_path.svg new file mode 100644 index 00000000..2f1dc23b --- /dev/null +++ b/src/tilde_early_deadline_detector/default_path.svg @@ -0,0 +1 @@ +/sensing/lidar/top/crop_box_filter_self/sensing/lidar/top/crop_box_filter_mirror/sensing/lidar/top/distortion_corrector_node/sensing/lidar/top/ring_outlier_filter/localization/util/crop_box_filter_measurement_range/localization/util/voxel_grid_downsample_filter/localization/util/random_downsample_filter/localization/pose_estimator/ndt_scan_matcher/localization/pose_twist_fusion_filter/ekf_localizer/localization/pose_twist_fusion_filter/stop_filter/planning/scenario_planning/scenario_selector/planning/scenario_planning/motion_velocity_smoother/control/trajectory_follower/controller_node_exe/control/vehicle_cmd_gate/sensing/lidar/top/self_cropped/pointcloud_ex/sensing/lidar/top/mirror_cropped/pointcloud_ex/sensing/lidar/top/rectified/pointcloud_ex/sensing/lidar/top/outlier_filtered/pointcloud/localization/util/measurement_range/pointcloud/localization/util/voxel_grid_downsample/pointcloud/localization/util/downsample/pointcloud/localization/pose_estimator/pose_with_covariance/localization/pose_twist_fusion/kinematic_state/localization/kinematic_state/planning/scenario_planning/scenario_selector/trajectory/planning/scenario_planning/trajectory/control/trajectory_follower/control_cmd \ No newline at end of file diff --git a/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp new file mode 100644 index 00000000..e61f884e --- /dev/null +++ b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp @@ -0,0 +1,140 @@ +// Copyright 2022 Research Institute of Systems Planning, 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 TILDE_EARLY_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ +#define TILDE_EARLY_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ + +#include +#include +#include +// NOLINT to prevent Found C system header after C++ system header +#include "rclcpp/rclcpp.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" + +#include // NOLINT +#include +#include +#include +#include + +namespace tilde_early_deadline_detector +{ + +template +bool contains(const C & cnt, const T & v) +{ + return cnt.find(v) != cnt.end(); +} + +class ForwardEstimator +{ +public: + using TopicName = std::string; + using MessageTrackingTagMsg = tilde_msg::msg::MessageTrackingTag; + using HeaderStamp = rclcpp::Time; + using RefToSource = std::weak_ptr; + + /// sensor sources: [sensor_topic][sensor_header_stamp] = MessageTrackingTagMsg + using Sources = + std::map>>; + /// input sensor topics of the target topic + using TopicVsSensors = std::map>; + /// to know sources + using RefToSources = std::set>; + /// sources of the specific message + // if target topic is sensor, MessageInputs[topic][stamp] points itself source. + using MessageSources = std::map>; + /// input sources which output consists of + using InputSources = std::map>; + /// pending messages: : + using Message = std::tuple; + using PendingMessages = std::map>>; + + /// Constructor + ForwardEstimator(); + + /// skip_out_to_in_ setter + /** + * \param skip_out_to_in skip topic setting + */ + void set_skip_out_to_in(const std::map & skip_out_to_in); + + /// add MessageTrackingTag + void add(std::unique_ptr message_tracking_tag, bool is_sensor = false); + + /// get sources of give message + /** + * \param topic_name Target topic name + * \param stamp Target header stamp + * \return set of references to sources + */ + RefToSources get_ref_to_sources(const std::string & topic_name, const HeaderStamp & stamp) const; + + /// get all sensor time + /** + * \param topic_name Target topic name + * \param stamp Target header stamp + * \return sensor topic vs its header stamps + */ + InputSources get_input_sources(const std::string & topic_name, const HeaderStamp & stamp) const; + + /// get the oldest sensor time + /** + * \param topic_name Target topic name + * \param stamp Target header stamp + * \return the oldest header.stamp of all sensors. + * + * Calculated latency is best effort i.e. + * when it cannot gather all sensor MessageTrackingTag, + * it returns the longest latency in gathered MessageTrackingTag. + */ + std::optional get_oldest_sensor_stamp( + const std::string & topic_name, const HeaderStamp & stamp) const; + + /// delete old data + /** + * \param threshold Time point to delete data whose stamp <= threshold + */ + void delete_expired(const rclcpp::Time & threshold); + + void debug_print(bool verbose = false) const; + + /// get pending message counts + /** + * \return pending topic name vs the number of waited stamps. + */ + std::map get_pending_message_counts() const; + +private: + /// all shared_ptr of sensors to control pointer life time + Sources sources_; + + /// skip topic setting + std::map skip_out_to_in_; + + /// input sensor information of (topic vs stamp). + MessageSources message_sources_; + + /// gather sensor topics of topics to know graph + TopicVsSensors topic_sensors_; + + /// pending messages + PendingMessages pending_messages_; + + void update_pending(std::shared_ptr message_tracking_tag); +}; + +} // namespace tilde_early_deadline_detector + +#endif // TILDE_EARLY_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ diff --git a/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp new file mode 100644 index 00000000..16e4ca1f --- /dev/null +++ b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp @@ -0,0 +1,156 @@ +// Copyright 2022 Research Institute of Systems Planning, 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 TILDE_EARLY_DEADLINE_DETECTOR__TILDE_EARLY_DEADLINE_DETECTOR_NODE_HPP_ +#define TILDE_EARLY_DEADLINE_DETECTOR__TILDE_EARLY_DEADLINE_DETECTOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tilde_early_deadline_detector/forward_estimator.hpp" +// #include "tilde_deadline_detector/tilde_deadline_detector_node.hpp" +#include "tilde_msg/msg/deadline_notification.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" + +#include + +#include +#include +#include +#include + +// map header +#include + +namespace tilde_early_deadline_detector +{ +struct PerformanceCounter +{ + void add(float v); + + float avg{0.0}; + float max{0.0}; + uint64_t cnt{0}; +}; + +class TildeEarlyDeadlineDetectorNode : public rclcpp::Node +{ + using MessageTrackingTagSubscription = rclcpp::Subscription; + +public: + RCLCPP_PUBLIC + explicit TildeEarlyDeadlineDetectorNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit TildeEarlyDeadlineDetectorNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + RCLCPP_PUBLIC + explicit TildeEarlyDeadlineDetectorNode(const rclcpp::NodeOptions & options); + + RCLCPP_PUBLIC + virtual ~TildeEarlyDeadlineDetectorNode(); + + std::set get_message_tracking_tag_topics() const; + +private: + ForwardEstimator fe; + std::set sensor_topics_; + std::set target_topics_; + std::set end_topics_; + std::map topic_vs_deadline_ms_; + + uint64_t expire_ms_; + uint64_t cleanup_ms_; + bool print_report_{false}; + bool print_pending_messages_{false}; + + // work around for no `/clock` bag files. + // TODO(y-okumura-isp): delete related codes + rclcpp::Time latest_; + + std::vector subs_; + rclcpp::TimerBase::SharedPtr timer_; + + rclcpp::Publisher::SharedPtr notification_pub_; + + PerformanceCounter message_tracking_tag_callback_counter_; + PerformanceCounter timer_callback_counter_; + + void init(); + void message_tracking_tag_callback(tilde_msg::msg::MessageTrackingTag::UniquePtr msg); +}; + +/// change here +/// changed constructor name +// TildeEarlyDeadlineDetectorNode inherits TildeDeadlineDetectorNode +// override the part differs from TildeDeadlineDetectorNode +// delete the same code(compare to TildeDeadlineDetectorNode) later +// class TildeEarlyDeadlineDetectorNode : public tilde_deadline_detector::TildeDeadlineDetectorNode{ +// using MessageTrackingTagSubscription = +// rclcpp::Subscription; + +// public: +// // constructors +// RCLCPP_PUBLIC +// explicit TildeEarlyDeadlineDetectorNode( +// const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +// /// see corresponding rclcpp::Node constructor +// RCLCPP_PUBLIC +// explicit TildeEarlyDeadlineDetectorNode( +// const std::string & node_name, const std::string & namespace_, +// const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + +// RCLCPP_PUBLIC +// explicit TildeEarlyDeadlineDetectorNode(const rclcpp::NodeOptions & options); + +// RCLCPP_PUBLIC +// virtual ~TildeEarlyDeadlineDetectorNode(); + +// std::set get_message_tracking_tag_topics() const; + +// private: +// tilde_deadline_detector::ForwardEstimator fe; +// std::set sensor_topics_; +// std::set target_topics_; +// std::set end_topics_; +// std::map topic_vs_deadline_ms_; + +// uint64_t expire_ms_; +// uint64_t cleanup_ms_; +// bool print_report_{false}; +// bool print_pending_messages_{false}; + +// // work around for no `/clock` bag files. +// // TODO(y-okumura-isp): delete related codes +// rclcpp::Time latest_; + +// std::vector subs_; +// rclcpp::TimerBase::SharedPtr timer_; + +// rclcpp::Publisher::SharedPtr notification_pub_; + +// tilde_deadline_detector::PerformanceCounter message_tracking_tag_callback_counter_; +// tilde_deadline_detector::PerformanceCounter timer_callback_counter_; + +// // main function +// void init(); +// void message_tracking_tag_callback(tilde_msg::msg::MessageTrackingTag::UniquePtr msg); +// }; + +} // namespace tilde_early_deadline_detector + +#endif // TILDE_EARLY_DEADLINE_DETECTOR__TILDE_EARLY_DEADLINE_DETECTOR_NODE_HPP_ diff --git a/src/tilde_early_deadline_detector/package.xml b/src/tilde_early_deadline_detector/package.xml new file mode 100644 index 00000000..aa9b70f7 --- /dev/null +++ b/src/tilde_early_deadline_detector/package.xml @@ -0,0 +1,25 @@ + + + + tilde_early_deadline_detector + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + ament_lint_auto + caret_lint_common + + rclcpp + rclcpp_components + sensor_msgs + tilde + tilde_deadline_detector + tilde_msg + + + ament_cmake + + diff --git a/src/tilde_early_deadline_detector/src/forward_estimator.cpp b/src/tilde_early_deadline_detector/src/forward_estimator.cpp new file mode 100644 index 00000000..71948c6a --- /dev/null +++ b/src/tilde_early_deadline_detector/src/forward_estimator.cpp @@ -0,0 +1,296 @@ +// Copyright 2022 Research Institute of Systems Planning, 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. + +#include "tilde_early_deadline_detector/forward_estimator.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tilde_early_deadline_detector +{ + +ForwardEstimator::ForwardEstimator() {} + +void ForwardEstimator::set_skip_out_to_in(const std::map & skip_out_to_in) +{ + skip_out_to_in_ = skip_out_to_in; +} + +void ForwardEstimator::add( + std::unique_ptr _message_tracking_tag, bool is_sensor) +{ + if (!_message_tracking_tag->output_info.has_header_stamp) { + return; + } + + std::shared_ptr message_tracking_tag = std::move(_message_tracking_tag); + + const auto & topic_name = message_tracking_tag->output_info.topic_name; + const auto stamp = rclcpp::Time(message_tracking_tag->output_info.header_stamp); + + // no input => it may be sensor source + if (is_sensor || message_tracking_tag->input_infos.size() == 0) { + // TODO(y-okumura-isp): what if timer fires without no new input in explicit API case + + sources_[topic_name][stamp] = message_tracking_tag; + message_sources_[topic_name][stamp].insert( + std::weak_ptr(message_tracking_tag)); + topic_sensors_[topic_name].insert(topic_name); + + auto pending_messages_topic_it = pending_messages_.find(topic_name); + if (pending_messages_topic_it == pending_messages_.end()) { + return; + } + + auto pending_messages_it = pending_messages_topic_it->second.find(stamp); + if (pending_messages_it == pending_messages_topic_it->second.end()) { + return; + } + + for (auto it : pending_messages_it->second) { + const auto & waited_topic = std::get<0>(it); + const auto & waited_stamp = std::get<1>(it); + const auto & input_sources = message_sources_[topic_name][stamp]; + const auto & input_source_topics = topic_sensors_[topic_name]; + + message_sources_[waited_topic][waited_stamp].insert( + input_sources.begin(), input_sources.end()); + topic_sensors_[waited_topic].insert(input_source_topics.begin(), input_source_topics.end()); + } + + pending_messages_topic_it->second.erase(pending_messages_it); + // we keep pending_messages_[topic_name] because it is fixed size resources + return; + } + + // if some messages wait this message, then + // input of these messages are changed + std::set pending_messages = std::set(); + auto pending_messages_topic_it = pending_messages_.find(topic_name); + if (pending_messages_topic_it != pending_messages_.end()) { + auto pending_messages_it = pending_messages_topic_it->second.find(stamp); + if (pending_messages_it != pending_messages_topic_it->second.end()) { + pending_messages.merge(pending_messages_it->second); + pending_messages_topic_it->second.erase(pending_messages_it); + } + // we keep pending_messages_[topic_name] because it is fixed size resources + } + // have input => get reference + for (const auto & input : message_tracking_tag->input_infos) { + // get sources of input + if (!input.has_header_stamp) { + continue; + } + + auto out_to_in_it = skip_out_to_in_.find(input.topic_name); + const auto & input_topic = + out_to_in_it != skip_out_to_in_.end() ? out_to_in_it->second : input.topic_name; + + const auto & input_stamp = rclcpp::Time(input.header_stamp); + const auto & input_source_topics = topic_sensors_[input_topic]; + + // if input of this message lacks, + // both this message and pending messages wait the input + auto message_sources_it = message_sources_[input_topic].find(input_stamp); + if (message_sources_it == message_sources_[input_topic].end()) { + auto & waiters = pending_messages_[input_topic][input_stamp]; + waiters.insert({topic_name, stamp}); + waiters.insert(pending_messages.begin(), pending_messages.end()); + continue; + } + + const auto & input_sources = message_sources_[input_topic][input_stamp]; + + // register sources + message_sources_[topic_name][stamp].insert(input_sources.begin(), input_sources.end()); + topic_sensors_[topic_name].insert(input_source_topics.begin(), input_source_topics.end()); + + // pending messages also get sources + for (const auto & wait : pending_messages) { + const auto & wait_topic = std::get<0>(wait); + const auto & wait_stamp = std::get<1>(wait); + message_sources_[wait_topic][wait_stamp].insert(input_sources.begin(), input_sources.end()); + topic_sensors_[wait_topic].insert(input_source_topics.begin(), input_source_topics.end()); + } + } +} + +std::string _time2str(const builtin_interfaces::msg::Time & time) +{ + std::ostringstream ret; + ret << std::to_string(time.sec); + ret << "."; + ret << std::setfill('0') << std::setw(9) << std::to_string(time.nanosec); + return ret.str(); +} + +ForwardEstimator::RefToSources ForwardEstimator::get_ref_to_sources( + const std::string & topic_name, const HeaderStamp & stamp) const +{ + RefToSources ret; + auto message_sources_topic_it = message_sources_.find(topic_name); + if (message_sources_topic_it == message_sources_.end()) { + return ret; + } + + auto stamps_sources_it = message_sources_topic_it->second.find(stamp); + if (stamps_sources_it == message_sources_topic_it->second.end()) { + return ret; + } + + return stamps_sources_it->second; +} + +ForwardEstimator::InputSources ForwardEstimator::get_input_sources( + const std::string & topic_name, const HeaderStamp & stamp) const +{ + InputSources is; + auto message_sources_topic_it = message_sources_.find(topic_name); + if (message_sources_topic_it == message_sources_.end()) { + // std::cout << topic_name << ": not found in message_sources_" << std::endl; + return is; + } + + auto stamps_sources_it = message_sources_topic_it->second.find(stamp); + if (stamps_sources_it == message_sources_topic_it->second.end()) { + /* + std::cout << topic_name << ":" + << _time2str(stamp) << ": not found in message_sources_" + << std::endl; + */ + return is; + } + + for (auto & weak_src : stamps_sources_it->second) { + auto src = weak_src.lock(); + if (!src) { + // std::cout << topic_name << ":" << _time2str(stamp) << " source deleted" << std::endl; + continue; + } + + is[src->output_info.topic_name].insert(src->output_info.header_stamp); + } + + return is; +} + +std::optional ForwardEstimator::get_oldest_sensor_stamp( + const std::string & topic_name, const HeaderStamp & stamp) const +{ + auto is = get_input_sources(topic_name, stamp); + if (is.empty()) { + return std::nullopt; + } + + std::set mins; + for (auto & it : is) { + mins.insert(*std::min_element(it.second.begin(), it.second.end())); + } + + return *(std::min_element(mins.begin(), mins.end())); +} + +void ForwardEstimator::delete_expired(const rclcpp::Time & threshold) +{ + // delete references + for (auto & it : message_sources_) { + auto & stamp_refs = it.second; + for (auto stamp_refs_it = stamp_refs.begin(); stamp_refs_it != stamp_refs.end();) { + if (threshold < stamp_refs_it->first) { + break; + } + stamp_refs_it = stamp_refs.erase(stamp_refs_it); + } + } + + // delete sources + for (auto & it : sources_) { + auto & stamp_message_tracking_tag = it.second; + for (auto stamp_message_tracking_tag_it = stamp_message_tracking_tag.begin(); + stamp_message_tracking_tag_it != stamp_message_tracking_tag.end();) { + if (threshold < stamp_message_tracking_tag_it->first) { + break; + } + stamp_message_tracking_tag_it->second.reset(); + stamp_message_tracking_tag_it = + stamp_message_tracking_tag.erase(stamp_message_tracking_tag_it); + } + } + + // delete pending_messages + for (auto & it : pending_messages_) { + auto & stamp_messages = it.second; + for (auto stamp_messages_it = stamp_messages.begin(); + stamp_messages_it != stamp_messages.end();) { + if (threshold < stamp_messages_it->first) { + break; + } + stamp_messages_it = stamp_messages.erase(stamp_messages_it); + } + } +} + +void ForwardEstimator::debug_print(bool verbose) const +{ + if (verbose) { + std::cout << "sources_: " << sources_.size() << std::endl; + for (auto & it : sources_) { + std::cout << " " << it.first << ": " << it.second.size() << std::endl; + } + + std::cout << "message_sources_: " << message_sources_.size() << std::endl; + for (auto & it : message_sources_) { + std::cout << " " << it.first << ": " << it.second.size() << std::endl; + } + + std::cout << "topic_sensors_: " << topic_sensors_.size() << std::endl; + for (auto & it : topic_sensors_) { + std::cout << " " << it.first << ": " << it.second.size() << std::endl; + } + } else { + auto n_sources = 0; + for (auto & it : sources_) { + n_sources += it.second.size(); + } + auto n_message_sources = 0; + for (auto & it : message_sources_) { + n_message_sources += it.second.size(); + } + auto n_topic_sensors = 0; + for (auto & it : topic_sensors_) { + n_topic_sensors += it.second.size(); + } + + std::cout << "sources: " << n_sources << " " + << "message_sources: " << n_message_sources << " " + << "topic_sensors: " << n_topic_sensors << std::endl; + } +} + +std::map ForwardEstimator::get_pending_message_counts() const +{ + std::map ret; + + for (const auto & pending_message : pending_messages_) { + ret[pending_message.first] = pending_message.second.size(); + } + + return ret; +} + +} // namespace tilde_early_deadline_detector diff --git a/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp b/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp new file mode 100644 index 00000000..3b599437 --- /dev/null +++ b/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp @@ -0,0 +1,534 @@ +// Copyright 2022 Research Institute of Systems Planning, 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. + +#include "tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp" + +#include "builtin_interfaces/msg/time.hpp" +#include "rcutils/time.h" +#include "tilde_msg/msg/deadline_notification.hpp" +#include "tilde_msg/msg/source.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// map +#include + +// container +#include +#include + +using std::chrono::milliseconds; +using tilde_msg::msg::MessageTrackingTag; + +/// figures for measurement +// processed_num: execute time of the path +// deadline_miss_num: number of early deadline detection by early_deadline_detector +// deadline_miss_true_num: number of deadline miss that actually occurred +int processed_num = 0; +int deadline_miss_num = 0; +int deadline_miss_true_num = 0; +// indicators(initialize) +double tp = 0; +double tn = 0; +double fp = 0; +double fn = 0; +double accuracy = 0; +double precision = 0; +double recall = 0; +double f_measure = 0; + +namespace tilde_early_deadline_detector +{ +// get estimated latency of the rest part +// map means the sets of topic name and estimated latency to the end topic +// topics are from example path +double estimate_latency(std::string topic_name) +{ + // 99percentile + std::map map{ + // {"/sensing/lidar/top/self_cropped/pointcloud_ex", 552.49}, + // {"/sensing/lidar/top/mirror_cropped/pointcloud_ex", 539.55}, + // {"/sensing/lidar/top/rectified/pointcloud_ex", 493.69}, + // {"/sensing/lidar/top/outlier_filtered/pointcloud", 462.35}, + {"/localization/util/measurement_range/pointcloud", 447.42}, + {"/localization/util/voxel_grid_downsample/pointcloud", 421.69}, + {"/localization/util/downsample/pointcloud", 420.86}, + {"/localization/pose_estimator/pose_with_covariance", 341.25}, + {"/localization/pose_twist_fusion_filter/kinematic_state", 189.63}, + {"/localization/kinematic_state", 189.19}, + {"/planning/scenario_planning/scenario_selector/trajectory", 163.32}, + {"/planning/scenario_planning/trajectory", 143.17}, + {"/control/trajectory_follower/control_cmd", 0.8}}; + + // // 95percentile + // std::map map{ + // // {"/sensing/lidar/top/self_cropped/pointcloud_ex", 470.99}, + // // {"/sensing/lidar/top/mirror_cropped/pointcloud_ex", 459.84}, + // // {"/sensing/lidar/top/rectified/pointcloud_ex", 420.36}, + // // {"/sensing/lidar/top/outlier_filtered/pointcloud", 392.86}, + // {"/localization/util/measurement_range/pointcloud", 379.96}, + // {"/localization/util/voxel_grid_downsample/pointcloud", 357.79}, + // {"/localization/util/downsample/pointcloud", 357.11}, + // {"/localization/pose_estimator/pose_with_covariance", 290.88}, + // {"/localization/pose_twist_fusion_filter/kinematic_state", 161.65}, + // {"/localization/kinematic_state", 161.28}, + // {"/planning/scenario_planning/scenario_selector/trajectory", 139.07}, + // {"/planning/scenario_planning/trajectory", 122.15}, + // {"/control/trajectory_follower/control_cmd", 0.69} + // }; + + // // 90percentile + // std::map map{ + // // {"/sensing/lidar/top/self_cropped/pointcloud_ex", 429.28}, + // // {"/sensing/lidar/top/mirror_cropped/pointcloud_ex", 419.03}, + // // {"/sensing/lidar/top/rectified/pointcloud_ex", 382.82}, + // // {"/sensing/lidar/top/outlier_filtered/pointcloud", 357.3}, + // {"/localization/util/measurement_range/pointcloud", 345.44}, + // {"/localization/util/voxel_grid_downsample/pointcloud", 325.08}, + // {"/localization/util/downsample/pointcloud", 324.48}, + // {"/localization/pose_estimator/pose_with_covariance", 265.1}, + // {"/localization/pose_twist_fusion_filter/kinematic_state", 147.34}, + // {"/localization/kinematic_state", 147.0}, + // {"/planning/scenario_planning/scenario_selector/trajectory", 126.67}, + // {"/planning/scenario_planning/trajectory", 111.41}, + // {"/control/trajectory_follower/control_cmd", 0.64} + // }; + + double estimated_latency; + auto it = map.find(topic_name); + if (it != map.end()) { + estimated_latency = it->second; + } + + return estimated_latency; +} + +std::string time2str(const builtin_interfaces::msg::Time & time) +{ + std::ostringstream ret; + ret << std::to_string(time.sec); + ret << "."; + ret << std::setfill('0') << std::setw(9) << std::to_string(time.nanosec); + return ret.str(); +} + +void PerformanceCounter::add(float v) +{ + avg = (avg * cnt + v) / (cnt + 1); + max = std::max(v, max); + cnt++; +} + +/// changed constructor name +TildeEarlyDeadlineDetectorNode::TildeEarlyDeadlineDetectorNode( + const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + init(); +} + +TildeEarlyDeadlineDetectorNode::TildeEarlyDeadlineDetectorNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options) +: Node(node_name, namespace_, options) +{ + init(); +} + +TildeEarlyDeadlineDetectorNode::TildeEarlyDeadlineDetectorNode(const rclcpp::NodeOptions & options) +: Node("tilde_early_deadline_detector_node", options) +{ + init(); +} + +TildeEarlyDeadlineDetectorNode::~TildeEarlyDeadlineDetectorNode() {} + +/// changed class name +std::set TildeEarlyDeadlineDetectorNode::get_message_tracking_tag_topics() const +{ + std::set ret; + + const std::string msg_type = "tilde_msg/msg/MessageTrackingTag"; + auto topic_and_types = get_topic_names_and_types(); + for (const auto & it : topic_and_types) { + if (std::find(it.second.begin(), it.second.end(), msg_type) == it.second.end()) { + continue; + } + ret.insert(it.first); + } + + return ret; +} + +/// changed class name +// register parameters of autoware_sensors.yaml +void TildeEarlyDeadlineDetectorNode::init() +{ + auto ignores = + declare_parameter>("ignore_topics", std::vector{}); + + auto tmp_sensor_topics = + declare_parameter>("sensor_topics", std::vector{}); + sensor_topics_.insert(tmp_sensor_topics.begin(), tmp_sensor_topics.end()); + + auto tmp_target_topics = + declare_parameter>("target_topics", std::vector{}); + target_topics_.insert(tmp_target_topics.begin(), tmp_target_topics.end()); + + auto deadline_ms = declare_parameter>("deadline_ms", std::vector{}); + + auto skips_main_out = + declare_parameter>("skips_main_out", std::vector{}); + auto skips_main_in = + declare_parameter>("skips_main_in", std::vector{}); + + assert(skips_main_out.size() == skips_main_in.size()); + + std::map skips_out_to_in; + for (auto out_it = skips_main_out.begin(), in_it = skips_main_in.begin(); + (out_it != skips_main_out.end() && in_it != skips_main_in.end()); out_it++, in_it++) { + skips_out_to_in[*out_it] = *in_it; + } + fe.set_skip_out_to_in(skips_out_to_in); + + expire_ms_ = declare_parameter("expire_ms", 3 * 1000); + cleanup_ms_ = declare_parameter("cleanup_ms", 3 * 1000); + print_report_ = declare_parameter("print_report", false); + print_pending_messages_ = declare_parameter("print_pending_messages", false); + + bool clock_work_around = declare_parameter("clock_work_around", false); + bool show_performance = declare_parameter("show_performance", false); + + // init topic_vs_deadline_ms_ + // topic_vs_deadline_ms_[topic] means the deadline of each target topic(refer to + // autoware_sensors.yaml) + for (size_t i = 0; i < tmp_target_topics.size(); i++) { + auto topic = tmp_target_topics[i]; + auto deadline = i < deadline_ms.size() ? deadline_ms[i] : 0; + deadline = std::max(deadline, 0l); + topic_vs_deadline_ms_[topic] = deadline; + std::cout << "deadline setting: " << topic << " = " << deadline << std::endl; + } + + // topics subscribed by early_deadline_detector + // topics are from example path + std::set topics{ + "/sensing/lidar/top/pointcloud_raw_ex", + "/sensing/lidar/top/self_cropped/pointcloud_ex", + "/sensing/lidar/top/mirror_cropped/pointcloud_ex", + "/sensing/lidar/top/rectified/pointcloud_ex", + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/localization/util/measurement_range/pointcloud", + "/localization/util/voxel_grid_downsample/pointcloud", + "/localization/util/downsample/pointcloud", + "/localization/pose_estimator/pose_with_covariance", + "/localization/pose_twist_fusion_filter/kinematic_state", + "/localization/kinematic_state", + "/planning/scenario_planning/scenario_selector/trajectory", + "/planning/scenario_planning/trajectory", + "/control/trajectory_follower/control_cmd", + // MTT + "/sensing/lidar/top/pointcloud_raw_ex/message_tracking_tag", + "/sensing/lidar/top/self_cropped/pointcloud_ex/message_tracking_tag", + "/sensing/lidar/top/mirror_cropped/pointcloud_ex/message_tracking_tag", + "/sensing/lidar/top/rectified/pointcloud_ex/message_tracking_tag", + "/sensing/lidar/top/outlier_filtered/pointcloud/message_tracking_tag", + "/localization/util/measurement_range/pointcloud/message_tracking_tag", + "/localization/util/voxel_grid_downsample/pointcloud/message_tracking_tag", + "/localization/util/downsample/pointcloud/message_tracking_tag", + "/localization/pose_estimator/pose_with_covariance/message_tracking_tag", + "/localization/pose_twist_fusion_filter/kinematic_state/message_tracking_tag", + "/localization/kinematic_state/message_tracking_tag", + "/planning/scenario_planning/scenario_selector/trajectory/message_tracking_tag", + "/planning/scenario_planning/trajectory/message_tracking_tag", + "/control/trajectory_follower/control_cmd/message_tracking_tag", + }; + + // print topic names subscribed by early_deadline_detector + for (auto itr = topics.begin(); itr != topics.end(); ++itr) { + std::cout << *itr << "\n"; + } + + // wait discovery done + while (topics.size() == 0) { + RCLCPP_INFO(this->get_logger(), "wait discovery"); + std::this_thread::sleep_for(std::chrono::seconds(1)); + topics = get_message_tracking_tag_topics(); + } + + // ignore_topics(autoware_sensors.yaml) + for (const auto & ignore : ignores) { + topics.erase(ignore); + } + + rclcpp::QoS qos(5); + qos.best_effort(); + + for (const auto & topic : topics) { + RCLCPP_INFO(this->get_logger(), "subscribe: %s", topic.c_str()); + auto sub = create_subscription( + topic, qos, + std::bind( + &TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback, this, + std::placeholders::_1)); + subs_.push_back(sub); + } + + latest_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + + timer_ = create_wall_timer( + milliseconds(cleanup_ms_), [this, clock_work_around, show_performance]() -> void { + auto st = std::chrono::steady_clock::now(); + + auto t = this->now(); + if (clock_work_around) { + t = latest_; + } + + auto delta = rclcpp::Duration::from_nanoseconds(RCUTILS_MS_TO_NS(expire_ms_)); + this->fe.delete_expired(t - delta); + + auto et = std::chrono::steady_clock::now(); + timer_callback_counter_.add( + std::chrono::duration_cast(et - st).count()); + + if (show_performance) { + std::cout << "-------" << std::endl; + std::cout << "message_tracking_tag_callback: " + << " avg: " << message_tracking_tag_callback_counter_.avg << "\n" + << " max: " << message_tracking_tag_callback_counter_.max << "\n" + << "timer_callback: " + << " avg: " << timer_callback_counter_.avg << "\n" + << " max: " << timer_callback_counter_.max + << "\n" + // debug + // << " processed_num: " << processed_num << "\n" + // << " deadline_miss_num: " << deadline_miss_num << "\n" + // << " deadline_miss_true_num: " << deadline_miss_true_num + << std::endl; + } + }); + + notification_pub_ = create_publisher( + "deadline_notification", rclcpp::QoS(1).best_effort()); +} + +void print_report( + const std::string & topic, const builtin_interfaces::msg::Time & stamp, + const ForwardEstimator::InputSources & is) +{ + std::cout << "-------" << std::endl; + std::cout << topic << ": " << time2str(stamp) << "\n"; + for (auto & it : is) { + std::cout << " " << it.first << ": "; + for (auto input_stamp : it.second) { + std::cout << time2str(input_stamp) << ", "; + } + std::cout << "\n"; + } + std::cout << "-------" << std::endl; + // print figures for measurement + std::cout << " processed times: " << processed_num << "\n" + << " deadline_miss_num: " << deadline_miss_num << "\n" + << " deadline_miss_true_num: " << deadline_miss_true_num << "\n" + << " tp: " << tp << "\n" + << " tn: " << tn << "\n" + << " fp: " << fp << "\n" + << " fn: " << fn << "\n" + << " accuracy: " << accuracy << "\n" + << " precision: " << precision << "\n" + << " recall: " << recall << "\n" + << " f_measure: " << f_measure << "\n" + << std::endl; + std::cout << std::endl; +} + +/// changed class name +void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( + MessageTrackingTag::UniquePtr message_tracking_tag) +{ + auto st = std::chrono::steady_clock::now(); + + auto target = message_tracking_tag->output_info.topic_name; + auto stamp = message_tracking_tag->output_info.header_stamp; + auto pub_time_steady = message_tracking_tag->output_info.pub_time_steady; + + // measure e2e latency + builtin_interfaces::msg::Time pub_time_steady_e2e; + if (message_tracking_tag->output_info.topic_name == "/control/trajectory_follower/control_cmd") { + pub_time_steady_e2e = message_tracking_tag->output_info.pub_time_steady; + } + + // work around for non `/clock` bag file + latest_ = std::max(rclcpp::Time(stamp), latest_); + + bool is_sensor = + (sensor_topics_.find(message_tracking_tag->output_info.topic_name) != sensor_topics_.end()); + fe.add(std::move(message_tracking_tag), is_sensor); + + if (!contains(target_topics_, target)) { + return; + } + + // print debug messages if "print_report" parameter is true + if (print_report_) { + auto is = fe.get_input_sources(target, stamp); + print_report(target, stamp, is); + } + + // print debug messages if "print_pending_messages" parameter is true + if (print_pending_messages_) { + std::cout << "pending message counts:\n"; + for (const auto & pending_message_count : fe.get_pending_message_counts()) { + if (pending_message_count.second == 0) { + continue; + } + std::cout << pending_message_count.first << ": " << pending_message_count.second << "\n"; + } + std::cout << std::endl; + } + + // definition of deadline_notification + tilde_msg::msg::DeadlineNotification notification_msg; + notification_msg.header.stamp = this->now(); + notification_msg.topic_name = target; + notification_msg.stamp = stamp; + + // definition of deadline + auto deadline_ms = topic_vs_deadline_ms_[target]; + notification_msg.deadline_setting = + rclcpp::Duration::from_nanoseconds(RCUTILS_MS_TO_NS(deadline_ms)); + + auto is_overrun = false; + auto sources = fe.get_ref_to_sources(target, stamp); + for (const auto & weak_src : sources) { + auto src = weak_src.lock(); + if (!src) { + continue; + } + tilde_msg::msg::Source source_msg; + source_msg.topic = src->output_info.topic_name; + source_msg.stamp = src->output_info.header_stamp; + // // debug + // std::cout << "source_msg.topic: " << source_msg.topic << "\n" + // << "source_msg.stamp: " << time2str(source_msg.stamp) << std::endl; + + auto elapsed = rclcpp::Time(pub_time_steady) - rclcpp::Time(src->output_info.pub_time_steady); + auto e2e_latency = + rclcpp::Time(pub_time_steady_e2e) - rclcpp::Time(src->output_info.pub_time_steady); + source_msg.elapsed = elapsed; + processed_num++; + + // flags for counting tp, tn, fp, fn + int flag_deadline_miss = 0; + int flag_deadline_miss_true = 0; + + /// expression of early deadline detection + // x: RCUTILS_MS_TO_NS(deadline) <- deadline of whole path + // y: elapsed.nanoseconds() <- execution time of executed part + // z: estimate_latency(it->second) <- estimated execution time of the rest part <- called by + // hash(key: target) if x <= y + z, deadline miss is detected + if ( + RCUTILS_MS_TO_NS(deadline_ms) <= + elapsed.nanoseconds() + RCUTILS_MS_TO_NS(estimate_latency(target))) { + std::cout << "-------" << std::endl; + std::cout << "deadline miss" << std::endl; + source_msg.is_overrun = true; + is_overrun = true; + deadline_miss_num++; + flag_deadline_miss++; // flag_deadline_miss=1 + } + + /// expression of normal deadline detection + // a: RCUTILS_MS_TO_NS(deadline) <- deadline of whole path + // b: e2e_latency.nanoseconds() <- execution time of whole path + // if a <= b, deadline miss actually occurred + if (RCUTILS_MS_TO_NS(deadline_ms) <= e2e_latency.nanoseconds()) { + std::cout << "true deadline miss" << std::endl; + deadline_miss_true_num++; + flag_deadline_miss_true++; // flag_deadline_miss_true=1 + } + notification_msg.sources.push_back(source_msg); + + // count tp, tn, fp, fn + if (flag_deadline_miss != 0 && flag_deadline_miss_true != 0) + tp++; + else if (flag_deadline_miss == 0 && flag_deadline_miss_true == 0) + tn++; + else if (flag_deadline_miss != 0 && flag_deadline_miss_true == 0) + fp++; + else if (flag_deadline_miss == 0 && flag_deadline_miss_true != 0) + fn++; + + // calculate accuracy, precision, recall, f_measure + if (deadline_miss_true_num != 0 && (processed_num - deadline_miss_true_num) != 0) { + if (tp + tn + fp + fn > 0) { + accuracy = (tp + tn) / (tp + tn + fp + fn); + } + if (tp + fp > 0) { + precision = tp / (tp + fp); + } + if (tp + fn > 0) { + recall = tp / (tp + fn); + } + if (precision + recall > 0) { + f_measure = 2 * precision * recall / (precision + recall); + } + } + } + + if (is_overrun) { + // publish deadline_notification if overruns + notification_pub_->publish(notification_msg); + printf("notificated.\n"); + std::cout << " notification_msg.header.stamp: " << time2str(notification_msg.header.stamp) + << "\n" + << " notification_msg.topic_name: " << notification_msg.topic_name << "\n" + << " notification_msg.stamp: " << time2str(notification_msg.stamp) + << "\n" + // print figures for measurement + << " processed times: " << processed_num << "\n" + << " deadline miss times: " << deadline_miss_num << "\n" + << " true deadline miss times: " << deadline_miss_true_num << "\n" + << " tp: " << tp << "\n" + << " tn: " << tn << "\n" + << " fp: " << fp << "\n" + << " fn: " << fn << "\n" + << " accuracy: " << accuracy << "\n" + << " precision: " << precision << "\n" + << " recall: " << recall << "\n" + << " f_measure: " << f_measure << "\n" + << std::endl; + } + + // update performance counter + auto et = std::chrono::steady_clock::now(); + message_tracking_tag_callback_counter_.add( + std::chrono::duration_cast(et - st).count()); +} + +} // namespace tilde_early_deadline_detector + +#include "rclcpp_components/register_node_macro.hpp" +/// changed class name +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_early_deadline_detector::TildeEarlyDeadlineDetectorNode) diff --git a/src/tilde_vis/README.md b/src/tilde_vis/README.md index 8c8f5988..0859fc76 100644 --- a/src/tilde_vis/README.md +++ b/src/tilde_vis/README.md @@ -111,7 +111,7 @@ Columns are "topic name", "header stamp", "latency in ROS time", "latency in ste /vehicle/status/twist* NA NA NA ``` -See [latency_viewer.md](../../doc/latency_viewer.md) in detail (in Japanese). +See [latency_viewer.md](https://github.com/tier4/TILDE/blob/master/doc/latency_viewer.md) in detail (in Japanese). ### key binding in ncurses view