feat: early_deadline_detector

src/tilde/src/tilde_node.cpp

@@ -14,6 +14,7 @@
 
 #include "tilde/tilde_node.hpp"
 
+#include <iostream>
 #include <string>
 
 using tilde::TildeNode;
@@ -38,6 +39,7 @@ void TildeNode::init()
   this->declare_parameter<bool>("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<rclcpp::Parameter> & parameters) {

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()

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).

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

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 <chrono>
+#include <map>
+#include <memory>
+// NOLINT to prevent Found C system header after C++ system header
+#include "rclcpp/rclcpp.hpp"
+#include "tilde_msg/msg/message_tracking_tag.hpp"
+
+#include <optional>  // NOLINT
+#include <set>
+#include <string>
+#include <tuple>
+#include <unordered_set>
+
+namespace tilde_early_deadline_detector
+{
+
+template <class C, class T>
+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<MessageTrackingTagMsg>;
+
+  /// sensor sources: [sensor_topic][sensor_header_stamp] = MessageTrackingTagMsg
+  using Sources =
+    std::map<TopicName, std::map<HeaderStamp, std::shared_ptr<MessageTrackingTagMsg>>>;
+  /// input sensor topics of the target topic
+  using TopicVsSensors = std::map<TopicName, std::set<TopicName>>;
+  /// to know sources
+  using RefToSources = std::set<RefToSource, std::owner_less<RefToSource>>;
+  /// sources of the specific message
+  // if target topic is sensor, MessageInputs[topic][stamp] points itself source.
+  using MessageSources = std::map<TopicName, std::map<HeaderStamp, RefToSources>>;
+  /// input sources which output consists of
+  using InputSources = std::map<TopicName, std::set<HeaderStamp>>;
+  /// pending messages: <wanted message>: <pending messages>
+  using Message = std::tuple<TopicName, HeaderStamp>;
+  using PendingMessages = std::map<TopicName, std::map<HeaderStamp, std::set<Message>>>;
+
+  /// Constructor
+  ForwardEstimator();
+
+  /// skip_out_to_in_ setter
+  /**
+   * \param skip_out_to_in skip topic setting
+   */
+  void set_skip_out_to_in(const std::map<std::string, std::string> & skip_out_to_in);
+
+  /// add MessageTrackingTag
+  void add(std::unique_ptr<MessageTrackingTagMsg> 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<rclcpp::Time> 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<TopicName, size_t> get_pending_message_counts() const;
+
+private:
+  /// all shared_ptr<MessageTrackingTag> of sensors to control pointer life time
+  Sources sources_;
+
+  /// skip topic setting
+  std::map<std::string, std::string> 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<MessageTrackingTagMsg> message_tracking_tag);
+};
+
+}  // namespace tilde_early_deadline_detector
+
+#endif  // TILDE_EARLY_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_