From 67b72efddf30ecef86a37337f31d68c4a5eb1ec2 Mon Sep 17 00:00:00 2001 From: hsato-saitama Date: Thu, 23 Mar 2023 16:47:55 +0900 Subject: [PATCH 01/16] README.md --- .../CMakeLists.txt | 45 ++ src/tilde_early_deadline_detector/README.md | 74 +++ .../autoware_sensors.yaml | 30 + .../default_path.svg | 1 + .../forward_estimator.hpp | 140 +++++ .../tilde_early_deadline_detector_node.hpp | 157 ++++++ src/tilde_early_deadline_detector/package.xml | 25 + .../src/forward_estimator.cpp | 296 ++++++++++ .../tilde_early_deadline_detector_node.cpp | 521 ++++++++++++++++++ 9 files changed, 1289 insertions(+) create mode 100644 src/tilde_early_deadline_detector/CMakeLists.txt create mode 100644 src/tilde_early_deadline_detector/README.md create mode 100644 src/tilde_early_deadline_detector/autoware_sensors.yaml create mode 100644 src/tilde_early_deadline_detector/default_path.svg create mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp create mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp create mode 100644 src/tilde_early_deadline_detector/package.xml create mode 100644 src/tilde_early_deadline_detector/src/forward_estimator.cpp create mode 100644 src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp 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..cef7ed70 --- /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/TILDE/tree/master/doc](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..1086bc92 --- /dev/null +++ b/src/tilde_early_deadline_detector/autoware_sensors.yaml @@ -0,0 +1,30 @@ +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..e9ef26a5 --- /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_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..fb0f85e9 --- /dev/null +++ b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp @@ -0,0 +1,157 @@ +// 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_deadline_detector + +#endif // TILDE_EARLY_DEADLINE_DETECTOR__TILDE_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..f2c1e58a --- /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_msg + tilde_deadline_detector + + + 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..651211ed --- /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_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..2074c5f7 --- /dev/null +++ b/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp @@ -0,0 +1,521 @@ +// 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 +#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!=0 || fp!=0 || fn!=0){ + accuracy = (tp + tn) / (tp + tn + fp + fn); + precision = tp / (tp + fp); + recall = tp / (tp + fn); + if(precision != 0 || 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) From a6e09e7a39fcbf547c8c3783ee3706bf2de1d328 Mon Sep 17 00:00:00 2001 From: hsato-saitama Date: Thu, 23 Mar 2023 17:15:48 +0900 Subject: [PATCH 02/16] README.md --- src/tilde/src/tilde_node.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp index 29350744..a14de72d 100644 --- a/src/tilde/src/tilde_node.cpp +++ b/src/tilde/src/tilde_node.cpp @@ -15,6 +15,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) { From 9c97fdb8a4d6468536066347414bf835b7eba755 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 17:23:00 +0900 Subject: [PATCH 03/16] Add files via upload --- src/autoware_common/CODE_OF_CONDUCT.md | 132 ++++++++++++ src/autoware_common/CONTRIBUTING.md | 3 + src/autoware_common/CPPLINT.cfg | 14 ++ src/autoware_common/DISCLAIMER.md | 46 ++++ src/autoware_common/LICENSE | 201 ++++++++++++++++++ src/autoware_common/README.md | 3 + .../caret_lint_common/CMakeLists.txt | 11 + .../caret_lint_common/README.md | 44 ++++ .../caret_lint_common/package.xml | 25 +++ src/autoware_common/setup.cfg | 15 ++ 10 files changed, 494 insertions(+) create mode 100644 src/autoware_common/CODE_OF_CONDUCT.md create mode 100644 src/autoware_common/CONTRIBUTING.md create mode 100644 src/autoware_common/CPPLINT.cfg create mode 100644 src/autoware_common/DISCLAIMER.md create mode 100644 src/autoware_common/LICENSE create mode 100644 src/autoware_common/README.md create mode 100644 src/autoware_common/caret_lint_common/CMakeLists.txt create mode 100644 src/autoware_common/caret_lint_common/README.md create mode 100644 src/autoware_common/caret_lint_common/package.xml create mode 100644 src/autoware_common/setup.cfg diff --git a/src/autoware_common/CODE_OF_CONDUCT.md b/src/autoware_common/CODE_OF_CONDUCT.md new file mode 100644 index 00000000..c493cad4 --- /dev/null +++ b/src/autoware_common/CODE_OF_CONDUCT.md @@ -0,0 +1,132 @@ +# Contributor Covenant Code of Conduct + +## Our Pledge + +We as members, contributors, and leaders pledge to make participation in our +community a harassment-free experience for everyone, regardless of age, body +size, visible or invisible disability, ethnicity, sex characteristics, gender +identity and expression, level of experience, education, socio-economic status, +nationality, personal appearance, race, caste, color, religion, or sexual +identity and orientation. + +We pledge to act and interact in ways that contribute to an open, welcoming, +diverse, inclusive, and healthy community. + +## Our Standards + +Examples of behavior that contributes to a positive environment for our +community include: + +- Demonstrating empathy and kindness toward other people +- Being respectful of differing opinions, viewpoints, and experiences +- Giving and gracefully accepting constructive feedback +- Accepting responsibility and apologizing to those affected by our mistakes, + and learning from the experience +- Focusing on what is best not just for us as individuals, but for the overall + community + +Examples of unacceptable behavior include: + +- The use of sexualized language or imagery, and sexual attention or advances of + any kind +- Trolling, insulting or derogatory comments, and personal or political attacks +- Public or private harassment +- Publishing others' private information, such as a physical or email address, + without their explicit permission +- Other conduct which could reasonably be considered inappropriate in a + professional setting + +## Enforcement Responsibilities + +Community leaders are responsible for clarifying and enforcing our standards of +acceptable behavior and will take appropriate and fair corrective action in +response to any behavior that they deem inappropriate, threatening, offensive, +or harmful. + +Community leaders have the right and responsibility to remove, edit, or reject +comments, commits, code, wiki edits, issues, and other contributions that are +not aligned to this Code of Conduct, and will communicate reasons for moderation +decisions when appropriate. + +## Scope + +This Code of Conduct applies within all community spaces, and also applies when +an individual is officially representing the community in public spaces. +Examples of representing our community include using an official e-mail address, +posting via an official social media account, or acting as an appointed +representative at an online or offline event. + +## Enforcement + +Instances of abusive, harassing, or otherwise unacceptable behavior may be +reported to the community leaders responsible for enforcement at +conduct@autoware.org. +All complaints will be reviewed and investigated promptly and fairly. + +All community leaders are obligated to respect the privacy and security of the +reporter of any incident. + +## Enforcement Guidelines + +Community leaders will follow these Community Impact Guidelines in determining +the consequences for any action they deem in violation of this Code of Conduct: + +### 1. Correction + +**Community Impact**: Use of inappropriate language or other behavior deemed +unprofessional or unwelcome in the community. + +**Consequence**: A private, written warning from community leaders, providing +clarity around the nature of the violation and an explanation of why the +behavior was inappropriate. A public apology may be requested. + +### 2. Warning + +**Community Impact**: A violation through a single incident or series of +actions. + +**Consequence**: A warning with consequences for continued behavior. No +interaction with the people involved, including unsolicited interaction with +those enforcing the Code of Conduct, for a specified period of time. This +includes avoiding interactions in community spaces as well as external channels +like social media. Violating these terms may lead to a temporary or permanent +ban. + +### 3. Temporary Ban + +**Community Impact**: A serious violation of community standards, including +sustained inappropriate behavior. + +**Consequence**: A temporary ban from any sort of interaction or public +communication with the community for a specified period of time. No public or +private interaction with the people involved, including unsolicited interaction +with those enforcing the Code of Conduct, is allowed during this period. +Violating these terms may lead to a permanent ban. + +### 4. Permanent Ban + +**Community Impact**: Demonstrating a pattern of violation of community +standards, including sustained inappropriate behavior, harassment of an +individual, or aggression toward or disparagement of classes of individuals. + +**Consequence**: A permanent ban from any sort of public interaction within the +community. + +## Attribution + +This Code of Conduct is adapted from the [Contributor Covenant][homepage], +version 2.1, available at +[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. + +Community Impact Guidelines were inspired by +[Mozilla's code of conduct enforcement ladder][mozilla coc]. + +For answers to common questions about this code of conduct, see the FAQ at +[https://www.contributor-covenant.org/faq][faq]. Translations are available at +[https://www.contributor-covenant.org/translations][translations]. + +[homepage]: https://www.contributor-covenant.org +[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html +[mozilla coc]: https://github.com/mozilla/diversity +[faq]: https://www.contributor-covenant.org/faq +[translations]: https://www.contributor-covenant.org/translations diff --git a/src/autoware_common/CONTRIBUTING.md b/src/autoware_common/CONTRIBUTING.md new file mode 100644 index 00000000..22c7ee28 --- /dev/null +++ b/src/autoware_common/CONTRIBUTING.md @@ -0,0 +1,3 @@ +# Contributing + +See . diff --git a/src/autoware_common/CPPLINT.cfg b/src/autoware_common/CPPLINT.cfg new file mode 100644 index 00000000..ba6bdf08 --- /dev/null +++ b/src/autoware_common/CPPLINT.cfg @@ -0,0 +1,14 @@ +# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120 +set noparent +linelength=100 +includeorder=standardcfirst +filter=-build/c++11 # we do allow C++11 +filter=-build/namespaces_literals # we allow using namespace for literals +filter=-runtime/references # we consider passing non-const references to be ok +filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions +filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space +filter=-whitespace/parens # we allow closing parenthesis to be on the next line +filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon +filter=-build/header_guard # we automatically fix the names of header guards using pre-commit +filter=-build/include_order # we use the custom include order +filter=-build/include_subdir # we allow the style of "foo.hpp" diff --git a/src/autoware_common/DISCLAIMER.md b/src/autoware_common/DISCLAIMER.md new file mode 100644 index 00000000..1b5a9bbe --- /dev/null +++ b/src/autoware_common/DISCLAIMER.md @@ -0,0 +1,46 @@ +DISCLAIMER + +“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0. +This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with +the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents +specified in this disclaimer below and will be deemed to consent to this +disclaimer without any objection upon utilizing or downloading Autoware. + +Disclaimer and Waiver of Warranties + +1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) + including but not limited to any representation or warranty (i) of fitness or + suitability for a particular purpose contemplated by the Users, (ii) of the + expected functions, commercial value, accuracy, or usefulness of the Service, + (iii) that the use by the Users of the Service complies with the laws and + regulations applicable to the Users or any internal rules established by + industrial organizations, (iv) that the Service will be free of interruption or + defects, (v) of the non-infringement of any third party's right and (vi) the + accuracy of the content of the Services and the software itself. + +2. The Autoware Foundation shall not be liable for any damage incurred by the + User that are attributable to the Autoware Foundation for any reasons + whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR + INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. + +3. A User shall be entirely responsible for the content posted by the User and + its use of any content of the Service or the Website. If the User is held + responsible in a civil action such as a claim for damages or even in a criminal + case, the Autoware Foundation and member companies, governments and academic & + non-profit organizations and their directors, officers, employees and agents + (collectively, the “Indemnified Parties”) shall be completely discharged from + any rights or assertions the User may have against the Indemnified Parties, or + from any legal action, litigation or similar procedures. + +Indemnity + +A User shall indemnify and hold the Indemnified Parties harmless from any of +their damages, losses, liabilities, costs or expenses (including attorneys' +fees or criminal compensation), or any claims or demands made against the +Indemnified Parties by any third party, due to or arising out of, or in +connection with utilizing Autoware (including the representations and +warranties), the violation of applicable Product Liability Law of each country +(including criminal case) or violation of any applicable laws by the Users, or +the content posted by the User or its use of any content of the Service or the +Website. diff --git a/src/autoware_common/LICENSE b/src/autoware_common/LICENSE new file mode 100644 index 00000000..261eeb9e --- /dev/null +++ b/src/autoware_common/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/src/autoware_common/README.md b/src/autoware_common/README.md new file mode 100644 index 00000000..efe11f4c --- /dev/null +++ b/src/autoware_common/README.md @@ -0,0 +1,3 @@ +# caret_common + +common utility for CARET repositories diff --git a/src/autoware_common/caret_lint_common/CMakeLists.txt b/src/autoware_common/caret_lint_common/CMakeLists.txt new file mode 100644 index 00000000..b068de75 --- /dev/null +++ b/src/autoware_common/caret_lint_common/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.14) + +project(caret_lint_common NONE) + +find_package(ament_cmake_core REQUIRED) +find_package(ament_cmake_export_dependencies REQUIRED) +find_package(ament_cmake_test REQUIRED) + +ament_package_xml() +ament_export_dependencies(${${PROJECT_NAME}_EXEC_DEPENDS}) +ament_package() diff --git a/src/autoware_common/caret_lint_common/README.md b/src/autoware_common/caret_lint_common/README.md new file mode 100644 index 00000000..1b906750 --- /dev/null +++ b/src/autoware_common/caret_lint_common/README.md @@ -0,0 +1,44 @@ +# caret_lint_common + +A custom version of [ament_lint_common](https://github.com/ament/ament_lint/tree/master/ament_lint_common) for [CARET](https://github.com/tier4/caret). + +## Usage + +Add dependencies of `ament_lint_auto` and `caret_lint_common` to your package as below. + +`package.xml`: + +```xml +ament_lint_auto +caret_lint_common +``` + +`CMakeLists.txt`: + +```cmake +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() +``` + +Then, the following linters will run during `colcon test`. + +- [ament_cmake_copyright](https://github.com/ament/ament_lint/blob/master/ament_cmake_copyright/doc/index.rst) +- [ament_cmake_cppcheck](https://github.com/ament/ament_lint/blob/master/ament_cmake_cppcheck/doc/index.rst) +- [ament_cmake_lint_cmake](https://github.com/ament/ament_lint/blob/master/ament_cmake_lint_cmake/doc/index.rst) +- [ament_cmake_xmllint](https://github.com/ament/ament_lint/blob/master/ament_cmake_xmllint/doc/index.rst) + +## Design + +The original `ament_lint_common` contains other formatters/linters like `ament_cmake_uncrustify`, `ament_cmake_cpplint` and `ament_cmake_flake8`. +However, we don't include them because it's more useful to run them with `pre-commit` as [MoveIt](https://github.com/ros-planning/moveit2) does. + +For example, the benefits are: + +- We can use any version of tools independent of ament's version. +- We can easily integrate into IDE. +- We can easily check all the files in the repository without writing `test_depend` in each package. +- We can run formatters/linters without building, which makes error detection faster. + +Ideally, we think other linters should be moved to `pre-commit` as well, so we'll try to support them in the future. diff --git a/src/autoware_common/caret_lint_common/package.xml b/src/autoware_common/caret_lint_common/package.xml new file mode 100644 index 00000000..bf0cffb9 --- /dev/null +++ b/src/autoware_common/caret_lint_common/package.xml @@ -0,0 +1,25 @@ + + + + caret_lint_common + 0.1.0 + The list of commonly used linters in CARET + Takayuki Akamine + Apache License 2.0 + + ament_cmake_core + ament_cmake_export_dependencies + ament_cmake_test + + ament_cmake_core + ament_cmake_test + + ament_cmake_copyright + ament_cmake_cppcheck + ament_cmake_lint_cmake + ament_cmake_xmllint + + + ament_cmake + + diff --git a/src/autoware_common/setup.cfg b/src/autoware_common/setup.cfg new file mode 100644 index 00000000..5214751c --- /dev/null +++ b/src/autoware_common/setup.cfg @@ -0,0 +1,15 @@ +[flake8] +# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini +extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 +import-order-style = pep8 +max-line-length = 100 +show-source = true +statistics = true + +[isort] +profile=black +line_length=100 +force_sort_within_sections=true +force_single_line=true +reverse_relative=true +known_third_party=launch From b2c08d293c303ffaadf4c4f63395cab203b18d85 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 17:24:25 +0900 Subject: [PATCH 04/16] Add files via upload --- .../autoware_auto_msgs/DISCLAIMER.md | 50 +++++ src/autoware_msg/autoware_auto_msgs/LICENSE | 201 ++++++++++++++++++ src/autoware_msg/autoware_auto_msgs/README.md | 77 +++++++ .../autoware_auto_msgs/build_depends.repos | 1 + 4 files changed, 329 insertions(+) create mode 100644 src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md create mode 100644 src/autoware_msg/autoware_auto_msgs/LICENSE create mode 100644 src/autoware_msg/autoware_auto_msgs/README.md create mode 100644 src/autoware_msg/autoware_auto_msgs/build_depends.repos diff --git a/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md b/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md new file mode 100644 index 00000000..d0d6f97d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md @@ -0,0 +1,50 @@ +DISCLAIMER + +The open-source software for self-driving vehicles, Autoware.AI, Autoware.IO, +Autoware.Auto. (collectively, referred to as “Autoware”) will be provided by +The Autoware Foundation under the Apache 2.0 License. This “DISCLAIMER” will be +applied to all users of Autoware (a “User” or “Users”) with the Apache 2.0 +License and Users shall hereby approve and acknowledge all the contents +specified in this disclaimer below and will be deemed to consent to this +disclaimer without any objection upon utilizing or downloading Autoware. + + +Disclaimer and Waiver of Warranties + +1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, +EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) +including but not limited to any representation or warranty (i) of fitness or +suitability for a particular purpose contemplated by the Users, (ii) of the +expected functions, commercial value, accuracy, or usefulness of the Service, +(iii) that the use by the Users of the Service complies with the laws and +regulations applicable to the Users or any internal rules established by +industrial organizations, (iv) that the Service will be free of interruption or +defects, (v) of the non-infringement of any third party's right and (vi) the +accuracy of the content of the Services and the software itself. + +2. The Autoware Foundation shall not be liable for any damage incurred by the +User that are attributable to the Autoware Foundation for any reasons +whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR +INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. + +3. A User shall be entirely responsible for the content posted by the User and +its use of any content of the Service or the Website. If the User is held +responsible in a civil action such as a claim for damages or even in a criminal +case, the Autoware Foundation and member companies, governments and academic & +non-profit organizations and their directors, officers, employees and agents +(collectively, the “Indemnified Parties”) shall be completely discharged from +any rights or assertions the User may have against the Indemnified Parties, or +from any legal action, litigation or similar procedures. + + +Indemnity + +A User shall indemnify and hold the Indemnified Parties harmless from any of +their damages, losses, liabilities, costs or expenses (including attorneys' +fees or criminal compensation), or any claims or demands made against the +Indemnified Parties by any third party, due to or arising out of, or in +connection with utilizing Autoware (including the representations and +warranties), the violation of applicable Product Liability Law of each country +(including criminal case) or violation of any applicable laws by the Users, or +the content posted by the User or its use of any content of the Service or the +Website. diff --git a/src/autoware_msg/autoware_auto_msgs/LICENSE b/src/autoware_msg/autoware_auto_msgs/LICENSE new file mode 100644 index 00000000..b0d61354 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "{}" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2018 ApexAI + + 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. diff --git a/src/autoware_msg/autoware_auto_msgs/README.md b/src/autoware_msg/autoware_auto_msgs/README.md new file mode 100644 index 00000000..c1dbcc16 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/README.md @@ -0,0 +1,77 @@ +This is a Tier4 forked version of [autoware_auto_msgs](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs) + +# autoware_auto_msgs + +Interfaces between core Autoware.Auto components. + +## Conventions + +### Comments + +Add a comment to describe the meaning of the field. + +### Default Value + +Prefer a meaningful default value. Otherwise, the field is uninitialized. + +### Optional parameters + +There is nothing like `std::optional` in IDL, unfortunately. To accomodate the common use case of a +fixed message where some variables are not always filled, add an additional boolean variable with a +prefix `has_` and a default value of `FALSE`. + +### Units + +If a quantity described by a field has an associated unit of measurement, the following rules apply to determine the field name: + +1. If the unit is as base or [derived SI unit](https://en.wikipedia.org/wiki/International_System_of_Units#Derived_units), do not add a suffix and assume the default from [REP-103](https://www.ros.org/reps/rep-0103.html). +1. If the unit is a multiple of a base or derived SI unit, apply a suffix according to the table below. +1. If the unit is composed of non-SI units, apply a suffix according to the table below. + +Only deviate from the SI units when absolutely necessary and with justification. + +| Quantity | Unit | Suffix | Notes | +|-----------------|-----------------------------|---------|---------------------------------------------------------| +| distance | meters | None | | +| | micrometers | `_um` | | +| | millimeters | `_mm` | | +| | kilometers | `_km` | | +| speed, velocity | meters / second | None | Use speed for scalar and velocity for vector quantities | +| | kilometers / hour | `_kmph` | | +| acceleration | meters / second2 | None | | +| radial velocity | radians / second | None | | +| time | second | None | | +| | microsecond | `_us` | | +| | nanosecond | `_ns` | | + +#### Examples + +The first alternative is recommended, the second discouraged: + +1. `float elapsed_time` vs `float_elapsed_time_s` +1. `float distance_travelled` vs `float distance_travelled_m` +1. `int32 time_step_ms` vs `int32 time_step` +1. `float speed_mps` vs `float speed` + +### Minimal Example + +```idl +struct Foo { + @verbatim (language="comment", text= + " A multiline" "\n" + " comment") + @default (value=0.0) + float bar_speed; + + @verbatim (language="comment", text= + " Another multiline" "\n" + " comment") + @default (value=FALSE) + boolean has_bar_speed; + + @verbatim (language="comment", text= + " Describe the time stamp") + @default (value=0) + int32 timestamp_ns; +}; +``` diff --git a/src/autoware_msg/autoware_auto_msgs/build_depends.repos b/src/autoware_msg/autoware_auto_msgs/build_depends.repos new file mode 100644 index 00000000..56f46b6f --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/build_depends.repos @@ -0,0 +1 @@ +repositories: From 035cbf2ad8933df5e42d3948e44b0753bde169d7 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 17:28:34 +0900 Subject: [PATCH 05/16] Add files via upload --- .../msg/Complex32.idl | 20 + .../msg/Quaternion32.idl | 19 + .../RelativePositionWithCovarianceStamped.idl | 16 + .../autoware_auto_mapping_msgs/CMakeLists.txt | 26 + .../msg/HADMapBin.idl | 32 ++ .../msg/HADMapSegment.idl | 12 + .../msg/MapPrimitive.idl | 17 + .../autoware_auto_mapping_msgs/package.xml | 27 + .../srv/HADMapService.idl | 39 ++ .../autoware_auto_msgs/CHANGELOG.rst | 57 ++ .../autoware_auto_msgs/CMakeLists.txt | 16 + .../autoware_auto_internal_msgs-design.md | 67 +++ .../design/autoware_auto_msgs-design.md | 511 ++++++++++++++++++ .../design/bounding-box-design.md | 224 ++++++++ .../autoware_auto_msgs/package.xml | 27 + .../CMakeLists.txt | 52 ++ .../msg/BoundingBox.idl | 71 +++ .../msg/BoundingBoxArray.idl | 17 + .../msg/ClassifiedRoi.idl | 15 + .../msg/ClassifiedRoiArray.idl | 14 + .../msg/DetectedObject.idl | 16 + .../msg/DetectedObjectKinematics.idl | 41 ++ .../msg/DetectedObjects.idl | 13 + .../msg/LookingTrafficSignal.idl | 14 + .../msg/ObjectClassification.idl | 24 + .../msg/PointClusters.idl | 18 + .../msg/PointXYZIF.idl | 20 + .../msg/PredictedObject.idl | 20 + .../msg/PredictedObjectKinematics.idl | 18 + .../msg/PredictedObjects.idl | 13 + .../msg/PredictedPath.idl | 15 + .../msg/Shape.idl | 27 + .../msg/TrackedObject.idl | 20 + .../msg/TrackedObjectKinematics.idl | 45 ++ .../msg/TrackedObjects.idl | 13 + .../msg/TrafficLight.idl | 42 ++ .../msg/TrafficLightRoi.idl | 10 + .../msg/TrafficLightRoiArray.idl | 11 + .../msg/TrafficSignal.idl | 19 + .../msg/TrafficSignalArray.idl | 11 + .../msg/TrafficSignalStamped.idl | 11 + .../msg/TrafficSignalWithJudge.idl | 17 + .../autoware_auto_perception_msgs/package.xml | 31 ++ .../CMakeLists.txt | 41 ++ .../action/PlanTrajectory.idl | 27 + .../action/PlannerCostmap.idl | 26 + .../action/RecordTrajectory.idl | 19 + .../action/ReplayTrajectory.idl | 19 + .../msg/HADMapRoute.idl | 27 + .../msg/OrderMovement.idl | 21 + .../autoware_auto_planning_msgs/msg/Path.idl | 15 + .../msg/PathPoint.idl | 26 + .../msg/PathPointWithLaneId.idl | 15 + .../msg/PathWithLaneId.idl | 15 + .../autoware_auto_planning_msgs/msg/Route.idl | 22 + .../msg/Trajectory.idl | 17 + .../msg/TrajectoryPoint.idl | 31 ++ .../autoware_auto_planning_msgs/package.xml | 32 ++ .../srv/ModifyTrajectory.idl | 18 + .../autoware_auto_system_msgs/CMakeLists.txt | 32 ++ .../msg/AutowareState.idl | 26 + .../msg/ControlDiagnostic.idl | 33 ++ .../msg/DiagnosticHeader.idl | 20 + .../msg/DrivingCapability.idl | 22 + .../msg/EmergencyState.idl | 30 + .../msg/Float32MultiArrayDiagnostic.idl | 16 + .../msg/HazardStatus.idl | 49 ++ .../msg/HazardStatusStamped.idl | 18 + .../autoware_auto_system_msgs/package.xml | 29 + .../autoware_auto_vehicle_msgs/CMakeLists.txt | 53 ++ .../msg/ControlModeCommand.idl | 19 + .../msg/ControlModeReport.idl | 24 + .../autoware_auto_vehicle_msgs/msg/Engage.idl | 18 + .../msg/GearCommand.idl | 40 ++ .../msg/GearReport.idl | 40 ++ .../msg/HandBrakeCommand.idl | 16 + .../msg/HandBrakeReport.idl | 13 + .../msg/HazardLightsCommand.idl | 22 + .../msg/HazardLightsReport.idl | 18 + .../msg/HeadlightsCommand.idl | 23 + .../msg/HeadlightsReport.idl | 18 + .../msg/HornCommand.idl | 16 + .../msg/HornReport.idl | 13 + .../msg/RawControlCommand.idl | 21 + .../msg/StationaryLockingCommand.idl | 20 + .../msg/SteeringReport.idl | 17 + .../msg/TurnIndicatorsCommand.idl | 23 + .../msg/TurnIndicatorsReport.idl | 19 + .../msg/VehicleControlCommand.idl | 27 + .../msg/VehicleKinematicState.idl | 18 + .../msg/VehicleOdometry.idl | 20 + .../msg/VehicleStateCommand.idl | 57 ++ .../msg/VehicleStateReport.idl | 50 ++ .../msg/VelocityReport.idl | 20 + .../msg/WheelEncoder.idl | 17 + .../msg/WipersCommand.idl | 38 ++ .../msg/WipersReport.idl | 34 ++ .../autoware_auto_vehicle_msgs/package.xml | 29 + .../srv/AutonomyModeChange.idl | 22 + .../srv/ControlModeCommand.srv | 12 + 100 files changed, 3171 insertions(+) create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl new file mode 100644 index 00000000..dbf8b94f --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl @@ -0,0 +1,20 @@ +module autoware_auto_geometry_msgs { + module msg { + @verbatim (language="comment", text= + " Complex32.msg" "\n" + " Can be used to represent yaw angle for trajectories" "\n" + " To convert back to a yaw angle, see" "\n" + " https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles") + struct Complex32 { + @verbatim (language="comment", text= + " cos(yaw / 2)") + @default (value=1.0) + float real; + + @verbatim (language="comment", text= + " sin(yaw / 2)") + @default (value=0.0) + float imag; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl new file mode 100644 index 00000000..1535d57c --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl @@ -0,0 +1,19 @@ +module autoware_auto_geometry_msgs { + module msg { + @verbatim (language="comment", text= + " Represents rotation, but with less precision than the message in common_interfaces") + struct Quaternion32 { + @default (value=0.0) + float x; + + @default (value=0.0) + float y; + + @default (value=0.0) + float z; + + @default (value=1.0) + float w; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl new file mode 100644 index 00000000..9c79bcdb --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl @@ -0,0 +1,16 @@ +#include "geometry_msgs/msg/Point.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_geometry_msgs { + module msg { + @verbatim (language="comment", text= + " This message is a generalized representation of a 3D pose without" + " orientation of the origin of one frame within a child frame.") + struct RelativePositionWithCovarianceStamped { + std_msgs::msg::Header header; + string child_frame_id; + geometry_msgs::msg::Point position; + double covariance[9]; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt new file mode 100644 index 00000000..6c060600 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt @@ -0,0 +1,26 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_mapping_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/HADMapBin.idl" + "msg/HADMapSegment.idl" + "msg/MapPrimitive.idl" + "srv/HADMapService.idl" + DEPENDENCIES + "std_msgs" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl new file mode 100644 index 00000000..35594899 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl @@ -0,0 +1,32 @@ +#include "std_msgs/msg/Header.idl" + +module autoware_auto_mapping_msgs { + module msg { + module HADMapBin_Constants { + const uint8 MAP_FORMAT_LANELET2 = 0; + }; + @verbatim(language = "comment", text = + "HADMap contents in binary blob format") + struct HADMapBin + { + std_msgs::msg::Header header; + @verbatim(language = "comment", text = + "HADMap format identifier, allows supporting multiple map formats") + uint8 map_format; + + @verbatim(language = "comment", text = + "Version of map format. Keep as empty string if unnecssary") + @default(value = "") + string format_version; + + @verbatim(language = "comment", text = + "Version of map. Keep as empty empty if unnecessary") + @default(value = "") + string map_version; + + @verbatim(language = "comment", text = + "Binary map data") + sequence < uint8 > data; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl new file mode 100644 index 00000000..52ed8208 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl @@ -0,0 +1,12 @@ +#include "autoware_auto_mapping_msgs/msg/MapPrimitive.idl" + +module autoware_auto_mapping_msgs { + module msg { + @verbatim (language="comment", text= + " A segment of an HADMap which contains one or more MapPrimitives.") + struct HADMapSegment { + sequence primitives; + int64 preferred_primitive_id; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl new file mode 100644 index 00000000..6dea38bc --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl @@ -0,0 +1,17 @@ +#include "std_msgs/msg/Header.idl" + +module autoware_auto_mapping_msgs { + module msg { + @verbatim(language = "comment", text = + "Map primitive information") + struct MapPrimitive + { + int64 id; + + @verbatim(language = "comment", text = + "Type of primitive, such as lane, polygon, line.") + @default(value = "") + string primitive_type; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml new file mode 100644 index 00000000..a57f0ca4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml @@ -0,0 +1,27 @@ + + + + autoware_auto_mapping_msgs + 1.0.0 + Interfaces between core Autoware.Auto mapping components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + std_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl new file mode 100644 index 00000000..012cdac6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl @@ -0,0 +1,39 @@ +#include "autoware_auto_mapping_msgs/msg/HADMapBin.idl" + +module autoware_auto_mapping_msgs { + module srv { + // enum type not working yet on ROS2 implementation of idl + // enum HADPrimitive + // { + // FullMap, + // AllPrimitives, + // DriveableGeometry, + // RegulatoryElements, + // StaticObjects + // }; + + module HADMapService_Request_Constants { + const uint8 FULL_MAP = 0; + const uint8 ALL_PRIMITIVES = 1; + const uint8 DRIVEABLE_GEOMETRY = 2; + const uint8 REGULATORY_ELEMENTS = 3; + const uint8 STATIC_OBJECTS = 4; + }; + + struct HADMapService_Request + { + sequence < uint8 > requested_primitives; + @verbatim(language = "comment", text = + "Geometric upper bound of map data requested") + sequence < double, 3 > geom_upper_bound; + @verbatim(language = "comment", text = + "Geometric upper bound of map data requested") + sequence < double, 3 > geom_lower_bound; + }; + struct HADMapService_Response + { + autoware_auto_mapping_msgs::msg::HADMapBin map; + int32 answer; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst new file mode 100644 index 00000000..7e4ae3ea --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst @@ -0,0 +1,57 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package autoware_auto_msgs +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +1.0.0 (2021-01-05) +------------------ +* Merge branch 'BoundingBoxArray-design' into 'master' + Add design doc for bounding-box message + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!12 `_ +* Working around https://github.com/ros-tooling/libstatistics_collector/issues/51. +* Fixing constants in AutonomyModeChange. +* Making AutonomyModeChange return empty. +* Merge branch '657-autonomy-service' into 'master' + Adding AutonomyModeChange service definition. + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!11 `_ +* Adding velocity_mps to VehicleControlCommand. +* Merge branch '474-add-modify-trajectory-service' into 'master' + Adding ModifyTrajectory service definition. + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!10 `_ +* Merge branch 'modify-plan-trajectory-action' into 'master' + modify PlanTrajectory Action to return trajectory in Result + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!9 `_ +* Merge branch 'fix/plan_trajectory_action' into 'master' + fix include file and namespace of constants + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!8 `_ +* Merge branch 'add-actions-dependency' into 'master' + Cleaning up package.xml and adding action_msgs dependency. + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!7 `_ +* Merge branch 'add-plan-trajectory-action' into 'master' + add PlanTrajectory action + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!6 `_ +* Contributors: Frederik Beaujean, Joshua Whitley, mitsudome-r + +0.1.0 (2020-08-07) +------------------ +* Merge branch 'add-hadmap-msgs' into 'master' + had map msgs - respect idl constant naming conventions + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!5 `_ +* Merge branch 'add-hadmap-msgs' into 'master' + Add hadmap msgs + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!4 `_ +* Add hadmap msgs +* Merge branch '3-convert-messages-from-rosmsg-to-idl' into 'master' + Resolve "Convert messages from ROSmsg to IDL" + Closes `#3 `_ + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!2 `_ +* Resolve "Convert messages from ROSmsg to IDL" +* Contributors: Esteve Fernandez, Joshua Whitley, simon-t4 + +0.0.2 (2020-03-02) +------------------ +* Merge branch '1-import-autoware_auto_msgs-from-autoware-auto' into 'master' + Resolve "Import autoware_auto_msgs from Autoware.Auto" + Closes `#1 `_ + See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!1 `_ +* Initial import +* Contributors: Esteve Fernandez, Geoffrey Biggs diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt new file mode 100644 index 00000000..df1cc27d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt @@ -0,0 +1,16 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md new file mode 100644 index 00000000..af820341 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md @@ -0,0 +1,67 @@ +autoware_auto_msgs internal message design +========================= + +[TOC] + +This document is intended to track the design and rationale of messages internal to individual +components/stacks. + +# Perception + +This supersection tracks messages relating to the perception components of the autonomous driving +stack. + +## Object Detection + +This section tracks internal messages needed for the implementation of object detection stacks +that result in the emission of the `BoundingBoxArray` type as a common object list representation. + +### Classical 3d object detection + +This section covers the internal messages needed by a classical 3d object detection stack, +consisting of ground filtering, downsampling, clustering, and finally hull formation. + +#### PointClusters + +``` +sensor_msgs/PointCloud2[] clusters +``` + +This message represents a set of point clusters as a result of object detection or clustering. +`PointCloud2` was used as a cluster can be conceived as a subset of a point cloud, and +`PointCloud2` is the standard representation for point clouds. + + +## Tracking + +This section tracks messages internal to specific implementation of tracking stacks. + +# Localization + +This supersection tracks messages relating to the implementation of localization/mapping +components in the autonomous driving stack. + +# Planning + +This supersection tracks messages relating to the implementation of planning components in the +autonomous driving stack. + +## Global Planning + +This section tracks internal messages needed for the implementation of specific global planning +stacks. + +## Behavior Planning + +This section tracks internal messages needed for the implementation of specific behavior planning +stacks. + +## Motion Planning + +This section tracks internal messages needed for the implementation of specific motion planning +stacks. + +## Control + +This section tracks internal messages needed for the implementation of specific controller +stacks. diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md new file mode 100644 index 00000000..d34ac6a5 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md @@ -0,0 +1,511 @@ +autoware_auto_msgs +================== + +[TOC] + +This document contains the intended sources, recipients and rationale for each of the custom +message types. + +# Helper Types + +The following are helper types that are composed into messages that software components use. + +## Complex32 + +``` +float32 real +float32 imag +``` + +## BoundingBox + +``` +geometry_msgs/Point32 centroid +geometry_msgs/Point32 size +geometry_msgs/Quaternion orientation +geometry_msgs/Point32[4] corners +float32 value +uint32 label +``` +See the [design document](bounding-box-design.md) for further details. + +## BoundingBoxArray + +``` +std_msgs/Header header +BoundingBox[256] boxes +uint32 size +uint32 CAPACITY=256 +``` + + +## DiagnosticHeader + +``` +string name +builtin_interfaces/Time data_stamp +builtin_interfaces/Time computation_start +builtin_interfaces/Duration runtime +uint32 iterations +``` + +## TrajectoryPoint + +``` +builtin_interfaces/Duration time_from_start +float32 x +float32 y +Complex32 heading +float32 longitudinal_velocity_mps 0.0 +float32 lateral_velocity_mps 0.0 +float32 acceleration_mps2 +float32 heading_rate_rps +float32 front_wheel_angle_rad 0.0 +float32 rear_wheel_angle_rad 0.0 +``` + +And the heading field has the following form: + +A zero heading corresponds to the positive x direction in the given coordinate frame. + +**Default Values**: All trajectory point and trajectory fields should default to 0 + +**Default Values**: All complex numbers should default to the pair (1, 0), which corresponds to a +zero heading. + +**Extensions**: In the future, this message may include higher derivative information. + +**Rationale**: Trajectory points mirror, but are not the same as a standard JointTrajectory message. +This is because the standard message allows room for ambiguities whereas this message does not. + +**Rationale**: Higher derivative information (e.g. acceleration, heading rate) is available to +provide a more expressive language for controllers and trajectory planners. If the information +is not needed, a value of zero is provided, which is semantically consistent for controllers and +motion planners + +**Rationale**: Complex numbers are used to represent heading rather than an angle because it reduces +ambiguities between angles, and angle distances. In addition, it conveniently represents a +precomputed sine and cosine values, obviating the need for additional computations. + +For more details, see [Controller design](@ref controller-design) + +# Interface Messages + +## Control Diagnostic + +``` +DiagnosticHeader diag_header +bool new_trajectory +string trajectory_source +string pose_source +float32 lateral_error_m +float32 longitudinal_error_m +float32 velocity_error_mps +float32 acceleration_error_mps2 +float32 yaw_error_rad +float32 yaw_rate_error_rps +``` + +**Source**: Controller + +**Recipient(s)**: + +**Rationale**: Diagnostic information is helpful to detect if a fault occurred, or some incorrect +behavior leading up to a fault. It can also be used for infotainment applications. + +For more details, see [Controller design](@ref controller-design) + +## Trajectory + +``` +std_msgs/Header header +TrajectoryPoint[<=100] points +``` + +**Source**: Motion planner + +**Recipient(s)**: Controller + +**Rationale**: A bounded number of points is provided since a new trajectory should be provided +at a regular rate. Bounded data structures also imply more deterministic communication times +due to having a bounded size. + +For more details, see [Controller design](@ref controller-design) + +### RawControlCommand + +This message type is defined as follows: + +``` +builtin_interfaces/Time stamp +uint32 throttle +uint32 brake +int32 front_steer +int32 rear_steer +``` + +This type is intended to be used when direct control of the vehicle hardware is required. As such, +no assumptions about units are made. It is the responsibility of the system integrator to ensure +that the values used are appropriate for the vehicle platform. + +**Source**: Controller + +**Recipient(s)**: Vehicle Interface + +**Default Values**: The default value for all fields is 0 + +**Rationale**: A time stamp field is provided because a control command should be generated in +response to the input of a vehicle kinematic state. Retaining the time of the triggering data +should aid with diagnostics and traceability. + +**Rationale**: The rear wheel angle is also provided to enable support for rear drive vehicles, +such as forklifts. + +**Rationale**: A frame is not provided since it is implied that it is fixed to the vehicle frame. + +**Rationale**: Throttle and brake are separated as they are fundamentally controlled by two +different actuators + +**Rationale**: This is intended to be compatible with using the triggers of a gamepad as a first +pass for vehicle platform validation. + +**Rationale**: Integer values are used to be more compatible with discrete, or byte-level +representation that is more common with CAN interfaces. + +**Rationale**: Units are not provided since byte-level representations are generally platform- +specific. + +For more details see [vehicle interface design](). + +### HighLevelControlCommand + +This message type is defined as follows: + +``` +builtin_interfaces/Time stamp +# should be negative when reversed +float32 velocity_mps 0.0 +float32 curvature +``` + +This type is intended to be used as an interface when only simple control is required, and thus +abstracts away more direct control of the vehicle. + +**Source**: Controller + +**Recipient(s)**: Vehicle Interface + +**Default Values**: The default value for all fields is 0 + +**Rationale**: A time stamp field is provided because a control command should be generated in +response to the input of a vehicle kinematic state. Retaining the time of the triggering data +should aid with diagnostics and traceability. + +**Rationale**: A frame is not provided since it is implied that it is fixed to the vehicle frame. + +**Rationale**: Curvature is used as a control target as it does not require any vehicle-specific +information. This is inspired by the AutonomousStuff Low Level Controller + +**Rationale**: Velocity is used as a longitudinal control target to allow the vehicle to minimally +follow a geometric reference path. + +For more details see [vehicle interface design](). + + +## Vehicle Control Command +``` +builtin_interfaces/Time stamp +float32 long_accel_mps2 +float32 front_wheel_angle_rad +float32 rear_wheel_angle_rad +``` + +This message type is intended to provide a proxy for a foot on the accelerator and brake pedal, +and a pair of hands on the steering wheel, while also providing a convenient representation +for controller developers. + +These wheel angles are counter-clockwise positive, where 0 corresponds to a neutral, or +forward-facing orientation. + +**Source**: Controller + +**Recipient(s)**: Vehicle Interface + +**Default Values**: The default value for all fields is 0 + +**Rationale**: A time stamp field is provided because a control command should be generated in +response to the input of a vehicle kinematic state. Retaining the time of the triggering data +should aid with diagnostics and traceability. + +**Rationale**: A single acceleration field is provided because controllers typically plan a +longitudinal acceleration and turning angle for the rigid body. Braking is baked into this single +command for simplicity of controller development, and because it is generally not expected for both +the brake to be depressed and the accelerator at the same time. + +**Rationale**: Wheel steering angles are provided since it allows the controller to only require a +simplified view of the vehicle platform, such as knowing the wheelbase length. Depending on the +implementation of a drive-by-wire interface, producing a given wheel angle may require knowledge +of the mapping between steering wheel angle to wheel angle. + +**Rationale**: The rear wheel angle is also provided to enable support for rear drive vehicles, +such as forklifts. + +**Rational**: A frame is not provided since it is implied that it is fixed to the vehicle frame. + +For more details, see [Controller design](@ref controller-design) + +## Vehicle Kinematic State +**Source**: Vehicle interface + +**Recipient(s)**: Behavior Planner +``` +std_msgs/Header header +TrajectoryPoint state +geometry_msgs/Transform delta +``` + +The `TrajectoryPoint::time_from_start` field should also be filled with the time difference between +the current state and the previous state message. + +The position fields of the `TrajectoryPoint` member should contain the position of the ego vehicle +with respect to a fixed frame, if available. If the frame is a dynamic frame (e.g. `base_link`), +then this position field should be taken as unavailable or invalid. The further kinematics of this +member should always be available, and populated according to the appropriate coordinate frame, +making the additional assumption that the coordinate frame is static. + +The delta member is the transform of the message should represent the positional update of the +ego vehicle's coordinate frame relative to it's last position and orientation. + +**Source**: Vehicle State Estimator + +**Recipient(s)**: Planning stack (Route planner, Behavior Planner, Motion Planner, Controller), +Tracker + +**Default Values**: The trajectory point should be appropriately default initialized + +**Default Values**: The delta transform field should have zero translation and the unit quaternion +by default. + +**Extensions**: State and transform uncertainty may be added as variances or covariance matrices. +This may be necessary for probabilistic algorithms (e.g. robust MPC) + +**Rationale**: An underlying trajectory point is used because kinematic information may be needed +by controllers. Further, representing the vehicle state in a manner that mirrors the trajectory +fits with the semantic purpose of a controller: matching the current state to a state sequence + +**Rationale**: A transform relative to the previous position and orientation of the ego vehicle is +provided because while absolute position may not always be available with accuracy, this relative +transform should be available with some degree of accuracy (e.g. via IMU information, odometry +information, etc.). The availability of this information allows for planning and tracking within a +local coordinate frame. For example, in a level 3 highway driving use case, a trajectory may be +rooted at the ego vehicle's position at some time stamp. The accumulation of these relative +transforms would allow for a controller to properly follow the trajectory. + +**Rationale**: The kinematics are populated with a fixed frame assumption to simplify the updates +and usage of this field. + +For more details, see [Controller design](@ref controller-design) + +## Vehicle Odometry + +``` +builtin_interfaces/Time stamp +float32 velocity_mps +float32 front_wheel_angle_rad +float32 rear_wheel_angle_rad +``` + +This message reports the kinematic state of the vehicle as the vehicle itself reports. The intended +use case for this message could be in motion planning for initial conditions, or dead reckoning. + +**Source**: Vehicle interface + +**Recipient(s)**: Vehicle State Estimator + +**Default Values**: The default value for this message should be 0 in all fields. + +**Rationale**: This message is separate from the vehicle state report since they are typically for +distinct use cases (e.g. behavior planning vs dead reckoning) + +**Rationale**: A vehicle is expected to have encoders which provide velocity, and steering angle. +Acceleration and other fields may not be available on all vehicle platforms. + +**Rationale**: A stamp is expected here because this is timely information. In this sense, the +vehicle interface is acting as a sensor driver. + +**Rationale**: A frame id is not provided because these are assumed to be fixed to the vehicle +frame, and heading information is not available. + +## Vehicle State Command + +``` +builtin_interfaces/Time stamp +uint8 blinker +uint8 headlight +uint8 wiper +uint8 gear +uint8 mode +bool hand_brake +bool horn +bool autonomous + +### Definitions +# Blinker +uint8 BLINKER_NO_COMMAND = 0; +uint8 BLINKER_OFF = 1; +uint8 BLINKER_LEFT = 2; +uint8 BLINKER_RIGHT = 3; +uint8 BLINKER_HAZARD = 4; +# Headlight +uint8 HEADLIGHT_NO_COMMAND = 0; +uint8 HEADLIGHT_OFF = 1; +uint8 HEADLIGHT_ON = 2; +uint8 HEADLIGHT_HIGH = 3; +# Wiper +uint8 WIPER_NO_COMMAND = 0; +uint8 WIPER_OFF = 1; +uint8 WIPER_LOW = 2; +uint8 WIPER_HIGH = 3; +uint8 WIPER_CLEAN = 4; +# Gear +uint8 GEAR_NO_COMMAND = 0 +uint8 GEAR_DRIVE = 1 +uint8 GEAR_REVERSE = 2 +uint8 GEAR_PARK = 3 +uint8 GEAR_LOW = 4 +uint8 GEAR_NEUTRAL = 5 +# Mode +uint8 MODE_NO_COMMAND = 0; +uint8 MODE_AUTONOMOUS = 1; +uint8 MODE_MANUAL = 2; +``` + +This message is intended to control the remainder of the vehicle state, e.g. those not required for +minimal collision-free driving. + +**Source**: Behavior Planner + +**Recipient(s)**: Vehicle interface + +**Default Values**: The default value for all fields should be zero. + +**Rationale**: Hazard lights superseded a left/right signal, and as such are an exclusive state + +**Rationale**: Cleaning might be necessary to ensure adequate operation of sensors mounted behind +the wind shield + +**Rationale**: A horn might be required to signal to other drivers + +**Rationale**: Autonomous is a flag because this is a command message: true requests a transition +to autonomous, false requests a disengagement to manual. + +**Rationale**: While additional states are possible for other fields (e.g. headlights, wipers, +gears, etc.), this message only prescribes the minimal set of states that most or all vehicles +can satisfy. + +**Rationale**: Since commands here represent a state transition, a command of NONE denotion "no +state transition" is also valid + + +## Vehicle State Report + +``` +builtin_interfaces/Time stamp +uint8 fuel # 0 to 100 +uint8 blinker +uint8 headlight +uint8 wiper +uint8 gear +uint8 mode +bool hand_brake +bool horn + +### Definitions +# Blinker +uint8 BLINKER_OFF = 1 +uint8 BLINKER_LEFT = 2 +uint8 BLINKER_RIGHT = 3 +uint8 BLINKER_HAZARD = 4 +# Headlight +uint8 HEADLIGHT_OFF = 1 +uint8 HEADLIGHT_ON = 2 +uint8 HEADLIGHT_HIGH = 3 +# Wiper +uint8 WIPER_OFF = 1 +uint8 WIPER_LOW = 2 +uint8 WIPER_HIGH = 3 +uint8 WIPER_CLEAN = 4 +# Gear +uint8 GEAR_DRIVE = 1 +uint8 GEAR_REVERSE = 2 +uint8 GEAR_PARK = 3 +uint8 GEAR_LOW = 4 +uint8 GEAR_NEUTRAL = 5 +# Autonomous +uint8 MODE_AUTONOMOUS = 1 +uint8 MODE_MANUAL = 2 +uint8 MODE_DISENGAGED = 3 +uint8 MODE_NOT_READY = 4 +``` + +**Source**: Vehicle interface + +**Recipient(s)**: Behavior Planner + +**Default Value**: N/A. All fields should be populatable by the vehicle interface. + +**Rationale**: A discrete fuel range is provided as finer granularity is likely unnecessary. + +**Rationale**: A simple state machine is provided to ensure that disambiguate between intentionally +manual, and two modes of unintentionally being in manual mode: due to preconditions not being met, +or due to some failure of the autonomous driving stack. + +**Rationale**: Constants are kept the same as VehicleStateCommand to prevent subtle bugs from being +introduced. The constant 0 is reserved for NO_COMMAND in VehicleStateCommand. + +# References + +- AutowareAuto#62 + +## Related Message Types + +### ROS + +- [JointTrajectory.msg](https://github.com/ros2/common_interfaces/blob/master/trajectory_msgs/msg/JointTrajectoryPoint.msg) +- [PointStamped](http://docs.ros.org/lunar/api/geometry_msgs/html/msg/PointStamped.html) +- [PoseStamped](http://docs.ros.org/lunar/api/geometry_msgs/html/msg/PoseStamped.html) +- [TransformStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/TransformStamped.html) +- [NavSatFix](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/NavSatFix.html) + +### Autoware.AI + +- [AccelCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/AccelCmd.msg) +- [BrakeCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/BrakeCmd.msg) +- [ControlCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/ControlCommand.msg) +- [IndicatorCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/IndicatorCmd.msg) +- [LampCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/LampCmd.msg) +- [RemoteCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/RemoteCmd.msg) +- [SteerCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/SteerCmd.msg) +- [VehicleCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/VehicleCmd.msg) +- [VehicleStatus](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/VehicleStatus.msg) +- [Waypoint](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/Waypoint.msg) + +### Apollo + +Controller/Interface Messages +- [VehicleSignal](https://github.com/ApolloAuto/apollo/blob/fb12723bd6dcba88ecccb6123ea850da1e050171/modules/common/proto/vehicle_signal.proto) +- [Lexus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/canbus/proto/lexus.proto) +- [DriveState](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/common/proto/drive_state.proto) +- [VehicleState](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/common/vehicle_state/proto/vehicle_state.proto) +- [ControlCmd](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/control_cmd.proto) + +Configuration Messages +- [LonControllerConf](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/lon_controller_conf.proto) +- [LatControllerConf](https://github.com/ApolloAuto/apollo/blob/e9156ced04a8f0e372c781d803fd43428ab6c497/modules/control/proto/lat_controller_conf.proto) +- [ControlConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/control_conf.proto) +- [MPCControllerConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/mpc_controller_conf.proto) + +Algorithm Status Messages +- [PlanningStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_status.proto) +- [LocalizationStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/localization/proto/localization_status.proto) +- [PlanningStats](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_stats.proto) diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md new file mode 100644 index 00000000..696fa1df --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md @@ -0,0 +1,224 @@ +BoundingBox message design {#bounding-box-design} +========================== + +# Motivation + +In order to ensure a modular system, it's assumed that object detectors are architecturally +delineated. In other words, software components which can produce instantaneous observations of +objects are delineated from other components in the autonomous driving stack. + +This assumption or decomposition allows us to interchangeably use various forms of object detectors +with various forms of multi-object tracking (by assignment) algorithms. If a tighter coupling +between object detection and tracking is required, specialized combined stacks can be developed. + +In order to facilitate the isolation of object detectors as a module, common messages types are +defined as an interface or output of the object detectors to other components. + + +# Use cases + +The instantaneous detection of objects primarily has three use cases: + +1. Collision detection (as a part of planning) +2. Tracking (which may eventually be an input to planning) +3. Region of Interest identification (as a preprocessing step to localization/mapping) + +In addition, there is the implied use case that this message representation is used on a safety- +critical system. + + +# Requirements + +## Safety-critical systems + +Safety-critical systems require real-time and static memory applications. As such, this implies +that the representation of the message must be upper bounded in size. + +For the purpose of performance, an optional requirement is added to message definitions such that +the maximum size is no more than `64kB`, which is also the maximum size of a single UDP packet. This +requirement ensures that latency is kept to a minimum if interprocess communication is necessary at +the interface of the object detection algorithms. + + +## Collision detection + +In order to detect collisions, the representation must have: + +1. A position at least in 2-space (with the assumption that the object is on the same plane as the +ego) +2. A bounding volume representation which fully contains the detected object +3. A way to ensure the object representation is in a consistent coordinate frame with respect to the +ego +4. Optionally uncertainty representation of the above parameters + + +## Tracking + +The purpose of tracking is to maintain a consistent identity of a detected object over time. +In addition, tracking may also produce state estimates over time, such as kinematic estimates, +shape estimates, or label estimates. + +To satisfy the fundamental requirement of tracking, the message representation must provide features +that can be used for assignment, or association of current observations with previous observations. +Features that can satisfy this requirement include: + +1. Position information +2. Shape information (e.g. convex hull, bounding ellipse/box, shape features, etc.) +3. Object information (e.g. color/intensity distribution, classification label, etc.) +4. Other sensor information (e.g. velocity, heading, turn rate (via FMCW sensors)) +5. Optionally uncertainty information for the above parameters + +Classically, position information is the most important association feature, assuming a continuous +world and a relatively high sampling frequency. + +The other features can be used to motivate various state estimates in the tracking stack. + +## Region of interest detection + +An object detector can also be used to identify regions of interest for various other stacks, such +as feature-based localization/mapping, or scene understanding (sign/scenario detection). + +The localization/mapping use case is largely similar to the tracking use case insofar as it requires +assignment or association of features. In contrast, scene understanding requires an identification +of the relevant objects and their position in a space that is compatible with the ego frame. + +As such, the features needed to satisfy this use case are similar to that of the tracking use case: + +1. Position information +2. Shape/object information +3. Optionally uncertainty information + + +# Message definition + +Based on the requirements laid out, some assumptions and simplifications can be made, and a message +type can be defined. + +If any of the assumptions and simplifications must be broken for the proper operation of the +algorithm, then the choice of algorithm should be reconsidered, or a parallel construct which +encapsulates multiple software components should be made. + +In the case when a stack cannot populate certain fields of the interface, the field should be in +an identifiably non-normal state, such as zero or NaN. + + +## Safety critical requirements + +To satisfy the minimal latency requirement, the representation must be bounded in size to `64kB`. + +If it's additionally assumed that `1kB` is used for secondary representation and there are +piggy-backed submessages in the communication sublayer, the maximum representation size is +`63kB`. + +Next, it's assumed that some maximum number of objects that are detected instantaneously. Based on +current observations of Apex.AI urban driving use cases, typically 50-70 objects are observed per +scene. Applying a healthy safety factor gives a maximum object bound of 256 objects per frame. + +These two assumptions combined lead us to have a maximum representation size of `250B`. + + +## Position requirements + +Minimally, a 3D position is used to satisfy the position requirement inherent in all use cases. A 3D +position is used to be more general, and apply to other stacks, such as a vision/camera-based stack. + +To satisfy the compatibility of representational spaces, it's assumed that all observations are made +in a common frame. This frame can then be represented by a string in the aggregate object type. +It is assumed that it is the responsibility of the consuming algorithm to have and transform the +position space into the correct coordinate frame given this information. + + +## Shape requirements + +Next, the shape of the object is represented as a bounding box in 3 interdependent ways: + +- A quaternion for orientation +- A 3D size parameter +- Four 3D positions for the corners of the bounding box + +A bounding box representation was used because many objects in the autonomous driving use case +are approximately box-shaped. In addition, many efficient algorithms exist in both the 2D and 3D +case to compute bounding boxes. Finally, bounding boxes are convenient, close-formed representations +of objects, as opposed to a convex hull which can be potentially unbounded in representation size. + +A quaternion is used to represent orientation. An oriented bounding box is used as opposed to +axis-aligned bounding boxes because it can better represent and fit to objects. A quaternion +is used to represent orientation in the SO(3) space because it is continuously varying without +singularities. In addition, it can be used to efficiently calculate sine and cosine values. + +Finally, the size and corners of the bounding box is represented. While the two +together represent redundant information, both parameters are byproducts of the bounding box +computation process, and communicating both forms of information downstream can reduce the +computational burden and repeated work of algorithms downstream. + +For example, size can be used as a coarse collision-checking step, whereas the corners are used +for fine-grained collision detection. Similarly, the size of the bounding may be treated as direct +extent observations in the tracking case, whereas the corners may be treated as proxy observations +of the centroid. + + +## Object/Other requirements + +Of all the object features proposed, only a classification label is bounded in representation size +(assuming a finite set of classes). While the other features could be bounded, they would be +severely limited in their representational capabilities due to the size limitations. + +As such, only a classification label is provided with the current form: + +`vehicle_label` is an 8-bit integer depicting the classification label of the bounding box. It +can take any of the following default values: + +``` +NO_LABEL=0 +CAR=1 +PEDESTRIAN=2 +CYCLIST=3 +MOTORCYCLE=4 +``` + +The `signal_label` field contains the back signal state of the car and can take any of the following + default values: + +``` +NO_SIGNAL=0 +LEFT_SIGNAL=1 +RIGHT_SIGNAL=2 +BRAKE=3 +``` + +Next, additional kinematic fields can potentially be populated at minimal size cost. These include +velocity, heading, and heading rate, assuming a FMCW/doppler-effect sensor is available. The +inclusion of such fields can greatly improve the performance of tracking, or in some cases remove +the necessity of tracking (i.e. predictive collision detection). + + +## Uncertainty requirements + +While uncertainty representation is not a strict requirement, the usage of uncertainty can greatly +improve the performance of downstream algorithms, both from the perspective of accuracy, and safety. + +Representing the full probabilistic state of the object is difficult due to size constraints, +as representing an 8-state covariance matrix would overrun the capacity, and +similarly representing the full classification distribution is potentially unbounded in size. + +As a compromise, several modeling simplifications and assumptions can be made for the bonus +representation of uncertainty. + +First, it can be assumed that the uncertainty of state variables are uncorrelated, implying all +off-diagonal elements of the full covariance matrix are zero, and thus need not be represented. +Second, the coarse modeling assumption of a uniformly distributed class assignment residual can +be used. As a result, class uncertainty can be represented with a single value. + + +# Future extensions + +The currently proposed representation for bounding boxes takes up approximately `150B` of the total +size budget of `230B`. This leaves room for up to 18 floating point values. These could be used to +cover the other optional requirements as a vector of floating points, with an optional ID denoting +the intended interpretation. + + +# Related issues + +- #3540: Update bounding box, add use cases +- #4069: Revise new articles for the 0.12.0 release diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml new file mode 100644 index 00000000..a7dc9c31 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml @@ -0,0 +1,27 @@ + + + + autoware_auto_msgs + 1.0.0 + Interfaces between core Autoware.Auto components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + autoware_auto_control_msgs + autoware_auto_geometry_msgs + autoware_auto_mapping_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_system_msgs + autoware_auto_vehicle_msgs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt new file mode 100644 index 00000000..2582b6e6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt @@ -0,0 +1,52 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_perception_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/BoundingBox.idl" + "msg/BoundingBoxArray.idl" + "msg/ClassifiedRoi.idl" + "msg/ClassifiedRoiArray.idl" + "msg/DetectedObject.idl" + "msg/DetectedObjectKinematics.idl" + "msg/DetectedObjects.idl" + "msg/LookingTrafficSignal.idl" + "msg/ObjectClassification.idl" + "msg/PointClusters.idl" + "msg/PointXYZIF.idl" + "msg/PredictedObject.idl" + "msg/PredictedObjectKinematics.idl" + "msg/PredictedObjects.idl" + "msg/PredictedPath.idl" + "msg/Shape.idl" + "msg/TrackedObject.idl" + "msg/TrackedObjectKinematics.idl" + "msg/TrackedObjects.idl" + "msg/TrafficLight.idl" + "msg/TrafficLightRoi.idl" + "msg/TrafficLightRoiArray.idl" + "msg/TrafficSignal.idl" + "msg/TrafficSignalArray.idl" + "msg/TrafficSignalStamped.idl" + "msg/TrafficSignalWithJudge.idl" + DEPENDENCIES + "autoware_auto_geometry_msgs" + "geometry_msgs" + "sensor_msgs" + "std_msgs" + "unique_identifier_msgs" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl new file mode 100644 index 00000000..0dfd11c6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl @@ -0,0 +1,71 @@ +#include "autoware_auto_geometry_msgs/msg/Quaternion32.idl" +#include "geometry_msgs/msg/Point32.idl" + +module autoware_auto_perception_msgs { + module msg { + typedef geometry_msgs::msg::Point32 geometry_msgs__msg__Point32; + typedef geometry_msgs__msg__Point32 geometry_msgs__msg__Point32__4[4]; + typedef float float__8[8]; + module BoundingBox_Constants { + const uint8 NO_LABEL = 0; + const uint8 CAR = 1; + const uint8 PEDESTRIAN = 2; + const uint8 CYCLIST = 3; + const uint8 MOTORCYCLE = 4; + const uint8 NO_SIGNAL = 0; + const uint8 LEFT_SIGNAL = 1; + const uint8 RIGHT_SIGNAL = 2; + const uint8 BRAKE = 3; + const uint32 POSE_X = 0; + const uint32 POSE_Y = 1; + const uint32 VELOCITY = 2; + const uint32 HEADING = 3; + const uint32 TURN_RATE = 4; + const uint32 SIZE_X = 5; + const uint32 SIZE_Y = 6; + const uint32 ACCELERATION = 7; + }; + @verbatim (language="comment", text= + " Oriented bounding box representation") + struct BoundingBox { + geometry_msgs::msg::Point32 centroid; + + geometry_msgs::msg::Point32 size; + + autoware_auto_geometry_msgs::msg::Quaternion32 orientation; + + @default (value=0.0) + float velocity; + + @default (value=0.0) + float heading; + + @default (value=0.0) + float heading_rate; + + geometry_msgs__msg__Point32__4 corners; + + float__8 variance; + + @verbatim (language="comment", text= + " can hold arbitrary value, e.g. likelihood, area, perimeter") + float value; + + @verbatim (language="comment", text= + " can hold one of the vehicle constants defined below" "\n" + " NO_LABEL as default value") + @default (value=0) + uint8 vehicle_label; + + @verbatim (language="comment", text= + " can hold one of the signal constants defined below" "\n" + " NO_SIGNAL as default value") + @default (value=0) + uint8 signal_label; + + @verbatim (language="comment", text= + " Likelihood of vehicle label") + float class_likelihood; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl new file mode 100644 index 00000000..cedcaf30 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl @@ -0,0 +1,17 @@ +#include "autoware_auto_perception_msgs/msg/BoundingBox.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + module BoundingBoxArray_Constants { + const uint32 CAPACITY = 256; + }; + @verbatim (language="comment", text= + " Message for a full set of bounding boxes") + struct BoundingBoxArray { + std_msgs::msg::Header header; + + sequence boxes; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl new file mode 100644 index 00000000..88ff9d24 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl @@ -0,0 +1,15 @@ +#include "geometry_msgs/msg/Polygon.idl" +#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text="A region of interest in an image with class information.") + struct ClassifiedRoi { + @verbatim (language="comment", text="A vector of possible classifications of this object.") + sequence classifications; + + @verbatim (language="comment", text="A 2D polygon describing the outline of an object in image coordinates.") + geometry_msgs::msg::Polygon polygon; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl new file mode 100644 index 00000000..1b58dd77 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl @@ -0,0 +1,14 @@ +#include "autoware_auto_perception_msgs/msg/ClassifiedRoi.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " Message for a full set of classified ROIs") + struct ClassifiedRoiArray { + std_msgs::msg::Header header; + + sequence rois; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl new file mode 100644 index 00000000..151e2c65 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl @@ -0,0 +1,16 @@ +#include "autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl" +#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" +#include "autoware_auto_perception_msgs/msg/Shape.idl" + +module autoware_auto_perception_msgs { + module msg { + struct DetectedObject { + @range (min=0.0, max=1.0) + float existence_probability; + + sequence classification; + autoware_auto_perception_msgs::msg::DetectedObjectKinematics kinematics; + autoware_auto_perception_msgs::msg::Shape shape; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl new file mode 100644 index 00000000..216cb667 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl @@ -0,0 +1,41 @@ +#include "geometry_msgs/msg/Point.idl" +#include "geometry_msgs/msg/Quaternion.idl" +#include "geometry_msgs/msg/TwistWithCovariance.idl" + +module autoware_auto_perception_msgs { + module msg { + module DetectedObjectKinematics_Constants { + /** + * Only position is available, orientation is empty. Note that the shape can be an oriented + * bounding box but the direction the object is facing is unknown, in which case + * orientation should be empty. + */ + const uint8 UNAVAILABLE = 0; + /** + * The orientation is determined only up to a sign flip. For instance, assume that cars are + * longer than they are wide, and the perception pipeline can accurately estimate the + * dimensions of a car. It should set the orientation to coincide with the major axis, with + * the sign chosen arbitrarily, and use this tag to signify that the orientation could + * point to the front or the back. + */ + const uint8 SIGN_UNKNOWN = 1; + /** + * The full orientation is available. Use e.g. for machine-learning models that can + * differentiate between the front and back of a vehicle. + */ + const uint8 AVAILABLE = 2; + }; + + struct DetectedObjectKinematics { + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + + boolean has_position_covariance; + uint8 orientation_availability; + + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + + boolean has_twist; + boolean has_twist_covariance; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl new file mode 100644 index 00000000..c257bb4d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl @@ -0,0 +1,13 @@ +#include "autoware_auto_perception_msgs/msg/DetectedObject.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " This is the output of object detection and the input to tracking.") + struct DetectedObjects { + std_msgs::msg::Header header; + sequence objects; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl new file mode 100644 index 00000000..ed82595a --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl @@ -0,0 +1,14 @@ +#include "autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + struct LookingTrafficSignal { + std_msgs::msg::Header header; + boolean is_module_running; + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge perception; + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge external; + autoware_auto_perception_msgs::msg::TrafficSignalWithJudge result; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl new file mode 100644 index 00000000..905c13e3 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl @@ -0,0 +1,24 @@ +module autoware_auto_perception_msgs { + module msg { + module ObjectClassification_Constants { + const uint8 UNKNOWN = 0; + const uint8 CAR = 1; + const uint8 TRUCK = 2; + const uint8 BUS = 3; + const uint8 TRAILER = 4; + const uint8 MOTORCYCLE = 5; + const uint8 BICYCLE = 6; + const uint8 PEDESTRIAN = 7; + }; + + struct ObjectClassification { + @verbatim (language="comment", text= + " Valid values for the label field are provided in" + " ObjectClassification_Constants.") + uint8 label; + + @range (min=0.0, max=1.0) + float probability; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl new file mode 100644 index 00000000..aad5cce1 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl @@ -0,0 +1,18 @@ +#include "autoware_auto_perception_msgs/msg/PointXYZIF.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " Represent the clusters" "\n" + " The cluster 0 is from point 0 to point cluster_boundary[0] - 1." "\n" + " Cluster i would be from point cluster_boundary[i - 1] to cluster_boundary[i] - 1, and so on") + struct PointClusters { + std_msgs::msg::Header header; + + sequence points; + + sequence cluster_boundary; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl new file mode 100644 index 00000000..14429ec7 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl @@ -0,0 +1,20 @@ +module autoware_auto_perception_msgs { + module msg { + module PointXYZIF_Constants { + const uint16 END_OF_SCAN_ID = 65535; + }; + @verbatim (language="comment", text= + " This message is meant to mirror autoware::common::types::PointXYZIF") + struct PointXYZIF { + float x; + + float y; + + float z; + + float intensity; + + uint16 id; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl new file mode 100644 index 00000000..ed611086 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl @@ -0,0 +1,20 @@ +#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" +#include "autoware_auto_perception_msgs/msg/Shape.idl" +#include "autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl" +#include "unique_identifier_msgs/msg/UUID.idl" + +module autoware_auto_perception_msgs { + module msg { + struct PredictedObject { + unique_identifier_msgs::msg::UUID object_id; + + @range (min=0.0, max=1.0) + float existence_probability; + + sequence classification; + autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics; + + autoware_auto_perception_msgs::msg::Shape shape; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl new file mode 100644 index 00000000..1cfed5e7 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl @@ -0,0 +1,18 @@ +#include "autoware_auto_perception_msgs/msg/PredictedPath.idl" +#include "geometry_msgs/msg/AccelWithCovariance.idl" +#include "geometry_msgs/msg/PoseWithCovariance.idl" +#include "geometry_msgs/msg/TwistWithCovariance.idl" + +module autoware_auto_perception_msgs { + module msg { + struct PredictedObjectKinematics { + geometry_msgs::msg::PoseWithCovariance initial_pose_with_covariance; + + geometry_msgs::msg::TwistWithCovariance initial_twist_with_covariance; + + geometry_msgs::msg::AccelWithCovariance initial_acceleration_with_covariance; + + sequence predicted_paths; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl new file mode 100644 index 00000000..f3d2ad66 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl @@ -0,0 +1,13 @@ +#include "autoware_auto_perception_msgs/msg/PredictedObject.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " This is the output of object prediction.") + struct PredictedObjects { + std_msgs::msg::Header header; + sequence objects; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl new file mode 100644 index 00000000..628cf6f0 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl @@ -0,0 +1,15 @@ +#include "builtin_interfaces/msg/Duration.idl" +#include "geometry_msgs/msg/Pose.idl" + +module autoware_auto_perception_msgs { + module msg { + struct PredictedPath { + sequence path; + + @verbatim (language="comment", text= + " The time_step field defines the interval between consecutive pose predictions in the path array.") + builtin_interfaces::msg::Duration time_step; + float confidence; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl new file mode 100644 index 00000000..94e88696 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl @@ -0,0 +1,27 @@ +#include "geometry_msgs/msg/Polygon.idl" + +module autoware_auto_perception_msgs { + module msg { + module Shape_Constants { + const uint8 BOUNDING_BOX=0; + const uint8 CYLINDER=1; + const uint8 POLYGON=2; + }; + + struct Shape { + @verbatim (language="comment", text= + " Type of the shape") + uint8 type; + + @verbatim (language="comment", text= + " The contour of the shape (POLYGON)") + geometry_msgs::msg::Polygon footprint; + + @verbatim (language="comment", text= + " x: the length of the object (BOUNDING_BOX) or diameter (CYLINDER)" + " y: the width of the object (BOUNDING_BOX)" + " z: the overall height of the object") + geometry_msgs::msg::Vector3 dimensions; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl new file mode 100644 index 00000000..b61cfe1c --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl @@ -0,0 +1,20 @@ +#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" +#include "autoware_auto_perception_msgs/msg/Shape.idl" +#include "autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl" +#include "unique_identifier_msgs/msg/UUID.idl" + +module autoware_auto_perception_msgs { + module msg { + struct TrackedObject { + unique_identifier_msgs::msg::UUID object_id; + + @range (min=0.0, max=1.0) + float existence_probability; + + sequence classification; + autoware_auto_perception_msgs::msg::TrackedObjectKinematics kinematics; + + autoware_auto_perception_msgs::msg::Shape shape; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl new file mode 100644 index 00000000..115d7c06 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl @@ -0,0 +1,45 @@ +#include "geometry_msgs/msg/AccelWithCovariance.idl" +#include "geometry_msgs/msg/Point.idl" +#include "geometry_msgs/msg/Quaternion.idl" +#include "geometry_msgs/msg/TwistWithCovariance.idl" + +module autoware_auto_perception_msgs { + module msg { + module TrackedObjectKinematics_Constants { + /** + * Only position is available, orientation is empty. Note that the shape can be an oriented + * bounding box but the direction the object is facing is unknown, in which case + * orientation should be empty. + */ + const uint8 UNAVAILABLE = 0; + /** + * The orientation is determined only up to a sign flip. For instance, assume that cars are + * longer than they are wide, and the perception pipeline can accurately estimate the + * dimensions of a car. It should set the orientation to coincide with the major axis, with + * the sign chosen arbitrarily, and use this tag to signify that the orientation could + * point to the front or the back. + */ + const uint8 SIGN_UNKNOWN = 1; + /** + * The full orientation is available. Use e.g. for machine-learning models that can + * differentiate between the front and back of a vehicle. + */ + const uint8 AVAILABLE = 2; + }; + + struct TrackedObjectKinematics { + @verbatim (language="comment", text= + " Pose covariance is always provided by tracking.") + geometry_msgs::msg::PoseWithCovariance pose_with_covariance; + + uint8 orientation_availability; + + geometry_msgs::msg::TwistWithCovariance twist_with_covariance; + + geometry_msgs::msg::AccelWithCovariance acceleration_with_covariance; + + @value (default=False) + boolean is_stationary; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl new file mode 100644 index 00000000..4be6c44b --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl @@ -0,0 +1,13 @@ +#include "autoware_auto_perception_msgs/msg/TrackedObject.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " This is the output of object tracking and the input to prediction.") + struct TrackedObjects { + std_msgs::msg::Header header; + sequence objects; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl new file mode 100644 index 00000000..844e3aef --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl @@ -0,0 +1,42 @@ +module autoware_auto_perception_msgs { + module msg { + module TrafficLight_Constants { + // constants for color + const uint8 RED = 1; + const uint8 AMBER = 2; + const uint8 GREEN = 3; + const uint8 WHITE = 4; + + // constants for shape + const uint8 CIRCLE = 5; + const uint8 LEFT_ARROW = 6; + const uint8 RIGHT_ARROW = 7; + const uint8 UP_ARROW = 8; + const uint8 DOWN_ARROW = 9; + const uint8 DOWN_LEFT_ARROW = 10; + const uint8 DOWN_RIGHT_ARROW = 11; + const uint8 CROSS = 12; + + // constants for status + const uint8 SOLID_OFF = 13; + const uint8 SOLID_ON = 14; + const uint8 FLASHING = 15; + + // constants for common use + const uint8 UNKNOWN = 16; + }; + struct TrafficLight { + @default (value=0) + uint8 color; + + @default (value=0) + uint8 shape; + + @default (value=0) + uint8 status; + + @default (value=0.0) + float confidence; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl new file mode 100644 index 00000000..10976cb4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl @@ -0,0 +1,10 @@ +#include "sensor_msgs/msg/RegionOfInterest.idl" + +module autoware_auto_perception_msgs { + module msg { + struct TrafficLightRoi { + sensor_msgs::msg::RegionOfInterest roi; + int32 id; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl new file mode 100644 index 00000000..094f485d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl @@ -0,0 +1,11 @@ +#include "autoware_auto_perception_msgs/msg/TrafficLightRoi.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + struct TrafficLightRoiArray { + std_msgs::msg::Header header; + sequence rois; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl new file mode 100644 index 00000000..ef17c012 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl @@ -0,0 +1,19 @@ +#include "autoware_auto_perception_msgs/msg/TrafficLight.idl" + +module autoware_auto_perception_msgs { + module msg { + @verbatim (language="comment", text= + " A TrafficSignal is defined here as a group of multiple TrafficLights" "\n" + " which each represent a single light, indicator, or bulb.") + struct TrafficSignal { + @verbatim (language="comment", text= + " A value of 0 indicates an invalid map_primitive_id. Signals which are not" + " associated with map primitives should not be used in planning because this" + " indicates that the required signal-to-lane mapping is not available.") + @default (value=0) + int32 map_primitive_id; + + sequence lights; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl new file mode 100644 index 00000000..22de5791 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl @@ -0,0 +1,11 @@ +#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + struct TrafficSignalArray { + std_msgs::msg::Header header; + sequence signals; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl new file mode 100644 index 00000000..31a47674 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl @@ -0,0 +1,11 @@ +#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_perception_msgs { + module msg { + struct TrafficSignalStamped { + std_msgs::msg::Header header; + autoware_auto_perception_msgs::msg::TrafficSignal signal; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl new file mode 100644 index 00000000..45c61c25 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl @@ -0,0 +1,17 @@ +#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" + +module autoware_auto_perception_msgs { + module msg { + module TrafficSignalWithJudge_Constants { + const uint8 JUDGE = 1; + const uint8 NONE = 2; + const uint8 STOP = 3; + const uint8 GO = 4; + }; + struct TrafficSignalWithJudge { + uint8 judge; + boolean has_state; + autoware_auto_perception_msgs::msg::TrafficSignal signal; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml new file mode 100644 index 00000000..6161f058 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml @@ -0,0 +1,31 @@ + + + + autoware_auto_perception_msgs + 1.0.0 + Interfaces between core Autoware.Auto perception components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + autoware_auto_geometry_msgs + geometry_msgs + sensor_msgs + std_msgs + unique_identifier_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt new file mode 100644 index 00000000..41006d16 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt @@ -0,0 +1,41 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_planning_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "action/PlannerCostmap.idl" + "action/PlanTrajectory.idl" + "action/RecordTrajectory.idl" + "action/ReplayTrajectory.idl" + "msg/HADMapRoute.idl" + "msg/OrderMovement.idl" + "msg/Route.idl" + "msg/Trajectory.idl" + "msg/TrajectoryPoint.idl" + "msg/Path.idl" + "msg/PathPoint.idl" + "msg/PathWithLaneId.idl" + "msg/PathPointWithLaneId.idl" + "srv/ModifyTrajectory.idl" + DEPENDENCIES + "autoware_auto_geometry_msgs" + "autoware_auto_mapping_msgs" + "builtin_interfaces" + "geometry_msgs" + "nav_msgs" + "std_msgs" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl new file mode 100644 index 00000000..a59c73f2 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl @@ -0,0 +1,27 @@ +#include "autoware_auto_planning_msgs/msg/HADMapRoute.idl" +#include "autoware_auto_planning_msgs/msg/Trajectory.idl" + +module autoware_auto_planning_msgs { + module action { + module PlanTrajectory_Result_Constants { + const uint8 SUCCESS = 0; + const uint8 FAIL = 1; + }; + struct PlanTrajectory_Goal { + autoware_auto_planning_msgs::msg::HADMapRoute sub_route; + }; + + struct PlanTrajectory_Result { + @verbatim(language = "comment", text = + "Report of end condition. Value should be one of PlanTrajectory_Constants") + uint8 result; + autoware_auto_planning_msgs::msg::Trajectory trajectory; + }; + + struct PlanTrajectory_Feedback { + @verbatim(language = "comment", text = + "Currently we don't need feedback, but we need some variable to compile") + uint8 unused_variable; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl new file mode 100644 index 00000000..2fd8dca8 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl @@ -0,0 +1,26 @@ +#include "autoware_auto_planning_msgs/msg/HADMapRoute.idl" +#include "nav_msgs/msg/OccupancyGrid.idl" +#include "std_msgs/msg/Empty.idl" + +module autoware_auto_planning_msgs { + module action { + struct PlannerCostmap_Goal + { + @verbatim(language = "comment", text = + "Route defined by start and goal point and map primitives" + "between given points.") + autoware_auto_planning_msgs::msg::HADMapRoute route; + }; + struct PlannerCostmap_Result + { + @verbatim(language = "comment", text = + "Costmap with obstacles and lanelets position applied") + nav_msgs::msg::OccupancyGrid costmap; + }; + struct PlannerCostmap_Feedback { + @verbatim(language = "comment", text = + "Currently there is no feedback, but variable is needed to compile") + std_msgs::msg::Empty unused_variable; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl new file mode 100644 index 00000000..2697cdb6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl @@ -0,0 +1,19 @@ +module autoware_auto_planning_msgs { + module action { + struct RecordTrajectory_Goal { + @verbatim(language = "comment", text = + "A path to the CSV file which will be created to save the recorded trajectory.") + string record_path; + }; + + struct RecordTrajectory_Result { + @verbatim(language = "comment", text = + "This action has no result but we must provide something here.") + boolean unused_flag; + }; + + struct RecordTrajectory_Feedback { + int32 current_length; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl new file mode 100644 index 00000000..ab74fddf --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl @@ -0,0 +1,19 @@ +module autoware_auto_planning_msgs { + module action { + struct ReplayTrajectory_Goal { + @verbatim(language = "comment", text = + "A path to the CSV file which contains the recorded trajectory.") + string replay_path; + }; + + struct ReplayTrajectory_Result { + @verbatim(language = "comment", text = + "This action has no result but we must provide something here.") + boolean unused_flag; + }; + + struct ReplayTrajectory_Feedback { + int32 remaining_length; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl new file mode 100644 index 00000000..fb49654c --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl @@ -0,0 +1,27 @@ +#include "autoware_auto_mapping_msgs/msg/HADMapSegment.idl" +#include "geometry_msgs/msg/Pose.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language="comment", text= + " A route within a high-definition map defined by" + " the start and goal points and map primitives" + " describing the route between the two.") + struct HADMapRoute { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + " The start_pose must exist within the bounds of the primitives in the first" + " segment defined in the route_segments array.") + geometry_msgs::msg::Pose start_pose; + + @verbatim (language="comment", text= + " The goal_pose must exist within the bounds of the primitives in the last" + " segment defined in the route_semgents array.") + geometry_msgs::msg::Pose goal_pose; + + sequence segments; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl new file mode 100644 index 00000000..6d3ea20b --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl @@ -0,0 +1,21 @@ +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + module OrderMovement_Constants { + const uint8 NOTSET = 0; + const uint8 STOP = 1; + const uint8 GO = 2; + const uint8 SLOWDOWN = 3; + }; + + @verbatim (language="comment", text= + "Movement order for planner to follow") + struct OrderMovement { + std_msgs::msg::Header header; + + @default (value=0) + uint8 order_movement; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl new file mode 100644 index 00000000..f4808810 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl @@ -0,0 +1,15 @@ +#include "autoware_auto_planning_msgs/msg/PathPoint.idl" +#include "nav_msgs/msg/OccupancyGrid.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language = "comment", text= + "Contains a PathPoint path and an OccupancyGrid of drivable_area.") + struct Path { + std_msgs::msg::Header header; + sequence points; + nav_msgs::msg::OccupancyGrid drivable_area; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl new file mode 100644 index 00000000..1ce0ea06 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl @@ -0,0 +1,26 @@ +#include "geometry_msgs/msg/Pose.idl" +#include "geometry_msgs/msg/Twist.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language = "comment", text= + "Represents a pose from a lanelet map, contains twist information.") + struct PathPoint { + geometry_msgs::msg::Pose pose; + + @default (value=0.0) + float longitudinal_velocity_mps; + + @default (value=0.0) + float lateral_velocity_mps; + + @default (value=0.0) + float heading_rate_rps; + + @verbatim(language = "comment", text = + "Denotes that the point is final, doesn't need further updates.") + @default (value = FALSE) + boolean is_final; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl new file mode 100644 index 00000000..500c3e73 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl @@ -0,0 +1,15 @@ +#include "autoware_auto_planning_msgs/msg/PathPoint.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language = "comment", text= + "Contains a PathPoint and lanelet lane_id information.") + struct PathPointWithLaneId { + autoware_auto_planning_msgs::msg::PathPoint point; + + @verbatim(language = "comment", text = + "Lanelet lane_id information.") + sequence lane_ids; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl new file mode 100644 index 00000000..d87dd61a --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl @@ -0,0 +1,15 @@ +#include "autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl" +#include "nav_msgs/msg/OccupancyGrid.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language = "comment", text= + "Contains a PathPointWithLaneId path and an OccupancyGrid of drivable_area.") + struct PathWithLaneId { + std_msgs::msg::Header header; + sequence points; + nav_msgs::msg::OccupancyGrid drivable_area; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl new file mode 100644 index 00000000..76e0fa3f --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl @@ -0,0 +1,22 @@ +#include "autoware_auto_mapping_msgs/msg/MapPrimitive.idl" +#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + module Route_Constants { + const uint32 CAPACITY = 100; + }; + @verbatim (language="comment", text= + "Global route information for the planner") + struct Route { + std_msgs::msg::Header header; + + autoware_auto_planning_msgs::msg::TrajectoryPoint start_point; + + autoware_auto_planning_msgs::msg::TrajectoryPoint goal_point; + + sequence primitives; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl new file mode 100644 index 00000000..4f9381c4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl @@ -0,0 +1,17 @@ +#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_planning_msgs { + module msg { + module Trajectory_Constants { + const uint32 CAPACITY = 10000; + }; + @verbatim (language="comment", text= + " A set of trajectory points for the controller") + struct Trajectory { + std_msgs::msg::Header header; + + sequence points; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl new file mode 100644 index 00000000..d07bdf90 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl @@ -0,0 +1,31 @@ +#include "builtin_interfaces/msg/Duration.idl" + +module autoware_auto_planning_msgs { + module msg { + @verbatim (language="comment", text= + " Representation of a trajectory point for the controller") + struct TrajectoryPoint { + builtin_interfaces::msg::Duration time_from_start; + + geometry_msgs::msg::Pose pose; + + @default (value=0.0) + float longitudinal_velocity_mps; + + @default (value=0.0) + float lateral_velocity_mps; + + @default (value=0.0) + float acceleration_mps2; + + @default (value=0.0) + float heading_rate_rps; + + @default (value=0.0) + float front_wheel_angle_rad; + + @default (value=0.0) + float rear_wheel_angle_rad; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml new file mode 100644 index 00000000..ee143ba9 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml @@ -0,0 +1,32 @@ + + + + autoware_auto_planning_msgs + 1.0.0 + Interfaces between core Autoware.Auto planning components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + autoware_auto_geometry_msgs + autoware_auto_mapping_msgs + builtin_interfaces + geometry_msgs + nav_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl new file mode 100644 index 00000000..23689287 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl @@ -0,0 +1,18 @@ +#include "autoware_auto_planning_msgs/msg/Trajectory.idl" + +module autoware_auto_planning_msgs { + module srv { + struct ModifyTrajectory_Request + { + @verbatim(language = "comment", text = + "Trajectory to be modified") + autoware_auto_planning_msgs::msg::Trajectory original_trajectory; + }; + struct ModifyTrajectory_Response + { + @verbatim(language = "comment", text = + "Trajectory after modification") + autoware_auto_planning_msgs::msg::Trajectory modified_trajectory; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt new file mode 100644 index 00000000..fb334ae8 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt @@ -0,0 +1,32 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_system_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AutowareState.idl" + "msg/ControlDiagnostic.idl" + "msg/DiagnosticHeader.idl" + "msg/DrivingCapability.idl" + "msg/EmergencyState.idl" + "msg/Float32MultiArrayDiagnostic.idl" + "msg/HazardStatus.idl" + "msg/HazardStatusStamped.idl" + DEPENDENCIES + "builtin_interfaces" + "diagnostic_msgs" + "std_msgs" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl new file mode 100644 index 00000000..bbe0c3da --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl @@ -0,0 +1,26 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_system_msgs { + module msg { + module AutowareState_Constants { + const uint8 INITIALIZING = 1; + const uint8 WAITING_FOR_ROUTE = 2; + const uint8 PLANNING = 3; + const uint8 WAITING_FOR_ENGAGE = 4; + const uint8 DRIVING = 5; + const uint8 ARRIVED_GOAL = 6; + const uint8 FINALIZING = 7; + }; + + @verbatim (language="comment", text= + " A message for reporting the Autoware system status.") + struct AutowareState { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Current state of the Autoware system.") + uint8 state; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl new file mode 100644 index 00000000..524d9ad0 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl @@ -0,0 +1,33 @@ +#include "autoware_auto_system_msgs/msg/DiagnosticHeader.idl" + +module autoware_auto_system_msgs { + module msg { + @verbatim (language="comment", text= + " Diagnostic information for the controller") + struct ControlDiagnostic { + autoware_auto_system_msgs::msg::DiagnosticHeader diag_header; + + @verbatim (language="comment", text= + " Controller specific information") + boolean new_trajectory; + + string<256> trajectory_source; + + string<256> pose_source; + + @verbatim (language="comment", text= + " the error between the current vehicle and the nearest neighbor point") + float lateral_error_m; + + float longitudinal_error_m; + + float velocity_error_mps; + + float acceleration_error_mps2; + + float yaw_error_rad; + + float yaw_rate_error_rps; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl new file mode 100644 index 00000000..27686038 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl @@ -0,0 +1,20 @@ +#include "builtin_interfaces/msg/Duration.idl" +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_system_msgs { + module msg { + @verbatim (language="comment", text= + " Base information that all diagnostic messages should have") + struct DiagnosticHeader { + string<256> name; + + builtin_interfaces::msg::Time data_stamp; + + builtin_interfaces::msg::Time computation_start; + + builtin_interfaces::msg::Duration runtime; + + uint32 iterations; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl new file mode 100644 index 00000000..0066cf51 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl @@ -0,0 +1,22 @@ +#include "builtin_interfaces/msg/Time.idl" +#include "autoware_auto_system_msgs/msg/HazardStatus.idl" + +module autoware_auto_system_msgs { + module msg { + + @verbatim (language="comment", text= + " A status message for reporting the vehicle driving capabilities.") + struct DrivingCapability { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Status for the autonomous driving mode.") + autoware_auto_system_msgs::msg::HazardStatus autonomous_driving; + + @verbatim (language="comment", text= + " Status for the remote control mode.") + autoware_auto_system_msgs::msg::HazardStatus remote_control; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl new file mode 100644 index 00000000..709ff59d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl @@ -0,0 +1,30 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_system_msgs { + module msg { + + module EmergencyState_Constants { + const uint8 NORMAL = 1; + const uint8 OVERRIDE_REQUESTING = 2; + const uint8 MRM_OPERATING = 3; + const uint8 MRM_SUCCEEDED = 4; + const uint8 MRM_FAILED = 5; + }; + + @verbatim (language="comment", text= + " Message for reporting the emergency state.") + struct EmergencyState { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Current emergency state fo the system. Possible states are as follows." + " - NORMAL - the system is not in emergency mode." + " - OVERRIDE_REQUESTING - the override is requesting." + " - MRM_OPERATING - during the minimal risk maneuver (MRM)" + " - MRM_SUCCEEDED - MRM operation succeeded." + " - MRM_FAILED - MRM operation failed.") + uint8 state; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl new file mode 100644 index 00000000..ec9b0b9d --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl @@ -0,0 +1,16 @@ +#include "autoware_auto_system_msgs/msg/DiagnosticHeader.idl" +#include "std_msgs/msg/Float32MultiArray.idl" + +module autoware_auto_system_msgs { + module msg { + @verbatim (language="comment", text= + " Generic diagnostic information to be used for debugging purposes") + struct Float32MultiArrayDiagnostic { + autoware_auto_system_msgs::msg::DiagnosticHeader diag_header; + + @verbatim (language="comment", text= + " Debug information as a Float32MultiArray") + std_msgs::msg::Float32MultiArray diag_array; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl new file mode 100644 index 00000000..057ada72 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl @@ -0,0 +1,49 @@ +#include "diagnostic_msgs/msg/DiagnosticStatus.idl" + +module autoware_auto_system_msgs { + module msg { + module HazardStatus_Constants { + const uint8 NO_FAULT = 0; + const uint8 SAFE_FAULT = 1; + const uint8 LATENT_FAULT = 2; + const uint8 SINGLE_POINT_FAULT = 3; + }; + + @verbatim (language="comment", text= + " A message for reporting the hazard status.") + struct HazardStatus { + + @verbatim (language="comment", text= + " Determines the hazard level.") + @default (value=0) + uint8 level; + + @verbatim (language="comment", text= + " Determines whether the vehicle is in the emergency state.") + @default (value=FALSE) + boolean emergency; + + @verbatim (language="comment", text= + " Determines whether the vehicle emergency state should be held.") + @default (value=FALSE) + boolean emergency_holding; + + @verbatim (language="comment", text= + " Diagnostics categorized as no fault.") + sequence diag_no_fault; + + @verbatim (language="comment", text= + " Diagnostics categorized as safe fault.") + sequence diag_safe_fault; + + @verbatim (language="comment", text= + " Diagnostics categorized as latent fault.") + sequence diag_latent_fault; + + @verbatim (language="comment", text= + " Diagnostics categorized as single point fault.") + sequence diag_single_point_fault; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl new file mode 100644 index 00000000..91a42b67 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl @@ -0,0 +1,18 @@ +#include "builtin_interfaces/msg/Time.idl" +#include "autoware_auto_system_msgs/msg/HazardStatus.idl" + +module autoware_auto_system_msgs { + module msg { + + @verbatim (language="comment", text= + " A message for reporting the hazard status with timestamp.") + struct HazardStatusStamped { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Contains the hazard status with diagnostics information.") + autoware_auto_system_msgs::msg::HazardStatus status; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml new file mode 100644 index 00000000..a017f628 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml @@ -0,0 +1,29 @@ + + + + autoware_auto_system_msgs + 1.0.0 + Interfaces between core Autoware.Auto system components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + builtin_interfaces + diagnostic_msgs + std_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt new file mode 100644 index 00000000..cece6a66 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt @@ -0,0 +1,53 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_vehicle_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/ControlModeCommand.idl" + "msg/ControlModeReport.idl" + "msg/Engage.idl" + "msg/GearCommand.idl" + "msg/GearReport.idl" + "msg/HandBrakeCommand.idl" + "msg/HandBrakeReport.idl" + "msg/HazardLightsCommand.idl" + "msg/HazardLightsReport.idl" + "msg/HeadlightsCommand.idl" + "msg/HeadlightsReport.idl" + "msg/HornCommand.idl" + "msg/HornReport.idl" + "msg/RawControlCommand.idl" + "msg/StationaryLockingCommand.idl" + "msg/SteeringReport.idl" + "msg/TurnIndicatorsCommand.idl" + "msg/TurnIndicatorsReport.idl" + "msg/VehicleControlCommand.idl" + "msg/VehicleKinematicState.idl" + "msg/VehicleOdometry.idl" + "msg/VehicleStateCommand.idl" + "msg/VehicleStateReport.idl" + "msg/VelocityReport.idl" + "msg/WheelEncoder.idl" + "msg/WipersCommand.idl" + "msg/WipersReport.idl" + "srv/AutonomyModeChange.idl" + "srv/ControlModeCommand.srv" + DEPENDENCIES + "autoware_auto_planning_msgs" + "builtin_interfaces" + "std_msgs" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl new file mode 100644 index 00000000..c365de69 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl @@ -0,0 +1,19 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module ControlModeCommand_Constants { + const uint8 NO_COMMAND = 0; + const uint8 AUTONOMOUS = 1; + const uint8 MANUAL = 2; + }; + @verbatim (language="comment", text= + " ControlModeCommand.msg") + struct ControlModeCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 mode; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl new file mode 100644 index 00000000..3ab71f50 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl @@ -0,0 +1,24 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module ControlModeReport_Constants { + const uint8 NO_COMMAND = 0; + const uint8 AUTONOMOUS = 1; + const uint8 AUTONOMOUS_STEER_ONLY = 2; + const uint8 AUTONOMOUS_VELOCITY_ONLY = 3; + const uint8 MANUAL = 4; + const uint8 DISENGAGED = 5; + const uint8 NOT_READY = 6; + }; + @verbatim (language="comment", text= + " ControlModeReport.msg") + + struct ControlModeReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 mode; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl new file mode 100644 index 00000000..316883ec --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl @@ -0,0 +1,18 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + + @verbatim (language="comment", text= + " Command for controlling the engagement state of the vehicle.") + struct Engage { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Determines if a vehicle should be engaged.") + @default (value=FALSE) + boolean engage; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl new file mode 100644 index 00000000..a0f606a4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl @@ -0,0 +1,40 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module GearCommand_Constants { + const uint8 NONE = 0; + const uint8 NEUTRAL = 1; + const uint8 DRIVE = 2; + const uint8 DRIVE_2 = 3; + const uint8 DRIVE_3 = 4; + const uint8 DRIVE_4 = 5; + const uint8 DRIVE_5 = 6; + const uint8 DRIVE_6 = 7; + const uint8 DRIVE_7 = 8; + const uint8 DRIVE_8 = 9; + const uint8 DRIVE_9 = 10; + const uint8 DRIVE_10 = 11; + const uint8 DRIVE_11 = 12; + const uint8 DRIVE_12 = 13; + const uint8 DRIVE_13 = 14; + const uint8 DRIVE_14 = 15; + const uint8 DRIVE_15 = 16; + const uint8 DRIVE_16 = 17; + const uint8 DRIVE_17 = 18; + const uint8 DRIVE_18 = 19; + const uint8 REVERSE = 20; + const uint8 REVERSE_2 = 21; + const uint8 PARK = 22; + const uint8 LOW = 23; + const uint8 LOW_2 = 24; + }; + + struct GearCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 command; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl new file mode 100644 index 00000000..d82c4de4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl @@ -0,0 +1,40 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module GearReport_Constants { + const uint8 NONE = 0; + const uint8 NEUTRAL = 1; + const uint8 DRIVE = 2; + const uint8 DRIVE_2 = 3; + const uint8 DRIVE_3 = 4; + const uint8 DRIVE_4 = 5; + const uint8 DRIVE_5 = 6; + const uint8 DRIVE_6 = 7; + const uint8 DRIVE_7 = 8; + const uint8 DRIVE_8 = 9; + const uint8 DRIVE_9 = 10; + const uint8 DRIVE_10 = 11; + const uint8 DRIVE_11 = 12; + const uint8 DRIVE_12 = 13; + const uint8 DRIVE_13 = 14; + const uint8 DRIVE_14 = 15; + const uint8 DRIVE_15 = 16; + const uint8 DRIVE_16 = 17; + const uint8 DRIVE_17 = 18; + const uint8 DRIVE_18 = 19; + const uint8 REVERSE = 20; + const uint8 REVERSE_2 = 21; + const uint8 PARK = 22; + const uint8 LOW = 23; + const uint8 LOW_2 = 24; + }; + + struct GearReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 report; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl new file mode 100644 index 00000000..55520826 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl @@ -0,0 +1,16 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " Command for controlling an electronic hand brake.") + + struct HandBrakeCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=FALSE) + boolean active; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl new file mode 100644 index 00000000..6360f82a --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl @@ -0,0 +1,13 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + struct HandBrakeReport { + builtin_interfaces::msg::Time stamp; + + @default (value=FALSE) + boolean report; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl new file mode 100644 index 00000000..dc094113 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl @@ -0,0 +1,22 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module HazardLightsCommand_Constants { + const uint8 NO_COMMAND = 0; + const uint8 DISABLE = 1; + const uint8 ENABLE = 2; + }; + + @verbatim (language="comment", text= + " Command for controlling a vehicle's hazard lights.") + + struct HazardLightsCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 command; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl new file mode 100644 index 00000000..c85d0ea6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl @@ -0,0 +1,18 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module HazardLightsReport_Constants { + const uint8 DISABLE = 1; + const uint8 ENABLE = 2; + }; + + struct HazardLightsReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 report; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl new file mode 100644 index 00000000..6cf67ffb --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl @@ -0,0 +1,23 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module HeadlightsCommand_Constants { + const uint8 NO_COMMAND = 0; + const uint8 DISABLE = 1; + const uint8 ENABLE_LOW = 2; + const uint8 ENABLE_HIGH = 3; + }; + + @verbatim (language="comment", text= + " Command for controlling headlights.") + + struct HeadlightsCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 command; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl new file mode 100644 index 00000000..498e58c7 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl @@ -0,0 +1,18 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module HeadlightsReport_Constants { + const uint8 DISABLE = 1; + const uint8 ENABLE_LOW = 2; + const uint8 ENABLE_HIGH = 3; + }; + + struct HeadlightsReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 report; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl new file mode 100644 index 00000000..c27d6ed3 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl @@ -0,0 +1,16 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " Command for controlling a horn.") + + struct HornCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=FALSE) + boolean active; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl new file mode 100644 index 00000000..b4c3fefd --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl @@ -0,0 +1,13 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + struct HornReport { + builtin_interfaces::msg::Time stamp; + + @default (value=FALSE) + boolean report; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl new file mode 100644 index 00000000..1bebaeba --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl @@ -0,0 +1,21 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + struct RawControlCommand { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Units for all below fields are not defined. The exact semantics for each field is defined by each" "\n" + " vehicle interface implementation. It is the system integrator's responsibility to ensure these are" "\n" + " consistent when using this interface") + uint32 throttle; + + uint32 brake; + + int32 front_steer; + + int32 rear_steer; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl new file mode 100644 index 00000000..6ff96a7e --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl @@ -0,0 +1,20 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " Command to avoid stationary locking \(Park gear, Parking Brake, etc.\)." + " This command assumes that the VehicleInterface or lower levels are controlling" + " the mechanism by which a vehicle is prevented from moving \(\"stationary locking\"\)" + " and will automatically attempt to apply stationary locking if a 0-velocity command" + " is applied for an arbitrary period of time. Setting the avoid_stationary_locking" + " field to TRUE tells the VehicleInterface or lower levels to avoid making this transition.") + + struct StationaryLockingCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=FALSE) + boolean avoid_stationary_locking; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl new file mode 100644 index 00000000..ed6a0545 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl @@ -0,0 +1,17 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " SteeringReport.msg") + struct SteeringReport { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Desired angle of the steering tire in radians left (positive)" + " or right (negative) of center (0.0)") + @default (value=0.0) + float steering_tire_angle; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl new file mode 100644 index 00000000..1e1059f6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl @@ -0,0 +1,23 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module TurnIndicatorsCommand_Constants { + const uint8 NO_COMMAND = 0; + const uint8 DISABLE = 1; + const uint8 ENABLE_LEFT = 2; + const uint8 ENABLE_RIGHT = 3; + }; + + @verbatim (language="comment", text= + " Command for controlling turn indicators.") + + struct TurnIndicatorsCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 command; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl new file mode 100644 index 00000000..3b1cf6e8 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl @@ -0,0 +1,19 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module TurnIndicatorsReport_Constants { + const uint8 DISABLE = 1; + const uint8 ENABLE_LEFT = 2; + const uint8 ENABLE_RIGHT = 3; + }; + + struct TurnIndicatorsReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 report; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl new file mode 100644 index 00000000..ebcc6a02 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl @@ -0,0 +1,27 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " Information that is sent to Vehicle interface") + struct VehicleControlCommand { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " should be negative when reversed") + @default (value=0.0) + float long_accel_mps2; + + @verbatim (language="comment", text= + " should be negative when reversed") + @default (value=0.0) + float velocity_mps; + + @default (value=0.0) + float front_wheel_angle_rad; + + @default (value=0.0) + float rear_wheel_angle_rad; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl new file mode 100644 index 00000000..f1589281 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl @@ -0,0 +1,18 @@ +#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" +#include "geometry_msgs/msg/Transform.idl" +#include "std_msgs/msg/Header.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " VehicleKinematicState.msg" "\n" + " Representation of a trajectory point with timestamp for the controller") + struct VehicleKinematicState { + std_msgs::msg::Header header; + + autoware_auto_planning_msgs::msg::TrajectoryPoint state; + + geometry_msgs::msg::Transform delta; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl new file mode 100644 index 00000000..50c699d2 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl @@ -0,0 +1,20 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " VehicleOdometry.msg") + struct VehicleOdometry { + builtin_interfaces::msg::Time stamp; + + @default (value=0.0) + float velocity_mps; + + @default (value=0.0) + float front_wheel_angle_rad; + + @default (value=0.0) + float rear_wheel_angle_rad; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl new file mode 100644 index 00000000..3cba7ca4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl @@ -0,0 +1,57 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module VehicleStateCommand_Constants { + const uint8 BLINKER_NO_COMMAND = 0; + const uint8 BLINKER_OFF = 1; + const uint8 BLINKER_LEFT = 2; + const uint8 BLINKER_RIGHT = 3; + const uint8 BLINKER_HAZARD = 4; + const uint8 HEADLIGHT_NO_COMMAND = 0; + const uint8 HEADLIGHT_OFF = 1; + const uint8 HEADLIGHT_ON = 2; + const uint8 HEADLIGHT_HIGH = 3; + const uint8 WIPER_NO_COMMAND = 0; + const uint8 WIPER_OFF = 1; + const uint8 WIPER_LOW = 2; + const uint8 WIPER_HIGH = 3; + const uint8 WIPER_CLEAN = 14; // Match WipersCommand::ENABLE_CLEAN + const uint8 GEAR_NO_COMMAND = 0; + const uint8 GEAR_DRIVE = 1; + const uint8 GEAR_REVERSE = 2; + const uint8 GEAR_PARK = 3; + const uint8 GEAR_LOW = 4; + const uint8 GEAR_NEUTRAL = 5; + const uint8 MODE_NO_COMMAND = 0; + const uint8 MODE_AUTONOMOUS = 1; + const uint8 MODE_MANUAL = 2; + }; + @verbatim (language="comment", text= + " VehicleStateCommand.msg") + struct VehicleStateCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 blinker; + + @default (value=0) + uint8 headlight; + + @default (value=0) + uint8 wiper; + + @default (value=0) + uint8 gear; + + @default (value=0) + uint8 mode; + + @default (value=FALSE) + boolean hand_brake; + + @default (value=FALSE) + boolean horn; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl new file mode 100644 index 00000000..1a85fa48 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl @@ -0,0 +1,50 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module VehicleStateReport_Constants { + const uint8 BLINKER_OFF = 1; + const uint8 BLINKER_LEFT = 2; + const uint8 BLINKER_RIGHT = 3; + const uint8 BLINKER_HAZARD = 4; + const uint8 HEADLIGHT_OFF = 1; + const uint8 HEADLIGHT_ON = 2; + const uint8 HEADLIGHT_HIGH = 3; + const uint8 WIPER_OFF = 1; + const uint8 WIPER_LOW = 2; + const uint8 WIPER_HIGH = 3; + const uint8 WIPER_CLEAN = 14; // Match WipersCommand::ENABLE_CLEAN + const uint8 GEAR_DRIVE = 1; + const uint8 GEAR_REVERSE = 2; + const uint8 GEAR_PARK = 3; + const uint8 GEAR_LOW = 4; + const uint8 GEAR_NEUTRAL = 5; + const uint8 MODE_AUTONOMOUS = 1; + const uint8 MODE_MANUAL = 2; + const uint8 MODE_DISENGAGED = 3; + const uint8 MODE_NOT_READY = 4; + }; + + struct VehicleStateReport { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " 0 to 100") + uint8 fuel; + + uint8 blinker; + + uint8 headlight; + + uint8 wiper; + + uint8 gear; + + uint8 mode; + + boolean hand_brake; + + boolean horn; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl new file mode 100644 index 00000000..04f32028 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl @@ -0,0 +1,20 @@ +#include "std_msgs/msg/Header.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + " VelocityReport.msg") + struct VelocityReport { + std_msgs::msg::Header header; + + @default (value=0.0) + float longitudinal_velocity; + + @default (value=0.0) + float lateral_velocity; + + @default (value=0.0) + float heading_rate; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl new file mode 100644 index 00000000..17f00f1f --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl @@ -0,0 +1,17 @@ +#include "std_msgs/msg/Header.idl" + +module autoware_auto_vehicle_msgs { + module msg { + @verbatim (language="comment", text= + "Representation of a wheel-encoder measurement") + struct WheelEncoder { + std_msgs::msg::Header header; + + @verbatim (language="comment", text= + " Negative speed values indicate rotation in the opposite " "\n" + " direction of the normal direction of travel of the vehicle.") + @default (value=0.0) + float speed_mps; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl new file mode 100644 index 00000000..96322998 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl @@ -0,0 +1,38 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module WipersCommand_Constants { + const uint8 NO_COMMAND = 0; + const uint8 DISABLE = 1; + const uint8 ENABLE_LOW = 2; + const uint8 ENABLE_HIGH = 3; + const uint8 ENABLE_INT_1 = 4; + const uint8 ENABLE_INT_2 = 5; + const uint8 ENABLE_INT_3 = 6; + const uint8 ENABLE_INT_4 = 7; + const uint8 ENABLE_INT_5 = 8; + const uint8 ENABLE_INT_6 = 9; + const uint8 ENABLE_INT_7 = 10; + const uint8 ENABLE_INT_8 = 11; + const uint8 ENABLE_INT_9 = 12; + const uint8 ENABLE_INT_10 = 13; + const uint8 ENABLE_CLEAN = 14; + }; + + @verbatim (language="comment", text= + " Command for controlling a wiper or group of wipers.") + + @verbatim (language="comment", text= + " Each wiper or group of simultaneously-controlled wipers" + " should have their own topic which receives this message.") + + struct WipersCommand { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 command; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl new file mode 100644 index 00000000..f5ceb7d4 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl @@ -0,0 +1,34 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_vehicle_msgs { + module msg { + module WipersReport_Constants { + const uint8 DISABLE = 1; + const uint8 ENABLE_LOW = 2; + const uint8 ENABLE_HIGH = 3; + const uint8 ENABLE_INT_1 = 4; + const uint8 ENABLE_INT_2 = 5; + const uint8 ENABLE_INT_3 = 6; + const uint8 ENABLE_INT_4 = 7; + const uint8 ENABLE_INT_5 = 8; + const uint8 ENABLE_INT_6 = 9; + const uint8 ENABLE_INT_7 = 10; + const uint8 ENABLE_INT_8 = 11; + const uint8 ENABLE_INT_9 = 12; + const uint8 ENABLE_INT_10 = 13; + const uint8 ENABLE_CLEAN = 14; + }; + + @verbatim (language="comment", text= + " Each wiper or group of simultaneously-controlled wipers" + " should have their own topic which receives this message.") + + struct WipersReport { + builtin_interfaces::msg::Time stamp; + + @default (value=0) + uint8 report; + }; + }; +}; + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml new file mode 100644 index 00000000..ff6db6dc --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml @@ -0,0 +1,29 @@ + + + + autoware_auto_vehicle_msgs + 1.0.0 + Interfaces between core Autoware.Auto vehicle components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + autoware_auto_planning_msgs + builtin_interfaces + std_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + + diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl new file mode 100644 index 00000000..7be179d8 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl @@ -0,0 +1,22 @@ +#include "std_msgs/msg/Empty.idl" + +module autoware_auto_vehicle_msgs { + module srv { + module AutonomyModeChange_Request_Constants { + const uint8 MODE_MANUAL = 0; + const uint8 MODE_AUTONOMOUS = 1; + }; + struct AutonomyModeChange_Request + { + @verbatim(language = "comment", text = + "The desired autonomy mode") + uint8 mode; + }; + struct AutonomyModeChange_Response + { + @verbatim(language = "comment", text = + "No response is used because changing the autonomy mode requires non-trivial time") + std_msgs::msg::Empty empty; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv new file mode 100644 index 00000000..988a7a66 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv @@ -0,0 +1,12 @@ +uint8 NO_COMMAND = 0 +uint8 AUTONOMOUS = 1 +uint8 AUTONOMOUS_STEER_ONLY = 2 +uint8 AUTONOMOUS_VELOCITY_ONLY = 3 +uint8 MANUAL = 4 + +builtin_interfaces/Time stamp +uint8 mode + +--- + +bool success From 5bf9838f03637572a03604d0fc12fecaf25a4442 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 17:33:04 +0900 Subject: [PATCH 06/16] Add files via upload --- .../autoware_auto_control_msgs/CMakeLists.txt | 26 ++++++++++++++++ .../msg/AckermannControlCommand.idl | 16 ++++++++++ .../msg/AckermannLateralCommand.idl | 25 ++++++++++++++++ .../msg/HighLevelControlCommand.idl | 18 +++++++++++ .../msg/LongitudinalCommand.idl | 30 +++++++++++++++++++ .../autoware_auto_control_msgs/package.xml | 27 +++++++++++++++++ 6 files changed, 142 insertions(+) create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl create mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt new file mode 100644 index 00000000..6cc556a6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt @@ -0,0 +1,26 @@ +# All rights reserved. +cmake_minimum_required(VERSION 3.5) + +### Export headers +project(autoware_auto_control_msgs) + +# Generate messages +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +rosidl_generate_interfaces(${PROJECT_NAME} + "msg/AckermannControlCommand.idl" + "msg/AckermannLateralCommand.idl" + "msg/HighLevelControlCommand.idl" + "msg/LongitudinalCommand.idl" + DEPENDENCIES + "builtin_interfaces" + ADD_LINTER_TESTS +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl new file mode 100644 index 00000000..916b1305 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl @@ -0,0 +1,16 @@ +#include "autoware_auto_control_msgs/msg/AckermannLateralCommand.idl" +#include "autoware_auto_control_msgs/msg/LongitudinalCommand.idl" +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_control_msgs { + module msg { + @verbatim (language="comment", text= + " Lateral and longitudinal control message for Ackermann-style platforms") + struct AckermannControlCommand { + builtin_interfaces::msg::Time stamp; + + autoware_auto_control_msgs::msg::AckermannLateralCommand lateral; + autoware_auto_control_msgs::msg::LongitudinalCommand longitudinal; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl new file mode 100644 index 00000000..e1df7fb0 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl @@ -0,0 +1,25 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_control_msgs { + module msg { + @verbatim (language="comment", text= + " Lateral control message for Ackermann-style platforms" "\n" + " Note regarding tires: If the platform has multiple steering tires, the commands" + " given here are for a virtual tire at the average lateral position of the steering tires.") + + struct AckermannLateralCommand { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Desired angle of the steering tire in radians left (positive)" + " or right (negative) of center (0.0)") + @default (value=0.0) + float steering_tire_angle; + + @verbatim (language="comment", text= + " Desired rate of change of the steering tire angle in radians per second") + @default (value=0.0) + float steering_tire_rotation_rate; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl new file mode 100644 index 00000000..4fb6fbe6 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl @@ -0,0 +1,18 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_control_msgs { + module msg { + struct HighLevelControlCommand { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " should be negative when reversed") + @default (value=0.0) + float velocity_mps; + + @verbatim (language="comment", text= + " units of inverse meters") + float curvature; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl new file mode 100644 index 00000000..4549bf0b --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl @@ -0,0 +1,30 @@ +#include "builtin_interfaces/msg/Time.idl" + +module autoware_auto_control_msgs { + module msg { + @verbatim (language="comment", text= + " Longitudinal control message for all vehicle types") + + struct LongitudinalCommand { + builtin_interfaces::msg::Time stamp; + + @verbatim (language="comment", text= + " Desired platform speed in meters per second." + " A positive value indicates movement in the positive X direction of the vehicle " + " while a negative value indicates movement in the negative X direction of the vehicle.") + @default (value=0.0) + float speed; + + @verbatim (language="comment", text= + " Desired platform acceleration in meters per second squared." + " A positive value indicates acceleration while a negative value indicates deceleration.") + @default (value=0.0) + float acceleration; + + @verbatim (language="comment", text= + " Desired platform jerk in meters per second cubed") + @default (value=0.0) + float jerk; + }; + }; +}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml new file mode 100644 index 00000000..cfddca34 --- /dev/null +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml @@ -0,0 +1,27 @@ + + + + autoware_auto_control_msgs + 1.0.0 + Interfaces between core Autoware.Auto control components + Apex.AI, Inc. + Apache 2 + + ament_cmake_auto + + rosidl_default_generators + + builtin_interfaces + std_msgs + + rosidl_default_runtime + + ament_lint_auto + ament_lint_common + + rosidl_interface_packages + + + ament_cmake + + From 855180b4acab0ac925f46e5989049d0c4adfdee8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 23 Mar 2023 11:32:31 +0000 Subject: [PATCH 07/16] ci(pre-commit): autofix --- .../autoware_auto_msgs/DISCLAIMER.md | 50 ++++--- src/autoware_msg/autoware_auto_msgs/README.md | 2 +- .../autoware_auto_mapping_msgs/package.xml | 1 - .../autoware_auto_internal_msgs-design.md | 4 +- .../design/autoware_auto_msgs-design.md | 13 +- .../design/bounding-box-design.md | 21 +-- .../autoware_auto_msgs/package.xml | 1 - .../autoware_auto_perception_msgs/package.xml | 1 - .../autoware_auto_planning_msgs/package.xml | 1 - .../msg/AutowareState.idl | 1 - .../msg/DrivingCapability.idl | 1 - .../msg/EmergencyState.idl | 1 - .../msg/HazardStatus.idl | 1 - .../msg/HazardStatusStamped.idl | 1 - .../autoware_auto_system_msgs/package.xml | 1 - .../autoware_auto_vehicle_msgs/msg/Engage.idl | 1 - .../msg/HandBrakeCommand.idl | 1 - .../msg/HandBrakeReport.idl | 1 - .../msg/HazardLightsCommand.idl | 1 - .../msg/HazardLightsReport.idl | 1 - .../msg/HeadlightsCommand.idl | 1 - .../msg/HornCommand.idl | 1 - .../msg/HornReport.idl | 1 - .../msg/TurnIndicatorsCommand.idl | 1 - .../msg/TurnIndicatorsReport.idl | 1 - .../msg/WipersCommand.idl | 1 - .../msg/WipersReport.idl | 1 - .../autoware_auto_vehicle_msgs/package.xml | 1 - src/tilde/src/tilde_node.cpp | 4 +- src/tilde_early_deadline_detector/README.md | 2 +- .../autoware_sensors.yaml | 6 +- .../forward_estimator.hpp | 2 +- .../tilde_early_deadline_detector_node.hpp | 13 +- src/tilde_early_deadline_detector/package.xml | 2 +- .../src/forward_estimator.cpp | 2 +- .../tilde_early_deadline_detector_node.cpp | 132 ++++++++++-------- 36 files changed, 124 insertions(+), 152 deletions(-) diff --git a/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md b/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md index d0d6f97d..1a13881f 100644 --- a/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md +++ b/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md @@ -8,34 +8,32 @@ License and Users shall hereby approve and acknowledge all the contents specified in this disclaimer below and will be deemed to consent to this disclaimer without any objection upon utilizing or downloading Autoware. - Disclaimer and Waiver of Warranties -1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, -EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) -including but not limited to any representation or warranty (i) of fitness or -suitability for a particular purpose contemplated by the Users, (ii) of the -expected functions, commercial value, accuracy, or usefulness of the Service, -(iii) that the use by the Users of the Service complies with the laws and -regulations applicable to the Users or any internal rules established by -industrial organizations, (iv) that the Service will be free of interruption or -defects, (v) of the non-infringement of any third party's right and (vi) the -accuracy of the content of the Services and the software itself. - -2. The Autoware Foundation shall not be liable for any damage incurred by the -User that are attributable to the Autoware Foundation for any reasons -whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR -INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. - -3. A User shall be entirely responsible for the content posted by the User and -its use of any content of the Service or the Website. If the User is held -responsible in a civil action such as a claim for damages or even in a criminal -case, the Autoware Foundation and member companies, governments and academic & -non-profit organizations and their directors, officers, employees and agents -(collectively, the “Indemnified Parties”) shall be completely discharged from -any rights or assertions the User may have against the Indemnified Parties, or -from any legal action, litigation or similar procedures. - +1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, + EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) + including but not limited to any representation or warranty (i) of fitness or + suitability for a particular purpose contemplated by the Users, (ii) of the + expected functions, commercial value, accuracy, or usefulness of the Service, + (iii) that the use by the Users of the Service complies with the laws and + regulations applicable to the Users or any internal rules established by + industrial organizations, (iv) that the Service will be free of interruption or + defects, (v) of the non-infringement of any third party's right and (vi) the + accuracy of the content of the Services and the software itself. + +2. The Autoware Foundation shall not be liable for any damage incurred by the + User that are attributable to the Autoware Foundation for any reasons + whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR + INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. + +3. A User shall be entirely responsible for the content posted by the User and + its use of any content of the Service or the Website. If the User is held + responsible in a civil action such as a claim for damages or even in a criminal + case, the Autoware Foundation and member companies, governments and academic & + non-profit organizations and their directors, officers, employees and agents + (collectively, the “Indemnified Parties”) shall be completely discharged from + any rights or assertions the User may have against the Indemnified Parties, or + from any legal action, litigation or similar procedures. Indemnity diff --git a/src/autoware_msg/autoware_auto_msgs/README.md b/src/autoware_msg/autoware_auto_msgs/README.md index c1dbcc16..35249982 100644 --- a/src/autoware_msg/autoware_auto_msgs/README.md +++ b/src/autoware_msg/autoware_auto_msgs/README.md @@ -31,7 +31,7 @@ If a quantity described by a field has an associated unit of measurement, the fo Only deviate from the SI units when absolutely necessary and with justification. | Quantity | Unit | Suffix | Notes | -|-----------------|-----------------------------|---------|---------------------------------------------------------| +| --------------- | --------------------------- | ------- | ------------------------------------------------------- | | distance | meters | None | | | | micrometers | `_um` | | | | millimeters | `_mm` | | diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml index a57f0ca4..7e763458 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml @@ -24,4 +24,3 @@ ament_cmake - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md index af820341..a6558634 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md @@ -1,5 +1,4 @@ -autoware_auto_msgs internal message design -========================= +# autoware_auto_msgs internal message design [TOC] @@ -31,7 +30,6 @@ This message represents a set of point clusters as a result of object detection `PointCloud2` was used as a cluster can be conceived as a subset of a point cloud, and `PointCloud2` is the standard representation for point clouds. - ## Tracking This section tracks messages internal to specific implementation of tracking stacks. diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md index d34ac6a5..78f89f93 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md @@ -1,5 +1,4 @@ -autoware_auto_msgs -================== +# autoware_auto_msgs [TOC] @@ -27,6 +26,7 @@ geometry_msgs/Point32[4] corners float32 value uint32 label ``` + See the [design document](bounding-box-design.md) for further details. ## BoundingBoxArray @@ -37,6 +37,7 @@ BoundingBox[256] boxes uint32 size uint32 CAPACITY=256 ``` + ## DiagnosticHeader @@ -211,8 +212,8 @@ follow a geometric reference path. For more details see [vehicle interface design](). - ## Vehicle Control Command + ``` builtin_interfaces/Time stamp float32 long_accel_mps2 @@ -255,9 +256,11 @@ such as forklifts. For more details, see [Controller design](@ref controller-design) ## Vehicle Kinematic State + **Source**: Vehicle interface **Recipient(s)**: Behavior Planner + ``` std_msgs/Header header TrajectoryPoint state @@ -406,7 +409,6 @@ can satisfy. **Rationale**: Since commands here represent a state transition, a command of NONE denotion "no state transition" is also valid - ## Vehicle State Report ``` @@ -493,6 +495,7 @@ introduced. The constant 0 is reserved for NO_COMMAND in VehicleStateCommand. ### Apollo Controller/Interface Messages + - [VehicleSignal](https://github.com/ApolloAuto/apollo/blob/fb12723bd6dcba88ecccb6123ea850da1e050171/modules/common/proto/vehicle_signal.proto) - [Lexus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/canbus/proto/lexus.proto) - [DriveState](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/common/proto/drive_state.proto) @@ -500,12 +503,14 @@ Controller/Interface Messages - [ControlCmd](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/control_cmd.proto) Configuration Messages + - [LonControllerConf](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/lon_controller_conf.proto) - [LatControllerConf](https://github.com/ApolloAuto/apollo/blob/e9156ced04a8f0e372c781d803fd43428ab6c497/modules/control/proto/lat_controller_conf.proto) - [ControlConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/control_conf.proto) - [MPCControllerConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/mpc_controller_conf.proto) Algorithm Status Messages + - [PlanningStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_status.proto) - [LocalizationStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/localization/proto/localization_status.proto) - [PlanningStats](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_stats.proto) diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md index 696fa1df..d109ae67 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md @@ -1,5 +1,4 @@ -BoundingBox message design {#bounding-box-design} -========================== +# BoundingBox message design {#bounding-box-design} # Motivation @@ -14,7 +13,6 @@ between object detection and tracking is required, specialized combined stacks c In order to facilitate the isolation of object detectors as a module, common messages types are defined as an interface or output of the object detectors to other components. - # Use cases The instantaneous detection of objects primarily has three use cases: @@ -26,7 +24,6 @@ The instantaneous detection of objects primarily has three use cases: In addition, there is the implied use case that this message representation is used on a safety- critical system. - # Requirements ## Safety-critical systems @@ -39,19 +36,17 @@ the maximum size is no more than `64kB`, which is also the maximum size of a sin requirement ensures that latency is kept to a minimum if interprocess communication is necessary at the interface of the object detection algorithms. - ## Collision detection In order to detect collisions, the representation must have: 1. A position at least in 2-space (with the assumption that the object is on the same plane as the -ego) + ego) 2. A bounding volume representation which fully contains the detected object 3. A way to ensure the object representation is in a consistent coordinate frame with respect to the -ego + ego 4. Optionally uncertainty representation of the above parameters - ## Tracking The purpose of tracking is to maintain a consistent identity of a detected object over time. @@ -88,7 +83,6 @@ As such, the features needed to satisfy this use case are similar to that of the 2. Shape/object information 3. Optionally uncertainty information - # Message definition Based on the requirements laid out, some assumptions and simplifications can be made, and a message @@ -101,7 +95,6 @@ encapsulates multiple software components should be made. In the case when a stack cannot populate certain fields of the interface, the field should be in an identifiably non-normal state, such as zero or NaN. - ## Safety critical requirements To satisfy the minimal latency requirement, the representation must be bounded in size to `64kB`. @@ -116,7 +109,6 @@ scene. Applying a healthy safety factor gives a maximum object bound of 256 obje These two assumptions combined lead us to have a maximum representation size of `250B`. - ## Position requirements Minimally, a 3D position is used to satisfy the position requirement inherent in all use cases. A 3D @@ -127,7 +119,6 @@ in a common frame. This frame can then be represented by a string in the aggrega It is assumed that it is the responsibility of the consuming algorithm to have and transform the position space into the correct coordinate frame given this information. - ## Shape requirements Next, the shape of the object is represented as a bounding box in 3 interdependent ways: @@ -156,7 +147,6 @@ for fine-grained collision detection. Similarly, the size of the bounding may be extent observations in the tracking case, whereas the corners may be treated as proxy observations of the centroid. - ## Object/Other requirements Of all the object features proposed, only a classification label is bounded in representation size @@ -177,7 +167,7 @@ MOTORCYCLE=4 ``` The `signal_label` field contains the back signal state of the car and can take any of the following - default values: +default values: ``` NO_SIGNAL=0 @@ -191,7 +181,6 @@ velocity, heading, and heading rate, assuming a FMCW/doppler-effect sensor is av inclusion of such fields can greatly improve the performance of tracking, or in some cases remove the necessity of tracking (i.e. predictive collision detection). - ## Uncertainty requirements While uncertainty representation is not a strict requirement, the usage of uncertainty can greatly @@ -209,7 +198,6 @@ off-diagonal elements of the full covariance matrix are zero, and thus need not Second, the coarse modeling assumption of a uniformly distributed class assignment residual can be used. As a result, class uncertainty can be represented with a single value. - # Future extensions The currently proposed representation for bounding boxes takes up approximately `150B` of the total @@ -217,7 +205,6 @@ size budget of `230B`. This leaves room for up to 18 floating point values. Thes cover the other optional requirements as a vector of floating points, with an optional ID denoting the intended interpretation. - # Related issues - #3540: Update bounding box, add use cases diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml index a7dc9c31..4a80c67c 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml @@ -24,4 +24,3 @@ ament_cmake - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml index 6161f058..16460341 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml @@ -28,4 +28,3 @@ ament_cmake - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml index ee143ba9..3ba50da3 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml @@ -29,4 +29,3 @@ ament_cmake - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl index bbe0c3da..7e173dd5 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl @@ -23,4 +23,3 @@ module autoware_auto_system_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl index 0066cf51..4210cc54 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl @@ -19,4 +19,3 @@ module autoware_auto_system_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl index 709ff59d..33ab1432 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl @@ -27,4 +27,3 @@ module autoware_auto_system_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl index 057ada72..5a397979 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl @@ -46,4 +46,3 @@ module autoware_auto_system_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl index 91a42b67..ce09cb4d 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl @@ -15,4 +15,3 @@ module autoware_auto_system_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml index a017f628..4759d007 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml @@ -26,4 +26,3 @@ ament_cmake - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl index 316883ec..57ee2109 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl @@ -15,4 +15,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl index 55520826..42323cde 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl @@ -13,4 +13,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl index 6360f82a..da1bf9e6 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl @@ -10,4 +10,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl index dc094113..d5a4405c 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl @@ -19,4 +19,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl index c85d0ea6..264bcded 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl @@ -15,4 +15,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl index 6cf67ffb..9512bb41 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl @@ -20,4 +20,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl index c27d6ed3..234ba028 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl @@ -13,4 +13,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl index b4c3fefd..89b2c9ef 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl @@ -10,4 +10,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl index 1e1059f6..342c22b7 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl @@ -20,4 +20,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl index 3b1cf6e8..2a7f975a 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl @@ -16,4 +16,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl index 96322998..1747f2d6 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl @@ -35,4 +35,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl index f5ceb7d4..fe422583 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl @@ -31,4 +31,3 @@ module autoware_auto_vehicle_msgs { }; }; }; - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml index ff6db6dc..19f65639 100644 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml +++ b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml @@ -26,4 +26,3 @@ ament_cmake - diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp index a14de72d..d63f19a6 100644 --- a/src/tilde/src/tilde_node.cpp +++ b/src/tilde/src/tilde_node.cpp @@ -14,8 +14,8 @@ #include "tilde/tilde_node.hpp" -#include #include +#include using tilde::TildeNode; @@ -39,7 +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; + 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/README.md b/src/tilde_early_deadline_detector/README.md index cef7ed70..844b45de 100644 --- a/src/tilde_early_deadline_detector/README.md +++ b/src/tilde_early_deadline_detector/README.md @@ -23,7 +23,7 @@ The default path is shown below. 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/TILDE/tree/master/doc](https://github.com/tier4/caret)). +- 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/TILDE/tree/master/doc](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. diff --git a/src/tilde_early_deadline_detector/autoware_sensors.yaml b/src/tilde_early_deadline_detector/autoware_sensors.yaml index 1086bc92..7d8af5c9 100644 --- a/src/tilde_early_deadline_detector/autoware_sensors.yaml +++ b/src/tilde_early_deadline_detector/autoware_sensors.yaml @@ -1,9 +1,7 @@ tilde_early_deadline_detector_node: ros__parameters: # input of e2e - sensor_topics: [ - "/sensing/lidar/top/self_cropped/pointcloud_ex" - ] + 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", @@ -23,7 +21,7 @@ tilde_early_deadline_detector_node: # 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] + deadline_ms: [553, 553, 553, 553, 553, 553, 553, 553, 553] # parameters of debug messages print_report: true show_performance: true 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 index e9ef26a5..e61f884e 100644 --- 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 @@ -135,6 +135,6 @@ class ForwardEstimator void update_pending(std::shared_ptr message_tracking_tag); }; -} // namespace tilde_deadline_detector +} // 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 index fb0f85e9..16e4ca1f 100644 --- 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 @@ -31,7 +31,6 @@ // map header #include - namespace tilde_early_deadline_detector { struct PerformanceCounter @@ -87,21 +86,21 @@ class TildeEarlyDeadlineDetectorNode : public rclcpp::Node rclcpp::Publisher::SharedPtr notification_pub_; - PerformanceCounter message_tracking_tag_callback_counter_; - PerformanceCounter timer_callback_counter_; + 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; +// using MessageTrackingTagSubscription = +// rclcpp::Subscription; // public: // // constructors @@ -152,6 +151,6 @@ class TildeEarlyDeadlineDetectorNode : public rclcpp::Node // void message_tracking_tag_callback(tilde_msg::msg::MessageTrackingTag::UniquePtr msg); // }; -} // namespace tilde_deadline_detector +} // namespace tilde_early_deadline_detector -#endif // TILDE_EARLY_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ +#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 index f2c1e58a..aa9b70f7 100644 --- a/src/tilde_early_deadline_detector/package.xml +++ b/src/tilde_early_deadline_detector/package.xml @@ -16,8 +16,8 @@ rclcpp_components sensor_msgs tilde - tilde_msg 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 index 651211ed..71948c6a 100644 --- a/src/tilde_early_deadline_detector/src/forward_estimator.cpp +++ b/src/tilde_early_deadline_detector/src/forward_estimator.cpp @@ -293,4 +293,4 @@ std::map ForwardEstimator::get_pending_mess return ret; } -} // namespace tilde_deadline_detector +} // 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 index 2074c5f7..c71ae5b6 100644 --- 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 @@ -13,6 +13,7 @@ // 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" @@ -34,8 +35,8 @@ #include // container -#include -#include +#include +#include using std::chrono::milliseconds; using tilde_msg::msg::MessageTrackingTag; @@ -44,24 +45,26 @@ using tilde_msg::msg::MessageTrackingTag; // 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; +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 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{ +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){ +double estimate_latency(std::string topic_name) +{ // 99percentile std::map map{ // {"/sensing/lidar/top/self_cropped/pointcloud_ex", 552.49}, @@ -76,8 +79,7 @@ double estimate_latency(std::string topic_name){ {"/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} - }; + {"/control/trajectory_follower/control_cmd", 0.8}}; // // 95percentile // std::map map{ @@ -191,7 +193,7 @@ void TildeEarlyDeadlineDetectorNode::init() sensor_topics_.insert(tmp_sensor_topics.begin(), tmp_sensor_topics.end()); auto tmp_target_topics = - declare_parameter>("target_topics", std::vector{}); + 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{}); @@ -219,7 +221,8 @@ void TildeEarlyDeadlineDetectorNode::init() 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) + // 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; @@ -263,7 +266,7 @@ void TildeEarlyDeadlineDetectorNode::init() }; // print topic names subscribed by early_deadline_detector - for(auto itr = topics.begin(); itr != topics.end(); ++itr) { + for (auto itr = topics.begin(); itr != topics.end(); ++itr) { std::cout << *itr << "\n"; } @@ -287,7 +290,8 @@ void TildeEarlyDeadlineDetectorNode::init() auto sub = create_subscription( topic, qos, std::bind( - &TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback, this, std::placeholders::_1)); + &TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback, this, + std::placeholders::_1)); subs_.push_back(sub); } @@ -316,7 +320,8 @@ void TildeEarlyDeadlineDetectorNode::init() << " max: " << message_tracking_tag_callback_counter_.max << "\n" << "timer_callback: " << " avg: " << timer_callback_counter_.avg << "\n" - << " max: " << timer_callback_counter_.max << "\n" + << " max: " << timer_callback_counter_.max + << "\n" // debug // << " processed_num: " << processed_num << "\n" // << " deadline_miss_num: " << deadline_miss_num << "\n" @@ -344,17 +349,17 @@ void print_report( } 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::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; } @@ -371,7 +376,7 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // measure e2e latency builtin_interfaces::msg::Time pub_time_steady_e2e; - if (message_tracking_tag->output_info.topic_name=="/control/trajectory_follower/control_cmd"){ + 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; } @@ -381,7 +386,7 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( 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; } @@ -428,30 +433,33 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // // 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); + 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; + 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))) { + // 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 + 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 @@ -459,27 +467,27 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( 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 + 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) + if (flag_deadline_miss != 0 && flag_deadline_miss_true != 0) tp++; - else if(flag_deadline_miss==0 && flag_deadline_miss_true==0) + else if (flag_deadline_miss == 0 && flag_deadline_miss_true == 0) tn++; - else if(flag_deadline_miss!=0 && flag_deadline_miss_true==0) + else if (flag_deadline_miss != 0 && flag_deadline_miss_true == 0) fp++; - else if(flag_deadline_miss==0 && flag_deadline_miss_true!=0) + 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!=0 || fp!=0 || fn!=0){ + if (deadline_miss_true_num != 0 && (processed_num - deadline_miss_true_num) != 0) { + if (tp != 0 || fp != 0 || fn != 0) { accuracy = (tp + tn) / (tp + tn + fp + fn); precision = tp / (tp + fp); recall = tp / (tp + fn); - if(precision != 0 || recall != 0){ + if (precision != 0 || recall != 0) { f_measure = 2 * precision * recall / (precision + recall); } } @@ -490,21 +498,23 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // 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" + 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" + << " 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" + << " 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; } From 95e1f94db0c752bac421efa3f874bb36015a9b27 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 20:41:51 +0900 Subject: [PATCH 08/16] Update README.md --- src/tilde_early_deadline_detector/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tilde_early_deadline_detector/README.md b/src/tilde_early_deadline_detector/README.md index 844b45de..998b13ac 100644 --- a/src/tilde_early_deadline_detector/README.md +++ b/src/tilde_early_deadline_detector/README.md @@ -23,7 +23,7 @@ The default path is shown below. 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/TILDE/tree/master/doc](https://github.com/tier4/caret)>). +- 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. From a7047b3a0871ccda6be1bbd478e8113785e5214b Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Thu, 23 Mar 2023 22:52:39 +0900 Subject: [PATCH 09/16] Update README.md --- README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/README.md b/README.md index 307ede9d..f2bf920c 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,7 @@ # TILDE - Tilde Is Latency Data Embedding +early_deadline_detector is included in the package. + [README(ja)](./doc/README.md) ## Contents From 1fc012ce328c7ffa6d893b7262a50b8ff0a02af4 Mon Sep 17 00:00:00 2001 From: hsato-saitama Date: Tue, 28 Mar 2023 15:39:13 +0900 Subject: [PATCH 10/16] README.md --- CPPLINT.cfg | 14 - README.md | 22 - build_depends.repos | 9 - codecov.yaml | 22 - doc/README.md | 148 -- doc/build.md | 18 - doc/images/tilde_dag.svg | 761 ----------- doc/images/tilde_deadline.svg | 325 ----- doc/latency_viewer.md | 50 - doc/mechanism.md | 320 ----- doc/usecase.md | 117 -- elisp/README.md | 50 - elisp/tilde-view-mode.el | 79 -- setup.cfg | 15 - src/analysis/README.md | 103 -- src/analysis/create_bag_for_concat_filter.py | 96 -- src/analysis/graph_around.py | 89 -- src/analysis/graph_common.py | 33 - src/analysis/message_tracking_tag.py | 131 -- src/analysis/parse_message_tracking_tag.py | 84 -- src/analysis/pubinfo_traverse.py | 206 --- src/analysis/rosgraph2_impl.py | 589 -------- src/analysis/topic_times.py | 69 - src/analysis/topic_traversal.py | 100 -- src/autoware_common/CODE_OF_CONDUCT.md | 132 -- src/autoware_common/CONTRIBUTING.md | 3 - src/autoware_common/CPPLINT.cfg | 14 - src/autoware_common/DISCLAIMER.md | 46 - src/autoware_common/LICENSE | 201 --- src/autoware_common/README.md | 3 - .../caret_lint_common/CMakeLists.txt | 11 - .../caret_lint_common/README.md | 44 - .../caret_lint_common/package.xml | 25 - src/autoware_common/setup.cfg | 15 - .../autoware_auto_msgs/DISCLAIMER.md | 48 - src/autoware_msg/autoware_auto_msgs/LICENSE | 201 --- src/autoware_msg/autoware_auto_msgs/README.md | 77 -- .../autoware_auto_control_msgs/CMakeLists.txt | 26 - .../msg/AckermannControlCommand.idl | 16 - .../msg/AckermannLateralCommand.idl | 25 - .../msg/HighLevelControlCommand.idl | 18 - .../msg/LongitudinalCommand.idl | 30 - .../autoware_auto_control_msgs/package.xml | 27 - .../msg/Complex32.idl | 20 - .../msg/Quaternion32.idl | 19 - .../RelativePositionWithCovarianceStamped.idl | 16 - .../autoware_auto_mapping_msgs/CMakeLists.txt | 26 - .../msg/HADMapBin.idl | 32 - .../msg/HADMapSegment.idl | 12 - .../msg/MapPrimitive.idl | 17 - .../autoware_auto_mapping_msgs/package.xml | 26 - .../srv/HADMapService.idl | 39 - .../autoware_auto_msgs/CHANGELOG.rst | 57 - .../autoware_auto_msgs/CMakeLists.txt | 16 - .../autoware_auto_internal_msgs-design.md | 65 - .../design/autoware_auto_msgs-design.md | 516 ------- .../design/bounding-box-design.md | 211 --- .../autoware_auto_msgs/package.xml | 26 - .../CMakeLists.txt | 52 - .../msg/BoundingBox.idl | 71 - .../msg/BoundingBoxArray.idl | 17 - .../msg/ClassifiedRoi.idl | 15 - .../msg/ClassifiedRoiArray.idl | 14 - .../msg/DetectedObject.idl | 16 - .../msg/DetectedObjectKinematics.idl | 41 - .../msg/DetectedObjects.idl | 13 - .../msg/LookingTrafficSignal.idl | 14 - .../msg/ObjectClassification.idl | 24 - .../msg/PointClusters.idl | 18 - .../msg/PointXYZIF.idl | 20 - .../msg/PredictedObject.idl | 20 - .../msg/PredictedObjectKinematics.idl | 18 - .../msg/PredictedObjects.idl | 13 - .../msg/PredictedPath.idl | 15 - .../msg/Shape.idl | 27 - .../msg/TrackedObject.idl | 20 - .../msg/TrackedObjectKinematics.idl | 45 - .../msg/TrackedObjects.idl | 13 - .../msg/TrafficLight.idl | 42 - .../msg/TrafficLightRoi.idl | 10 - .../msg/TrafficLightRoiArray.idl | 11 - .../msg/TrafficSignal.idl | 19 - .../msg/TrafficSignalArray.idl | 11 - .../msg/TrafficSignalStamped.idl | 11 - .../msg/TrafficSignalWithJudge.idl | 17 - .../autoware_auto_perception_msgs/package.xml | 30 - .../CMakeLists.txt | 41 - .../action/PlanTrajectory.idl | 27 - .../action/PlannerCostmap.idl | 26 - .../action/RecordTrajectory.idl | 19 - .../action/ReplayTrajectory.idl | 19 - .../msg/HADMapRoute.idl | 27 - .../msg/OrderMovement.idl | 21 - .../autoware_auto_planning_msgs/msg/Path.idl | 15 - .../msg/PathPoint.idl | 26 - .../msg/PathPointWithLaneId.idl | 15 - .../msg/PathWithLaneId.idl | 15 - .../autoware_auto_planning_msgs/msg/Route.idl | 22 - .../msg/Trajectory.idl | 17 - .../msg/TrajectoryPoint.idl | 31 - .../autoware_auto_planning_msgs/package.xml | 31 - .../srv/ModifyTrajectory.idl | 18 - .../autoware_auto_system_msgs/CMakeLists.txt | 32 - .../msg/AutowareState.idl | 25 - .../msg/ControlDiagnostic.idl | 33 - .../msg/DiagnosticHeader.idl | 20 - .../msg/DrivingCapability.idl | 21 - .../msg/EmergencyState.idl | 29 - .../msg/Float32MultiArrayDiagnostic.idl | 16 - .../msg/HazardStatus.idl | 48 - .../msg/HazardStatusStamped.idl | 17 - .../autoware_auto_system_msgs/package.xml | 28 - .../autoware_auto_vehicle_msgs/CMakeLists.txt | 53 - .../msg/ControlModeCommand.idl | 19 - .../msg/ControlModeReport.idl | 24 - .../autoware_auto_vehicle_msgs/msg/Engage.idl | 17 - .../msg/GearCommand.idl | 40 - .../msg/GearReport.idl | 40 - .../msg/HandBrakeCommand.idl | 15 - .../msg/HandBrakeReport.idl | 12 - .../msg/HazardLightsCommand.idl | 21 - .../msg/HazardLightsReport.idl | 17 - .../msg/HeadlightsCommand.idl | 22 - .../msg/HeadlightsReport.idl | 18 - .../msg/HornCommand.idl | 15 - .../msg/HornReport.idl | 12 - .../msg/RawControlCommand.idl | 21 - .../msg/StationaryLockingCommand.idl | 20 - .../msg/SteeringReport.idl | 17 - .../msg/TurnIndicatorsCommand.idl | 22 - .../msg/TurnIndicatorsReport.idl | 18 - .../msg/VehicleControlCommand.idl | 27 - .../msg/VehicleKinematicState.idl | 18 - .../msg/VehicleOdometry.idl | 20 - .../msg/VehicleStateCommand.idl | 57 - .../msg/VehicleStateReport.idl | 50 - .../msg/VelocityReport.idl | 20 - .../msg/WheelEncoder.idl | 17 - .../msg/WipersCommand.idl | 37 - .../msg/WipersReport.idl | 33 - .../autoware_auto_vehicle_msgs/package.xml | 28 - .../srv/AutonomyModeChange.idl | 22 - .../srv/ControlModeCommand.srv | 12 - .../autoware_auto_msgs/build_depends.repos | 1 - src/tilde/CMakeLists.txt | 158 --- src/tilde/README.md | 3 - .../include/tilde/message_conversion.hpp | 106 -- .../tilde/message_conversion_detail.hpp | 50 - src/tilde/include/tilde/stee_node.hpp | 256 ---- src/tilde/include/tilde/stee_publisher.hpp | 218 --- .../include/tilde/stee_sources_table.hpp | 90 -- src/tilde/include/tilde/stee_subscription.hpp | 100 -- src/tilde/include/tilde/tilde_node.hpp | 261 ---- src/tilde/include/tilde/tilde_publisher.hpp | 418 ------ src/tilde/include/tilde/tp.h | 70 - src/tilde/package.xml | 26 - src/tilde/src/stee_node.cpp | 54 - src/tilde/src/stee_republisher_node_imu.cpp | 87 -- src/tilde/src/stee_republisher_node_map.cpp | 86 -- .../src/stee_republisher_node_pointcloud2.cpp | 88 -- src/tilde/src/stee_sources_table.cpp | 111 -- src/tilde/src/tilde_node.cpp | 61 - src/tilde/src/tilde_publisher.cpp | 179 --- src/tilde/src/tp.c | 19 - src/tilde/test/test_stamp_processor.cpp | 153 --- src/tilde/test/test_stee_node.cpp | 655 --------- src/tilde/test/test_stee_source_table.cpp | 370 ----- src/tilde/test/test_tilde_node.cpp | 476 ------- src/tilde/test/test_tilde_publisher.cpp | 369 ----- src/tilde_cmake/CMakeLists.txt | 20 - src/tilde_cmake/README.md | 18 - src/tilde_cmake/cmake/tilde_package.cmake | 25 - src/tilde_cmake/package.xml | 18 - src/tilde_cmake/tilde_cmake-extras.cmake | 15 - src/tilde_deadline_detector/CMakeLists.txt | 73 - src/tilde_deadline_detector/README.md | 59 - .../autoware_sensors.yaml | 50 - .../forward_estimator.hpp | 140 -- .../tilde_deadline_detector_node.hpp | 93 -- src/tilde_deadline_detector/package.xml | 24 - .../src/forward_estimator.cpp | 296 ---- .../src/tilde_deadline_detector_node.cpp | 290 ---- .../test/test_forward_estimator.cpp | 582 -------- .../test_tilde_deadline_detector_node.cpp | 275 ---- .../CMakeLists.txt | 45 - src/tilde_early_deadline_detector/README.md | 74 - .../autoware_sensors.yaml | 28 - .../default_path.svg | 1 - .../forward_estimator.hpp | 140 -- .../tilde_early_deadline_detector_node.hpp | 156 --- src/tilde_early_deadline_detector/package.xml | 25 - .../src/forward_estimator.cpp | 296 ---- .../tilde_early_deadline_detector_node.cpp | 531 -------- src/tilde_message_filters/CMakeLists.txt | 145 -- src/tilde_message_filters/README.md | 51 - .../tilde_subscriber.hpp | 290 ---- .../tilde_synchronizer.hpp | 1191 ----------------- src/tilde_message_filters/package.xml | 24 - .../src/sample_sync_publisher.cpp | 150 --- .../src/sample_tilde_subscriber.cpp | 164 --- .../src/sample_tilde_synchronizer.cpp | 114 -- .../test/test_from_original_subscriber.cpp | 307 ----- .../test/test_from_original_synchronizer.cpp | 517 ------- .../test/test_tilde_subscriber.cpp | 178 --- .../test/test_tilde_synchronizer.cpp | 489 ------- src/tilde_msg/CMakeLists.txt | 89 -- src/tilde_msg/msg/DeadlineNotification.msg | 5 - src/tilde_msg/msg/MessageTrackingTag.msg | 4 - src/tilde_msg/msg/PubTopicTimeInfo.msg | 8 - src/tilde_msg/msg/Source.msg | 4 - .../msg/SteeAckermannControlCommand.msg | 2 - .../msg/SteeAckermannLateralCommand.msg | 2 - src/tilde_msg/msg/SteeDetectedObjects.msg | 2 - src/tilde_msg/msg/SteeImu.msg | 2 - src/tilde_msg/msg/SteeLongitudinalCommand.msg | 2 - src/tilde_msg/msg/SteeOccupancyGrid.msg | 2 - src/tilde_msg/msg/SteeOdometry.msg | 2 - src/tilde_msg/msg/SteePath.msg | 2 - src/tilde_msg/msg/SteePathWithLaneId.msg | 2 - src/tilde_msg/msg/SteePointCloud2.msg | 2 - src/tilde_msg/msg/SteePolygonStamped.msg | 2 - src/tilde_msg/msg/SteePoseStamped.msg | 2 - .../msg/SteePoseWithCovarianceStamped.msg | 2 - src/tilde_msg/msg/SteePredictedObjects.msg | 2 - src/tilde_msg/msg/SteeSource.msg | 3 - src/tilde_msg/msg/SteeTrackedObjects.msg | 2 - .../msg/SteeTrafficLightRoiArray.msg | 2 - src/tilde_msg/msg/SteeTrafficSignalArray.msg | 2 - src/tilde_msg/msg/SteeTrajectory.msg | 2 - src/tilde_msg/msg/SteeTwistStamped.msg | 2 - .../msg/SteeTwistWithCovarianceStamped.msg | 2 - src/tilde_msg/msg/SubTopicTimeInfo.msg | 5 - src/tilde_msg/msg/TestTopLevelStamp.msg | 1 - src/tilde_msg/package.xml | 31 - src/tilde_sample/CMakeLists.txt | 80 -- src/tilde_sample/README.md | 355 ----- .../multi_publisher_buffered_relay.launch.py | 35 - .../launch/multi_publisher_relay.launch.py | 35 - .../publisher_relay_with_header.launch.py | 31 - .../publisher_relay_without_header.launch.py | 31 - src/tilde_sample/package.xml | 27 - src/tilde_sample/src/goal.cpp | 55 - src/tilde_sample/src/relay_timer.cpp | 135 -- .../src/relay_timer_with_buffer.cpp | 142 -- src/tilde_sample/src/sample_publisher.cpp | 139 -- .../src/sample_stee_publisher.cpp | 86 -- src/tilde_vis/README.md | 194 --- src/tilde_vis/package.xml | 20 - src/tilde_vis/resource/tilde_vis | 0 src/tilde_vis/setup.cfg | 4 - src/tilde_vis/setup.py | 30 - src/tilde_vis/test/test_copyright.py | 23 - src/tilde_vis/test/test_data_as_tree.py | 192 --- src/tilde_vis/test/test_flake8.py | 25 - src/tilde_vis/test/test_latency_viewer.py | 272 ---- .../test/test_message_tracking_tag.py | 119 -- .../test_message_tracking_tag_traverse.py | 558 -------- src/tilde_vis/test/test_ncurse_printer.py | 35 - src/tilde_vis/test/test_pep257.py | 23 - src/tilde_vis/tilde_vis/__init__.py | 15 - src/tilde_vis/tilde_vis/caret_vis_tilde.py | 277 ---- src/tilde_vis/tilde_vis/data_as_tree.py | 128 -- src/tilde_vis/tilde_vis/latency_viewer.py | 792 ----------- .../tilde_vis/message_tracking_tag.py | 269 ---- .../message_tracking_tag_traverse.py | 462 ------- .../tilde_vis/parse_message_tracking_tag.py | 99 -- src/tilde_vis/tilde_vis/pathnode_vis.py | 202 --- src/tilde_vis/tilde_vis/printer.py | 215 --- src/tilde_vis_test/CMakeLists.txt | 61 - src/tilde_vis_test/README.md | 31 - .../launch/issues_23_1.launch.py | 36 - .../launch/issues_23_2.launch.py | 35 - src/tilde_vis_test/package.xml | 24 - src/tilde_vis_test/src/tilde_vis_test.cpp | 123 -- 274 files changed, 24514 deletions(-) delete mode 100644 CPPLINT.cfg delete mode 100644 README.md delete mode 100644 build_depends.repos delete mode 100644 codecov.yaml delete mode 100644 doc/README.md delete mode 100644 doc/build.md delete mode 100644 doc/images/tilde_dag.svg delete mode 100644 doc/images/tilde_deadline.svg delete mode 100644 doc/latency_viewer.md delete mode 100644 doc/mechanism.md delete mode 100644 doc/usecase.md delete mode 100644 elisp/README.md delete mode 100644 elisp/tilde-view-mode.el delete mode 100644 setup.cfg delete mode 100644 src/analysis/README.md delete mode 100755 src/analysis/create_bag_for_concat_filter.py delete mode 100755 src/analysis/graph_around.py delete mode 100644 src/analysis/graph_common.py delete mode 100644 src/analysis/message_tracking_tag.py delete mode 100755 src/analysis/parse_message_tracking_tag.py delete mode 100755 src/analysis/pubinfo_traverse.py delete mode 100644 src/analysis/rosgraph2_impl.py delete mode 100755 src/analysis/topic_times.py delete mode 100755 src/analysis/topic_traversal.py delete mode 100644 src/autoware_common/CODE_OF_CONDUCT.md delete mode 100644 src/autoware_common/CONTRIBUTING.md delete mode 100644 src/autoware_common/CPPLINT.cfg delete mode 100644 src/autoware_common/DISCLAIMER.md delete mode 100644 src/autoware_common/LICENSE delete mode 100644 src/autoware_common/README.md delete mode 100644 src/autoware_common/caret_lint_common/CMakeLists.txt delete mode 100644 src/autoware_common/caret_lint_common/README.md delete mode 100644 src/autoware_common/caret_lint_common/package.xml delete mode 100644 src/autoware_common/setup.cfg delete mode 100644 src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md delete mode 100644 src/autoware_msg/autoware_auto_msgs/LICENSE delete mode 100644 src/autoware_msg/autoware_auto_msgs/README.md delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl delete mode 100644 src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv delete mode 100644 src/autoware_msg/autoware_auto_msgs/build_depends.repos delete mode 100644 src/tilde/CMakeLists.txt delete mode 100644 src/tilde/README.md delete mode 100644 src/tilde/include/tilde/message_conversion.hpp delete mode 100644 src/tilde/include/tilde/message_conversion_detail.hpp delete mode 100644 src/tilde/include/tilde/stee_node.hpp delete mode 100644 src/tilde/include/tilde/stee_publisher.hpp delete mode 100644 src/tilde/include/tilde/stee_sources_table.hpp delete mode 100644 src/tilde/include/tilde/stee_subscription.hpp delete mode 100644 src/tilde/include/tilde/tilde_node.hpp delete mode 100644 src/tilde/include/tilde/tilde_publisher.hpp delete mode 100644 src/tilde/include/tilde/tp.h delete mode 100644 src/tilde/package.xml delete mode 100644 src/tilde/src/stee_node.cpp delete mode 100644 src/tilde/src/stee_republisher_node_imu.cpp delete mode 100644 src/tilde/src/stee_republisher_node_map.cpp delete mode 100644 src/tilde/src/stee_republisher_node_pointcloud2.cpp delete mode 100644 src/tilde/src/stee_sources_table.cpp delete mode 100644 src/tilde/src/tilde_node.cpp delete mode 100644 src/tilde/src/tilde_publisher.cpp delete mode 100644 src/tilde/src/tp.c delete mode 100644 src/tilde/test/test_stamp_processor.cpp delete mode 100644 src/tilde/test/test_stee_node.cpp delete mode 100644 src/tilde/test/test_stee_source_table.cpp delete mode 100644 src/tilde/test/test_tilde_node.cpp delete mode 100644 src/tilde/test/test_tilde_publisher.cpp delete mode 100644 src/tilde_cmake/CMakeLists.txt delete mode 100644 src/tilde_cmake/README.md delete mode 100644 src/tilde_cmake/cmake/tilde_package.cmake delete mode 100644 src/tilde_cmake/package.xml delete mode 100644 src/tilde_cmake/tilde_cmake-extras.cmake delete mode 100644 src/tilde_deadline_detector/CMakeLists.txt delete mode 100644 src/tilde_deadline_detector/README.md delete mode 100644 src/tilde_deadline_detector/autoware_sensors.yaml delete mode 100644 src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp delete mode 100644 src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp delete mode 100644 src/tilde_deadline_detector/package.xml delete mode 100644 src/tilde_deadline_detector/src/forward_estimator.cpp delete mode 100644 src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp delete mode 100644 src/tilde_deadline_detector/test/test_forward_estimator.cpp delete mode 100644 src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp delete mode 100644 src/tilde_early_deadline_detector/CMakeLists.txt delete mode 100644 src/tilde_early_deadline_detector/README.md delete mode 100644 src/tilde_early_deadline_detector/autoware_sensors.yaml delete mode 100644 src/tilde_early_deadline_detector/default_path.svg delete mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp delete mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp delete mode 100644 src/tilde_early_deadline_detector/package.xml delete mode 100644 src/tilde_early_deadline_detector/src/forward_estimator.cpp delete mode 100644 src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp delete mode 100644 src/tilde_message_filters/CMakeLists.txt delete mode 100644 src/tilde_message_filters/README.md delete mode 100644 src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp delete mode 100644 src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp delete mode 100644 src/tilde_message_filters/package.xml delete mode 100644 src/tilde_message_filters/src/sample_sync_publisher.cpp delete mode 100644 src/tilde_message_filters/src/sample_tilde_subscriber.cpp delete mode 100644 src/tilde_message_filters/src/sample_tilde_synchronizer.cpp delete mode 100644 src/tilde_message_filters/test/test_from_original_subscriber.cpp delete mode 100644 src/tilde_message_filters/test/test_from_original_synchronizer.cpp delete mode 100644 src/tilde_message_filters/test/test_tilde_subscriber.cpp delete mode 100644 src/tilde_message_filters/test/test_tilde_synchronizer.cpp delete mode 100644 src/tilde_msg/CMakeLists.txt delete mode 100644 src/tilde_msg/msg/DeadlineNotification.msg delete mode 100644 src/tilde_msg/msg/MessageTrackingTag.msg delete mode 100644 src/tilde_msg/msg/PubTopicTimeInfo.msg delete mode 100644 src/tilde_msg/msg/Source.msg delete mode 100644 src/tilde_msg/msg/SteeAckermannControlCommand.msg delete mode 100644 src/tilde_msg/msg/SteeAckermannLateralCommand.msg delete mode 100644 src/tilde_msg/msg/SteeDetectedObjects.msg delete mode 100644 src/tilde_msg/msg/SteeImu.msg delete mode 100644 src/tilde_msg/msg/SteeLongitudinalCommand.msg delete mode 100644 src/tilde_msg/msg/SteeOccupancyGrid.msg delete mode 100644 src/tilde_msg/msg/SteeOdometry.msg delete mode 100644 src/tilde_msg/msg/SteePath.msg delete mode 100644 src/tilde_msg/msg/SteePathWithLaneId.msg delete mode 100644 src/tilde_msg/msg/SteePointCloud2.msg delete mode 100644 src/tilde_msg/msg/SteePolygonStamped.msg delete mode 100644 src/tilde_msg/msg/SteePoseStamped.msg delete mode 100644 src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg delete mode 100644 src/tilde_msg/msg/SteePredictedObjects.msg delete mode 100644 src/tilde_msg/msg/SteeSource.msg delete mode 100644 src/tilde_msg/msg/SteeTrackedObjects.msg delete mode 100644 src/tilde_msg/msg/SteeTrafficLightRoiArray.msg delete mode 100644 src/tilde_msg/msg/SteeTrafficSignalArray.msg delete mode 100644 src/tilde_msg/msg/SteeTrajectory.msg delete mode 100644 src/tilde_msg/msg/SteeTwistStamped.msg delete mode 100644 src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg delete mode 100644 src/tilde_msg/msg/SubTopicTimeInfo.msg delete mode 100644 src/tilde_msg/msg/TestTopLevelStamp.msg delete mode 100644 src/tilde_msg/package.xml delete mode 100644 src/tilde_sample/CMakeLists.txt delete mode 100644 src/tilde_sample/README.md delete mode 100644 src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py delete mode 100644 src/tilde_sample/launch/multi_publisher_relay.launch.py delete mode 100644 src/tilde_sample/launch/publisher_relay_with_header.launch.py delete mode 100644 src/tilde_sample/launch/publisher_relay_without_header.launch.py delete mode 100644 src/tilde_sample/package.xml delete mode 100644 src/tilde_sample/src/goal.cpp delete mode 100644 src/tilde_sample/src/relay_timer.cpp delete mode 100644 src/tilde_sample/src/relay_timer_with_buffer.cpp delete mode 100644 src/tilde_sample/src/sample_publisher.cpp delete mode 100644 src/tilde_sample/src/sample_stee_publisher.cpp delete mode 100644 src/tilde_vis/README.md delete mode 100644 src/tilde_vis/package.xml delete mode 100644 src/tilde_vis/resource/tilde_vis delete mode 100644 src/tilde_vis/setup.cfg delete mode 100644 src/tilde_vis/setup.py delete mode 100644 src/tilde_vis/test/test_copyright.py delete mode 100644 src/tilde_vis/test/test_data_as_tree.py delete mode 100644 src/tilde_vis/test/test_flake8.py delete mode 100644 src/tilde_vis/test/test_latency_viewer.py delete mode 100644 src/tilde_vis/test/test_message_tracking_tag.py delete mode 100644 src/tilde_vis/test/test_message_tracking_tag_traverse.py delete mode 100644 src/tilde_vis/test/test_ncurse_printer.py delete mode 100644 src/tilde_vis/test/test_pep257.py delete mode 100644 src/tilde_vis/tilde_vis/__init__.py delete mode 100755 src/tilde_vis/tilde_vis/caret_vis_tilde.py delete mode 100644 src/tilde_vis/tilde_vis/data_as_tree.py delete mode 100644 src/tilde_vis/tilde_vis/latency_viewer.py delete mode 100644 src/tilde_vis/tilde_vis/message_tracking_tag.py delete mode 100755 src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py delete mode 100755 src/tilde_vis/tilde_vis/parse_message_tracking_tag.py delete mode 100644 src/tilde_vis/tilde_vis/pathnode_vis.py delete mode 100644 src/tilde_vis/tilde_vis/printer.py delete mode 100644 src/tilde_vis_test/CMakeLists.txt delete mode 100644 src/tilde_vis_test/README.md delete mode 100644 src/tilde_vis_test/launch/issues_23_1.launch.py delete mode 100644 src/tilde_vis_test/launch/issues_23_2.launch.py delete mode 100644 src/tilde_vis_test/package.xml delete mode 100644 src/tilde_vis_test/src/tilde_vis_test.cpp diff --git a/CPPLINT.cfg b/CPPLINT.cfg deleted file mode 100644 index ba6bdf08..00000000 --- a/CPPLINT.cfg +++ /dev/null @@ -1,14 +0,0 @@ -# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120 -set noparent -linelength=100 -includeorder=standardcfirst -filter=-build/c++11 # we do allow C++11 -filter=-build/namespaces_literals # we allow using namespace for literals -filter=-runtime/references # we consider passing non-const references to be ok -filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions -filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space -filter=-whitespace/parens # we allow closing parenthesis to be on the next line -filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon -filter=-build/header_guard # we automatically fix the names of header guards using pre-commit -filter=-build/include_order # we use the custom include order -filter=-build/include_subdir # we allow the style of "foo.hpp" diff --git a/README.md b/README.md deleted file mode 100644 index f2bf920c..00000000 --- a/README.md +++ /dev/null @@ -1,22 +0,0 @@ -# TILDE - Tilde Is Latency Data Embedding - -early_deadline_detector is included in the package. - -[README(ja)](./doc/README.md) - -## Contents - -| src/ | about | -| ------------ | ---------------------------------------------------- | -| tilde | TILDE main code | -| tilde_sample | samples including talker, relay, listener and so on. | -| tilde_msg | TILDE internal msg | -| tilde_vis | TILDE results visualization | - -## Build - -```bash -. /path/to/ros2_galactic/install/setup.bash -vcs import src < build_depends.repos -colcon build --symlink-install --cmake-args --cmake-args -DCMAKE_BUILD_TYPE=Release -``` diff --git a/build_depends.repos b/build_depends.repos deleted file mode 100644 index 4a3bc882..00000000 --- a/build_depends.repos +++ /dev/null @@ -1,9 +0,0 @@ -repositories: - autoware_msg/autoware_auto_msgs: - type: git - url: https://github.com/tier4/autoware_auto_msgs.git - version: tier4/main - autoware_common: - type: git - url: https://github.com/tier4/caret_common.git - version: main diff --git a/codecov.yaml b/codecov.yaml deleted file mode 100644 index 8ca21967..00000000 --- a/codecov.yaml +++ /dev/null @@ -1,22 +0,0 @@ -coverage: - status: - project: - default: - target: auto - patch: - default: - target: auto - -comment: - show_carryforward_flags: true - -flag_management: - default_rules: - carryforward: true - statuses: - - name_prefix: project- - type: project - target: auto - - name_prefix: patch- - type: patch - target: auto diff --git a/doc/README.md b/doc/README.md deleted file mode 100644 index 49ea945d..00000000 --- a/doc/README.md +++ /dev/null @@ -1,148 +0,0 @@ -# TILDE - Tilde Is Latency Data Embedding - -TILDE は複数のノードにまたがるレイテンシ測定やデッドライン検出の為のフレームワークです。 -TILDE はノードから出力されたトピックを遡り、元となった入力トピック(センサー)を特定することができます。 -入力から出力までを辿れる為、レイテンシ計測とデッドライン検出が可能です。 - - - -## Table of Contents - -- [TILDE - Tilde Is Latency Data Embedding](#tilde---tilde-is-latency-data-embedding) - - [Table of Contents](#table-of-contents) - - [TILDE の目標](#tilde-の目標) - - [想定ユースケース](#想定ユースケース) - - [動作原理](#動作原理) - - [API](#api) - - [tilde::TildeNode](#tildetildenode) - - [インストール, 組み込み](#インストール-組み込み) - - [デッドライン検出](#デッドライン検出) - - [周辺ツール](#周辺ツール) - - [リポジトリ一覧](#リポジトリ一覧) - - - -## TILDE の目標 - -ROS2 のトピック通信は、一般に以下の様な有向グラフ(DAG)を構成します。 - -![tilde_dag](./images/tilde_dag.svg) - -「センサーの情報がアクチュエーターにいつ反映されるか」つまり「あるトピックはどのタイミングのセンサーの情報を参照したものか」は難しい問いです。 - -- DAG は必ずしも一本道では無いため、経路ごとに参照されているセンサーの時刻が異なる可能性があります。 - - 例えば PlanningNode は SensorNodeB からの情報を FusionNode 経由で受信する場合と直接受信する場合があります。 - - また PlanningNode は FeedbackNode を通じて SensorNodeC の情報を参照しています。 -- ループが含まれる場合も考える必要があります。 - - FeedbackNode を通じて PlanningNode は潜在的には過去全ての SensorNodeA, SensorNodeB の情報を参照しているとも言えます。 -- DAG からは分からないこともあります。 - - 例えば、ノードによってはメッセージをバッファリングし選択的に入力データを利用している可能性があります。 - -TILDE ではこの様な複雑な問題にも対応できる様、 DAG を解析し、ランタイム時に各トピックがいつのセンサー情報を元に作成されたかを求め、その情報を元にレイテンシ解析やデッドライン検出をすることを目標としています。 - -## 想定ユースケース - -TILDE では以下の様なユースケースを想定ユースケースしています。 - -- オンラインレイテンシ測定 -- デッドライン検出 - - パスの途中終了 - - 入力情報の「賞味期限」検出 - -詳しくは [usecase](./usecase.md) をご覧下さい。 - -## 動作原理 - -TILDE ではメイントピックの publish 時に MessageTrackingTag というメタ情報を `/message_tracking_tag` に publish します。 -MessageTrackingTag は数百バイト程度のメッセージで、メイントピックを構成する入力トピックの情報が記載されます。 -MessageTrackingTag の詳細やオーバーヘッドについては [mechanism](./mechanism.md) をご覧下さい。 - -## API - -TILDE では ROS2 rclcpp と同じ引数で名前少し異なる API 群を用意しています。 -ご自分のアプリケーションに取り込む際は rclcpp::Node を tilde::TildeNode に置換するなど、機械的な置換で利用可能です。 - -- Node - - [tilde::TildeNode](../src/tilde/include/tilde/tilde_node.hpp). 下記も参照。 -- Publisher - - [tilde::Node::create_tilde_publisher()](../src/tilde/include/tilde/tilde_node.hpp) - - [tilde::TildePublisher](../src/tilde/include/tilde/tilde_publisher.hpp) - - [tilde::TildePublisher::publish()](../src/tilde/include/tilde/tilde_publisher.hpp) -- Subscription - - [tilde::Node::create_tilde_subscription()](../src/tilde/include/tilde/tilde_node.hpp) -- MessageTrackingTag explicit API - - [tilde::TildePublisherBase::set_explicit_input_info()](../src/tilde/include/tilde/tilde_publisher.hpp) - - [tilde::TildePublisherBase::set_max_sub_callback_infos_sec()](../src/tilde/include/tilde/tilde_publisher.hpp) -- Deadline detection - - T.B.D. - -### tilde::TildeNode - -rclcpp::Node の子クラスです。 -以下の Parameter を持ちます。 - -- `enable_tilde` - - boolean. - - false の場合 TILDE の機能がオフになる。つまり Subscription callback でのフックや publish 時の MessageTrackingTag 送信が抑制される。 - - 2022/03/02 現在初期化時のみ指定可能。 - -## インストール, 組み込み - -[build](./build.md) - -既存のアプリケーションに TILDE を組み込む際は以下の流れになります。 -※ TODO: TILDE ライブラリ（\*.so）の取込み手順 - -- packages.xml に以下を追加 - -```xml -tilde_cmake -tilde -``` - -- CMakeLists.txt に以下を追加 - -```cmake -find_package(tilde_cmake REQUIRED) -tilde_package() -find_package(tilde REQUIRED) -``` - -- rclcpp::Node, rclcpp::Node::create_publisher, rclcpp::Node::create_subscription に代わり tilde の各 API の利用 - - rclcpp::Node とコンストラクタを tilde::TildeNode に置換 - - create_subscription() を create_tilde_subscription() に置換 - - create_publisher() を create_tilde_publisher() に置換 - - rclcpp::Publisher を tilde::TildePublisher に置換 -- 対象ファイルに include 文の追加 - - `#include "tilde/tilde_node.hpp"` - - `#include "tilde/tilde_publisher.hpp"` -- package.xml に tilde を追加 - - `tilde` -- CMakeLists.txt に tilde を追加 -- メッセージをバッファしているノードがあれば MessageTrackingTag explicit API の呼び出しを追加 - -サンプルプロジェクトは [tilde_sample](../src/tilde_sample) をご覧下さい。 - -- sample_publisher.cpp - - ros2/demos の talker.cpp 相当のプログラムです - - メイントピックの header.stamp の有無が異なる 2 サンプルがあります。 - - `TildeNode`, `create_tilde_publisher` の例です。 -- relay_timer.cpp - - sample_publisher.cpp のトピックを受信・転送するプログラムです。 - - `create_tilde_subscription` の例です。 -- relay_timer_with_buffer.cpp - - relay_timer.cpp で buffer がある例です。 - - relay_timer.cpp に加え explicit API を用いています。 - -## デッドライン検出 - -> **_NOTE:_** 2022/01/21 現在本機能は未実装 - -## 周辺ツール - -- [latency viewer](./latency_viewer.md): オンラインレイテンシ表示ツール - -## リポジトリ一覧 - -- tilde 本体 -- tilde tools ← latency viewer を含む各種 TILDE ツールを 1 レポジトリにまとめる予定 diff --git a/doc/build.md b/doc/build.md deleted file mode 100644 index caffedd4..00000000 --- a/doc/build.md +++ /dev/null @@ -1,18 +0,0 @@ -# Tilde のビルド - -ROS2 galactic 環境が構築済みのこと。 - -```bash -. /path/to/ros2/galactic/setup.bash - -git clone git@github.com:tier4/TILDE.git - -rosdep install \ - --from-paths src --ignore-src \ - --rosdistro galactic -y \ - --skip-keys "console_bridge fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers" - -sudo apt-get install liblttng-ust-dev - -colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release -``` diff --git a/doc/images/tilde_dag.svg b/doc/images/tilde_dag.svg deleted file mode 100644 index 538b15c8..00000000 --- a/doc/images/tilde_dag.svg +++ /dev/null @@ -1,761 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - SensorNode - ObstacelCheker - FusionNode - - SensorNodeB - - /sensor/topic/A - - /sensor/fusion - - /sensor/topic/B - - /sensor/topic/C - - /planning/base - - /planning/fback - - /planning/refine - - ControlNode - - /control/cmd - - - - - - - - - - - - - - - - - - - FeedbackNode - - - - - - - - - SensorNodeC - - - - PlanningNode - - - - - diff --git a/doc/images/tilde_deadline.svg b/doc/images/tilde_deadline.svg deleted file mode 100644 index 55d05d68..00000000 --- a/doc/images/tilde_deadline.svg +++ /dev/null @@ -1,325 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - NodeA - - NodeB - - /topicA - - /topicB - - - - TargetNode - - - 10hz - 10hz - - case2 - - case1 - - <= 100 ms - - diff --git a/doc/latency_viewer.md b/doc/latency_viewer.md deleted file mode 100644 index ac133cc8..00000000 --- a/doc/latency_viewer.md +++ /dev/null @@ -1,50 +0,0 @@ -# Latency Viewer - -Latency Viewer はあるセンサー入力からのレンテンシの表示ツールです。 -MessageTrackingTag を利用して DAG を遡ることで、注目しているトピックからセンサーまで全てのパスにおけるレイテンシを確認できます。 -vmstat や top の様に定期的にレイテンシ情報が更新されます。 - -起動方法やオプションについては [ビシュアライズツールの README](../src/tilde_vis/README.md) をご覧下さい。 - -## 出力例 - -trajectory_follower を表示した結果は以下の通りです。 - -トピック・header stamp・レイテンシが 2 種類が表示されます。 - -- トピック名 - - 前のスペースの数でパスが表示されます -- header stamp - - メイントピックの header stamp です -- レイテンシ - - ROS time ベースと steady time の 2 種類が表示されています。 - - レイテンシはある行のトピックが publish されてから `/control/trajectory_follower/lateral/control_cmd` が publish されるまでの経過時間です - -```text -/control/trajectory_follower/lateral/control_cmd 1618559274.549348133 0 0 - /localization/twist 1618559274.544330488 5 3 - /planning/scenario_planning/trajectory 1618559274.479350019 15 12 - (snip) - /sensing/lidar/concatenated/pointcloud 1618559273.951109000 289 288 - /sensing/lidar/left/outlier_filtered/pointcloud 1618559274.168087000 305 302 - /sensing/lidar/left/mirror_cropped/pointcloud_ex 1618559274.168087000 305 304 - /sensing/lidar/left/self_cropped/pointcloud_ex 1618559274.168087000 305 304 - /sensing/lidar/left/pointcloud_raw_ex* NA NA NA - /sensing/lidar/right/outlier_filtered/pointcloud 1618559274.170810000 300 301 - /sensing/lidar/right/mirror_cropped/pointcloud_ex 1618559274.170810000 305 302 - /sensing/lidar/right/self_cropped/pointcloud_ex 1618559274.170810000 305 303 - /sensing/lidar/right/pointcloud_raw_ex* NA NA NA - /sensing/lidar/top/outlier_filtered/pointcloud 1618559273.951109000 334 333 - /sensing/lidar/top/mirror_cropped/pointcloud_ex 1618559273.951109000 410 407 - /sensing/lidar/top/self_cropped/pointcloud_ex 1618559273.951109000 435 433 - /sensing/lidar/top/pointcloud_raw_ex* NA NA NA - /vehicle/status/twist* NA NA NA -``` - -以下のことが読みとれます。 - -- `trajectory_follower` は `/localization/twist` と `/planning/scenario_planning/trajectory` を入力とする - - それぞれのレイテンシは ROS time (今回はシミュレーション時間) で 5 ms, 15 ms、 steady time で 3 ms, 12 ms. -- `/planning/scenario_planning/trajectory` に至るまでに concat filter (`/sensing/lidar/concatenated/pointcloud`) がある - - 最終的に `/sensing/lidar/left/self_cropped/pointcloud_ex` などに辿りつく. - - `/sensing/lidar/left/pointcloud_raw_ex` は終端(センサー)だが、TILDE 非対応の為、レイテンシ等は NA になっている diff --git a/doc/mechanism.md b/doc/mechanism.md deleted file mode 100644 index 07928034..00000000 --- a/doc/mechanism.md +++ /dev/null @@ -1,320 +0,0 @@ -# TILDE の動作原理 - -TILDE は、ユーザプログラムがメイントピックを publish するのに併せて MessageTrackingTag というトピックを `/message_tracking_tag` に publish します。 -MessageTrackingTag は数百バイト程度のメッセージで、メイントピックを構成する入力トピックの情報が記載されます。 - -ここで **メイントピック** とはアプリケーションが本来やりとりするメッセージです。 -TILDE が MessageTrackingTag という付加的なトピックをやりとりする為、アプリケーション本来のメッセージをメイントピックと呼称しています。 - - - -## Table of Contents - -- [TILDE の動作原理](#tilde-の動作原理) - - [Table of Contents](#table-of-contents) - - [MessageTrackingTag](#messagetrackingtag) - - [例](#例) - - [DAG と動作概要](#dag-と動作概要) - - [stamp](#stamp) - - [NodeC の MessageTrackingTag](#nodec-の-messagetrackingtag) - - [MessageTrackingTag の作成メカニズム](#messagetrackingtag-の作成メカニズム) - - [概要](#概要) - - [class](#class) - - [create_tilde_publisher](#create_tilde_publisher) - - [create_tilde_subscription](#create_tilde_subscription) - - [publish](#publish) - - [Explicit API](#explicit-api) - - [オーバーヘッド](#オーバーヘッド) - - - -まず MessageTrackingTag についてデータ構造と例を記述します。 -次に TILDE が MessageTrackingTag を作る仕組みを記述します。 -最後にオーバーヘッドについて記載します。 - -## MessageTrackingTag - -MessageTrackingTag はメイントピックの publish 時と同時に送信されるメタ情報です。 -`<メイントピック名>/message_tracking_tag` に送信されます。 - -メッセージ定義は以下の通りです。 -※ TODO: ファイル名やデータ構造はリファクタ予定 - -[MessageTrackingTag.msg](../src/tilde_msg/msg/MessageTrackingTag.msg) - -- Header: - - header - - シーケンス番号 - - 送信者情報(node 名や publisher ID) -- `output_info`: 出力トピックに関する情報 - - トピック名: メイントピックのトピック名 - - header stamp: メイントピックの header stamp - - 送信時刻: メイントピックの送信時刻 -- `input_infos`: 入力トピックに関する情報。下記を入力トピック分。 - - トピック名: メイントピックのトピック名 - - header stamp: メイントピックの header stamp - - 受信時刻: メイントピックの受信時刻 - -トピック名やノード名の長さにもよりますがデータサイズは以下の通りです。送信周波数はメイントピックと同じです。 - -- Header + output_info: 約 80 バイト + トピック名やノード名分のバイト数 -- input_infos: (入力トピック数 \* 約 40 バイト) + トピック名のバイト数 - -## 例 - -### DAG と動作概要 - -NodeA, NodeB, NodeC からなる以下の DAG を考えます(丸はノード、四角はトピック)。 - -```mermaid -graph LR - NodeA((NodeA)) --t=2,stamp=1--> topic_a - NodeB((NodeB)) --t=4,stamp=3--> topic_b - topic_a --> NodeC((NodeC)) - topic_b --> NodeC - NodeC --t=6,stamp=5--> topic_c -``` - -- 動作概要 - - NodeA は t=2 に、NodeB は t=4 に publish します。 - - NodeC は t=6 に起床し、これらの情報を元に自身の計算をして topic_c を送信します。 -- 補足 - - 矢印中の stamp は次に示す通り ros2 std_msgs::msg::Header の stamp フィールドです。 - - メッセージ識別に利用します。 - -### stamp - -各 Node では ros2 の sensor_msgs やアプリケーション固有のメッセージ型を送信します。 -例えば `sensor_msgs/msg/PointCloud2` の場合、以下の様なメッセージになります(説明の為フィールドは省略しています)。 - -```mermaid -classDiagram - direction LR - Header <.. PointCloud2 - class PointCloud2 { - +header: Header - +height - +width - +data: uint8[] - } - class Header { - +stamp: Time - } -``` - -ROS2 のセンサーやナーゲーションで用いられるメッセージでは標準的に header フィールドが付与されています。 -header フィールドには stamp フィールドがあり、ユーザ定義のタイムスタンプを記入します。 -TILDE では **header フィールドの stamp を利用** によりメッセージを識別します。 - -### NodeC の MessageTrackingTag - -TILDE はメインメッセージの publish をフックして MessageTrackingTag を送信します。 -NodeC の出力する MessageTrackingTag は以下の様な物になります(フィールドは一部省略しています)。 - -```mermaid -classDiagram - direction LR - OutputInfo <.. MessageTrackingTag_NodeC - InputInfo_TopicA <.. MessageTrackingTag_NodeC - InputInfo_TopicB <.. MessageTrackingTag_NodeC - class MessageTrackingTag_NodeC { - +seq: 0 - +Node: NodeC - +output_info - +input_infos[0]: InputInfo_TopicA - +input_infos[1]: InputInfo_TOpicB - } - class OutputInfo { - +topic: "topic_c" - +send_time: 6 - +stamp: 5 - } - class InputInfo_TopicA { - +topic: "topic_a" - +stamp: 1 - +recv_time: 2 - } - class InputInfo_TopicB { - +topic: "topic_b" - +stamp: 3 - +recv_time: 4 - } -``` - -この様に MessageTrackingTag には **Node 単位で、出力と入力の紐付け情報** が記載されます。 -より具体的には **送信したメインメッセージと、自身が subscription している入力トピックのメッセージを header stamp で紐付け** ます。 -上記の様に、デフォルトではメッセージの紐付けは各入力トピックについて **メインメッセージ送信前に受信した最新のメッセージ** になっています。 explicit API により明示的に紐付け情報を設定することが可能です。 - -## MessageTrackingTag の作成メカニズム - -MessageTrackingTag はメイントピックの publish 時に同時に送信されます。 -また `input_infos` では入力トピックとの紐付け情報が設定されます。 - -これらを行なうためには subscription 時に `input_infos` 用の情報を保持したり、 MessageTrackingTag のメッセージを作成・送信する必要がありますが、TILDE が publish や subscription callback をフックしてこれらの処理を実行する為、アプリケーションでこれらを意識する必要はありません。 - -### 概要 - -以下で UML 風の図を用いて TILDE の動作概要を記述します。先に簡単に言葉でまとめます。 - -- アプリケーションから見た TILDE - - MessageTrackingTag を作成するのに必要な情報の蓄積や MessageTrackingTag の送信は TILDE が行なう。 - - その為、基本的にはアプリケーションで TILDE や MessageTrackingTag のことを考える必要はない。 - - ただし内部でバッファリングしている場合、メッセージを正しくトラッキングするには input_info を明示的に登録する必要がある -- MessageTrackingTag の作成 - - TILDE のカスタム create_publisher によりカスタムの Publisher である TildePublisher が作成される。 - - TildePublisher は入力情報に関するデータを持っている。 - - メイントピックの publish をフックし、メイントピックを送信すると同時に入力情報データから MessageTrackingTag を作成して MessageTrackingTag を送信する。 -- input infos の紐付け - - TILE のカスタム create_subscription により subscription コールバックがフックされる。 - - フック中の処理で TildePublisher に対して入力トピックや header stamp などの情報を登録する。 - -### class - -TILDE では以下のクラス・API を提供します。 - -- rclcpp::Node や rclcpp::Publisher に相当する TildeNode や TIldePublisher -- `create_publisher` や `create_subscription` の TILDE カスタム版 -- いずれもクラス名・関数名が異なることを除けば ROS2 のものと同じシグニチャです。よって機械的変換で組み込むことができます。 - -```mermaid -classDiagram - Node <|-- TildeNode - Publisher <.. TildePublisher - TildeNode <|-- UserNode - TildePublisher <.. UserNode - Subscription~T~ <.. UserNode - - class Node{ - +create_publisher(topic) - +create_subscription(topic, cb) - } - class TildeNode{ - +create_tilde_publisher(topic) - +create_tilde_subscription(topic, cb) - } - class Publisher~T~ { - +publish(msg) - } - class TildePublisher { - +publish(msg) - +main_pub_: Publisher - +message_tracking_tag_pub_: Publisher - -input_info - } - class Subscription~T~ { - } - class UserNode { - +subscription_callback(T msg) - -pub_: TildePublisher - } -``` - -### create_tilde_publisher - -`create_tilde_publisher(topic, ...)` によりメインメッセージと MessageTrackingTag 用の publisher が作成されます。 -MessageTrackingTag 用のトピック名はメイントピック名に `/message_tracking_tag` という接尾語がついたものです。 - -```mermaid -sequenceDiagram - UserNode-->>TildeNode: create_tilde_publisher(topic, ...) - activate TildeNode - TildeNode-->TildeNode: tilde_pub_ = new TildePublisher - TildeNode-->topic: tilde_pub_.main_pub_ = create_publisher(topic, ...) - note over topic: topic created - TildeNode-->topic/message_tracking_tag: tilde_pub.message_tracking_tag_pub_ = create_publisher(topic + "/message_tracking_tag", ...) - note over topic/message_tracking_tag: topic created - TildeNode-->>UserNode: tilde_pub - deactivate TildeNode -``` - -`create_tilde_publisher` の返り値は rclcpp::Publisher ではなく TildePublisher です。 - -### create_tilde_subscription - -`create_tilde_subscription(topic, qos, cb)` により Subscription が作成されます。 -TILDE では subscription callback をフックする為、ユーザ指定のコールバック関数 cb を TILDE 用のコールバック関数でラップした新たなコールバック関数を登録します。 - -疑似コードで記載すると以下の様になります。 - -```cpp -void create_tilde_subscription(topic, qos, cb) { - auto tilde_cb = [this, topic, cb](T msg) { - // MessageTrackingTag 用に入力情報の紐付け - auto sub_time = now(); - this->tilde_pub.set_input_info( - topic, - sub_time, - msg.header.stamp); - - // cb の呼び出し - cb(msg); - }; - - return this->create_subscription(topic ,qos, tilde_cb); -} -``` - -subscription 時の動作は以下の通りです。 -subscription 時に TildePublisher の input_info 情報を登録します。 - -```mermaid -sequenceDiagram - topic-->>Executor: msg - Executor-->>+UserNode: tilde_callback(msg) - UserNode-->>UserNode: register input_info - activate UserNode - UserNode-->>UserNode: cb(msg) = user defined callback - deactivate UserNode - UserNode-->-Executor: ret -``` - -### publish - -TildePublisher は登録済みの `input_info` を参照して MessageTrackingTag を作成します。 -MessageTrackingTag とメインメッセージを送信します。 - -```mermaid -sequenceDiagram - UserNode-->>TildePublisher: pub.publish(msg) - activate TildePublisher - TildePublisher-->TildePublisher: message_tracking_tag = get_message_tracking_tag() - - TildePublisher-->>topic/message_tracking_tag: message_tracking_tag_pub_.publish(message_tracking_tag) - TildePublisher-->>topic: main_pub_.publish(msg); - deactivate TildePublisher -``` - -## Explicit API - -[NodeC の MessageTrackingTag](#nodec-の-messagetrackingtag) では「メインメッセージ送信前に受信した最新のメッセージ」に紐付けられると記述しました。 -受信メッセージを内部でバッファして選択的に利用している、あるいは入力トピックが複数ありそれぞれバッファリングしている等、入力トピックと出力トピックが明示的に紐付かないノードでは explicit API を使って明示的に紐付け情報を設定することが可能です。 - -下図は 4 入力、 1 出力のノードの例です。 -それぞれの入力はバッファされ publish 時に選択的に利用されます。 -この図では `/left` は L1-L3 の 3 世代あり、 L2 が使われています。 - -```text - 入力をバッファ callback の中でバッファの中から - 利用するデータを選択 - buffer +- callback-+ -/left --> L1 L2 L3 -> | L2 | - | | -/right --> R1 R2 -> | R1 | --> /concatenate - | | -/top --> T1 T2 T3 -> | T2 | - | | -/twist --> W1 W2 W3 -> | W2 W3 | - +-----------+ - ^ - TILDE では「最も最近受信した stamp」しか覚えていない為、正確な MessageTrackingTag を作成できない -``` - -Explicit API では「`/concatenate` を作成するのに `/left` の L2、`/right` の R1 (以下略)を使った」という指定ができます。 - -## オーバーヘッド - -※ TODO: 大体以下を記載する。 - -- TILDE 有無時のオーバヘッド - - autoware か demo システムを対象に TILDE 適用有無時のベンチマーク結果を記載する - - CPU 利用率やネットワーク帯域など ← リソース分析で検討しているプロセスリソースモニタリングツールが使える? diff --git a/doc/usecase.md b/doc/usecase.md deleted file mode 100644 index 2e561094..00000000 --- a/doc/usecase.md +++ /dev/null @@ -1,117 +0,0 @@ -# TILDE の想定ユースケース - -TILDE では以下の様なユースケースを想定ユースケースしています。 - -- オンライン測定 -- デッドライン検出 - - パスの途中終了 - - 入力情報の「賞味期限」検出 - - - -## Table of Contents - -- [TILDE の想定ユースケース](#tilde-の想定ユースケース) - - [Table of Contents](#table-of-contents) - - [TILDE で分かること](#tilde-で分かること) - - [オンラインレイテンシ計測](#オンラインレイテンシ計測) - - [オンライン性](#オンライン性) - - [オフライン性ツール CARET との比較](#オフライン性ツール-caret-との比較) - - [デッドライン検出](#デッドライン検出) - - - -## TILDE で分かること - -ROS2 のトピック通信は、一般に以下の様な有向グラフ(DAG)を構成します。 - -![tilde_dag](./images/tilde_dag.svg) - -TILDE では、各ノードでメインのトピックを publish する際に MessageTrackingTag という「メイントピックを構成する入力トピックの情報(トピック)」を同時に publish します。 -メッセージの特定の為、メインのトピックは ROS2 の [std_msgs/msg/Header](https://github.com/ros2/common_interfaces/blob/master/std_msgs/msg/Header.msg) フィールドを持ち stamp フィールドに適切な値を設定している必要があります。 - -以下の図の様なケースを考えます。 -stamp はメイントピックの Header stamp で、説明の為単位は秒とします。 - -```mermaid -graph LR - /sensor/topic/A --stamp=4--> FusionNode((FusionNode)) - /sensor/topic/B --stamp=6--> FusionNode - FusionNode --stamp=8-->/sensor/fusion - /sensor/fusion --stamp=8--> PlanningNode((PlanningNode)) - /sensor/topic/B --stamp=3--> PlanningNode - PlanningNode --stamp=10-->/planning/base -``` - -FusionNode は以下の様な MessageTrackingTag を送信します。 - -- **MessageTrackingTag(FusionNode)**: stamp=8 の `/sensor/fusion` は以下のトピックを参照した - - t=5 に受信した stamp=4 の `/sensor/topic/A` - - t=7 に受信した stamp=6 の `/sensor/topic/B` - - 他、トピックの送信時刻などの付属情報 - -PlanningNode も同様の MessageTrackingTag を送信します。 - -- **MessageTrackingTag(PlanningNode)**: stamp=10 の `/planning/base` は以下のトピックを参照した - - t=8 に受信した stamp=8 の `/sensor/fusion` - - t=4 に受信した stamp=3 の `/sensor/topic/B` - -これらの情報から、stamp=10 の `/planning/base` は以下のセンサー情報を元に算出されたと分かります。 - -- **推論**: stamp=10 の `/planning/base` は以下のトピックを参照している - - `/sensor/topic/A`: stamp=4 のもの(`/sensor/fusion` 経由) - - `/sensor/topic/B`: stamp=6 のもの(`/sensor/fusion` 経由) と stamp=3 のもの(直接受信) - -同様の推論を繰り返すことで「stamp=X の `/control/cmd` はいつのセンサー情報を用いたか?」という問いに答えることができます。 -MessageTrackingTag を元に、DAG を逆向きに遡ってデータの紐付けを行うことからこの様な推論を **MessageTrackingTag の探索** と呼称します。 - -## オンラインレイテンシ計測 - -上記の推論から stamp=10 の `/planning/base` は各センサーの最も古い値として以下を参照していることが分かります。 - -- `/sensor/topic/A`: stamp=4 のもの(`/sensor/fusion` 経由) -- `/sensor/topic/B`: stamp=3 のもの(直接受信) - -「センサーの stamp = センサーの計測時刻」と仮定するとこのデータを作るまで **センサー A 取得から 6 秒, センサー B からは 7 秒かかった** と言えます。 - -他のノードでも同様の推論が可能です。 - -また、 MessageTrackingTag の探索を途中で止めることでノード間にかかった時間も分かります。 -例えば 「/sensor/fusion が送信されてから /control/cmd が送信されるまで X 秒かかった」などの推論ができます。 - -この様にして TILDE を用いてレンテンシを計測することが可能です。 - -### オンライン性 - -MessageTrackingTag は topic で送信される為、計測対象のシステムを動かしながら MessageTrackingTag を解析することが可能です。 -latency viewer では top や vmstat の様にシステムを動かしながらレンテンシを見ることができます。 -TILDE ではこの様にシステムを動かしながらメトリククを計算できる性質を **オンライン** と呼称しています。 - -### オフライン性ツール CARET との比較 - -[CARET](https://tier4.github.io/CARET_doc/) は LTTng のトレースポイントを用いて計算する為、システムを動かした後に性能を評価します。この様に事後に解析することを、**オフライン** と呼称しています。 - -レンテンシ計測の観点では TILDE と CARET は似ていますが、以下の様に特徴付けられます。 - -- TILDE はオンラインにレイテンシを計測できるが取得できるレイテンシは荒い -- CARET はオフライン計測だが、細かいレイテンシを取得できる - -## デッドライン検出 - -> **_NOTE:_** (2022/01/21 現在本機能は未実装) - -以下の様な DAG を考えます。この DAG は所定の時間内(例えば 100 ms 以内)に処理を追える必要があるとします。 -NodeA、NodeB はタイマーで動作すると考えます。 - -![tilde_deadline](./images/tilde_deadline.svg) - -上記の通り TILDE により TargetNode は topicA の stamp や送信時間を取得できる為、「TargetNode の処理開始時点で topicA 送信から 80 ms 経過している」等と推論できます。 - -その他 TILDE の情報を用いることで以下の様な問題を検出することができます。 - -- (1) 直接的な入力トピックの遅延 (case1) - - NodeB やネットワークの遅延により TargetNode が topicB を想定した周期で受信できない場合があります。 - - TargetNode 実行時に「最後に topicB を受信した」を取得できる為、この様な問題を検出できます。 -- (2) 間接的な入力トピックの遅延 (case2) - - NodeB は正しい周波数で動作しているが NodeA が停止・あるいは topicA の遅延などで、NodeB が参照している情報が古い可能性があります。 - - TILDE では MessageTrackingTag を辿ることで、センサーや途中のトピックの諸元を調べることができるため、この様な問題を検出できます。 diff --git a/elisp/README.md b/elisp/README.md deleted file mode 100644 index aa7dea55..00000000 --- a/elisp/README.md +++ /dev/null @@ -1,50 +0,0 @@ -# TILDE view mode - -## About - -Emacs major mode for viewing latency viewer stdout log. -You can see stdout log just like ncurses window. - -## Usage - -First, run tilde latency viewer with `--batch` mode. -Redirect stdout to file. - -Open the log file, and `M-x tilde-view-mode`. -Of course, you need read tilde-view-mode.el in advance -by putting load path, or `M-x eval-buffer` or other fancy way. - -## Key Bindings - -The latency viewer log file has space-index structure. -In the non batch mode, latency viewer shows lines between `/target/topic`. -Let's call these lines a BLOCK. - -```text -stamp ... -/target/topic - /depth1/child/topic1 - /depth2/child/topic1 - /depth2/child/topic2 - /depth1/child/topic2 - /depth2/child/topic3 -stamp ... - (snip) -``` - -You can select "wide" and "narrow" view. - -### wide view mode - -In the wide view, we can see full text. -Additionally, we can fold sub trees by hitting "Tab" or "o". -Type "a" to show all lines. - -### narrow view mode - -Move cursor inside BLOCK, and hit "space" to narrow region. -Hit "n" or "p" to move BLOCKs. -Hit "w" to widen. - -We can use the narrow view mode, just like latency viewer non-batch mode. -We can also move the block with the region folded. diff --git a/elisp/tilde-view-mode.el b/elisp/tilde-view-mode.el deleted file mode 100644 index 270a7d56..00000000 --- a/elisp/tilde-view-mode.el +++ /dev/null @@ -1,79 +0,0 @@ -(defun tilde-narrow-block () - (interactive) - (save-excursion - (forward-char) - (let* ((bgn (re-search-backward "^/" nil t)) - (end (re-search-forward "^/" nil t 2)) - ) - (if end - (progn - (message (int-to-string end)) - (narrow-to-region bgn (- end 1)) - ) - (narrow-to-region bgn (point-max)) - )))) - -(defun tilde-next-block () - (interactive) - (forward-char) - (re-search-forward "^/" nil 1) - (backward-char)) - -(defun tilde-prev-block () - (interactive) - (re-search-backward "^/" nil 1)) - -(defun tilde-next-block-with-narrow () - (interactive) - (if (not (buffer-narrowed-p)) - (progn - (tilde-next-block)) - (widen) - (tilde-next-block) - (tilde-narrow-block) - )) - -(defun tilde-prev-block-with-narrow () - (interactive) - (if (not (buffer-narrowed-p)) - (progn - (tilde-prev-block)) - (widen) - (tilde-prev-block) - (tilde-narrow-block) - )) - -(defun tilde-previous-line () - (interactive) - (previous-line) - (move-beginning-of-line 1)) - -(defun tilde-next-line () - (interactive) - (next-line) - (move-beginning-of-line 1)) - -(defun tilde-view-mode () - (interactive) - (kill-all-local-variables) - (setq major-mode 'tilde-view-mode - mode-name "TILDE view mode") - (outline-minor-mode) - (setq outline-regexp " *") - - (setq tilde-view-mode-map (make-keymap)) - (suppress-keymap tilde-view-mode-map) - (define-key tilde-view-mode-map (kbd "TAB") 'outline-toggle-children) - (define-key tilde-view-mode-map "a" 'outline-show-all) - (define-key tilde-view-mode-map "o" 'outline-hide-sublevels) - - (define-key tilde-view-mode-map "j" 'tilde-next-line) - (define-key tilde-view-mode-map "k" 'tilde-previous-line) - (define-key tilde-view-mode-map "n" 'tilde-next-block-with-narrow) - (define-key tilde-view-mode-map "p" 'tilde-prev-block-with-narrow) - - (define-key tilde-view-mode-map "w" 'widen) - (define-key tilde-view-mode-map (kbd "SPC") 'tilde-narrow-block) - - (use-local-map tilde-view-mode-map) - ) diff --git a/setup.cfg b/setup.cfg deleted file mode 100644 index 5214751c..00000000 --- a/setup.cfg +++ /dev/null @@ -1,15 +0,0 @@ -[flake8] -# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini -extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 -import-order-style = pep8 -max-line-length = 100 -show-source = true -statistics = true - -[isort] -profile=black -line_length=100 -force_sort_within_sections=true -force_single_line=true -reverse_relative=true -known_third_party=launch diff --git a/src/analysis/README.md b/src/analysis/README.md deleted file mode 100644 index 7dd18c84..00000000 --- a/src/analysis/README.md +++ /dev/null @@ -1,103 +0,0 @@ -# old scripts for analysis - -## Description - -ros2 の bag や graph を操作するためのユーティリティ群。 -オプションは `-h` でヘルプを参照のこと。 - -## topic_times.py - -bag file 内のトピックの送信時刻を出力する。 -時刻は header field の timestamp を参照する。無い場合はその旨出力して終了する。 -シミュレータを動かし全トピックを計測の上、送信時刻を確認するなど。 - -```bash -$ ./topic_times.py - -$ ./topic_times.py lsim_all_topics/lsim_all_topics_0.db3 /sensing/lidar/top/rectified/pointcloud -1618559266.752622000: -1618559266.850460000: -1618559266.950311000: -1618559267.506740000: -1618559267.150548000: -1618559267.250492000: -1618559267.350317000: -1618559267.450254000: -1618559267.550121000: -1618559267.650272000: -1618559267.750288000: -``` - -## topic_traversal.py - -topic や node の from, to を指定して最短経路を取得する。 -`rqt_graph` がバックエンドの為、実機やシミュレータなど対象システムが動作している状態で実行すること。 - -```bash -$ ./topic_traversal.py - -$ ./topic_traversal.py /sensing/lidar/top/pointcloud_raw_ex /sensing/lidar/top/rectified/pointcloud -Graph refresh: done, updated[True] -ROS stats update took 1.032935380935669s - -args: /sensing/lidar/top/pointcloud_raw_ex -> /sensing/lidar/top/rectified/pointcloud -graph: /sensing/lidar/top/pointcloud_raw_ex| -> /sensing/lidar/top/rectified/pointcloud| - -Found - ' /sensing/lidar/top/pointcloud_raw_ex|' - '/sensing/lidar/top/crop_box_filter_self|' - ' /sensing/lidar/top/self_cropped/pointcloud_ex|' - '/sensing/lidar/top/crop_box_filter_mirror|' - ' /sensing/lidar/top/mirror_cropped/pointcloud_ex|' - '/sensing/lidar/top/velodyne_interpolate_node|' - ' /sensing/lidar/top/rectified/pointcloud|' -``` - -Found のうち、半角スペース始まりは topic、そうでないものは Node の様子(不確かか書き方なのは内部で使っている `rqt_graph` が加工したもののため)。 - -## graph_around - -ある地点からグラフを BFS してノードやトピックを出力する。 - -```bash -$ ./graph_around.py /localization/pose_twist_fusion_filter/ekf_localizer --depth 2 - -depth: 0 -now: '/localization/pose_twist_fusion_filter/ekf_localizer|' - -depth: 1 -now: ' /tf|' -now: ' /localization/pose_twist_fusion_filter/pose|' -now: ' /localization/pose_twist_fusion_filter/pose_with_covariance|' -now: ' /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias|' -now: ' /localization/pose_twist_fusion_filter/twist|' - -depth: 2 -now: '/perception/object_recognition/prediction/transform_listener_impl_55931a9941c0|' -now: '/sensing/lidar/pointcloud_preprocessor/transform_listener_impl_5588e2e7d940|' -now: '/sensing/lidar/pointcloud_preprocessor/transform_listener_impl_5588e2f57090|' -(snip) -``` - -## parse_message_tracking_tag.py - -PugInfo が保存された bag ファイルを message_tracking_tag.py のデータ書式に変換し -pkl 形式で保存する。 -結果は `${CWD}/topic_infos.pkl` に保存される。 - -```bash -parse_message_tracking_tag.py -``` - -## message_tracking_tag_traverse.py - -parse_message_tracking_tag.py で作成された pkl ファイルを元に E2E latency を算出する。 -第三引数が終点となるトピック名。 -第二引数は何番目に送信されたトピック(正確には Message_Tracking_Tag)を対象とするか。あまり若い数字だと初期化中の為辿り付けないセンサーが発生することがある。 - -```bash -./message_tracking_tag_traverse.py \ - /topic_infos.pkl \ - 1000 \ - /localization/pose_twist_fusion_filter/twist_with_covariance -``` diff --git a/src/analysis/create_bag_for_concat_filter.py b/src/analysis/create_bag_for_concat_filter.py deleted file mode 100755 index f00eeb68..00000000 --- a/src/analysis/create_bag_for_concat_filter.py +++ /dev/null @@ -1,96 +0,0 @@ -#!/usr/bin/python3 - -import argparse -import os -import shutil - -import rosbag2_py -from rosidl_runtime_py.utilities import get_message -from rclpy.serialization import deserialize_message - -serialization_format = 'cdr' - - -def main(args): - in_bag_path = args.in_bag - out_bag_path = args.out_dir - - if os.path.exists(out_bag_path): - shutil.rmtree(out_bag_path) - - in_storage_options = rosbag2_py.StorageOptions(uri=in_bag_path, storage_id='sqlite3') - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format) - - reader = rosbag2_py.SequentialReader() - reader.open(in_storage_options, converter_options) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - topics = [ - "/sensing/lidar/left/outlier_filtered/pointcloud", - "/sensing/lidar/right/outlier_filtered/pointcloud", - "/sensing/lidar/top/outlier_filtered/pointcloud", - "/vehicle/status/twist", - "/clock" - ] - - # topics to save. - # design: - # - # We save all of twist and clock. - topics_to_save = { - "/sensing/lidar/left/outlier_filtered/pointcloud": [ - 500, 505 - ], - "/sensing/lidar/right/outlier_filtered/pointcloud": [ - 500, 504, 505 - ], - "/sensing/lidar/top/outlier_filtered/pointcloud": [ - 500, - ] - } - - storage_filter = rosbag2_py.StorageFilter(topics=topics) - reader.set_filter(storage_filter) - - out_storage_options = rosbag2_py.StorageOptions(out_bag_path, "sqlite3") - writer = rosbag2_py.SequentialWriter() - writer.open(out_storage_options, converter_options) - - for meta in reader.get_all_topics_and_types(): - writer.create_topic(meta) - - counts = {k: 0 for k in topics} - while reader.has_next(): - (topic, data, t) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - - counts[topic] += 1 - - if "clock" in topic: - writer.write(topic, data, t) - elif "twist" in topic: - writer.write(topic, data, t) - elif counts[topic] in topics_to_save[topic]: - print(f"{topic}: {msg.header.stamp}") - writer.write(topic, data, t) - - if counts["/sensing/lidar/left/outlier_filtered/pointcloud"] == 1000: - break - - del writer - rosbag2_py.Reindexer().reindex(out_storage_options) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("in_bag", help="input bagfile") - parser.add_argument("out_dir", help="output directory") - args = parser.parse_args() - - main(args) diff --git a/src/analysis/graph_around.py b/src/analysis/graph_around.py deleted file mode 100755 index ab86ddba..00000000 --- a/src/analysis/graph_around.py +++ /dev/null @@ -1,89 +0,0 @@ -#!/usr/bin/env python3 -# list around the node - -import argparse -from collections import deque - -import rclpy -from rosgraph2_impl import Edge - -from graph_common import find_key, get_graph - -EXCLUDE_TOPICS = [ - "/parameter_events", - "/rosout", - "/clock", - ] - - -def main(args): - frm = args.frm - max_depth = args.depth - least_edges = args.least_edges - - rclpy.init() - graph = get_graph(least_edges) - - key = find_key(graph.nt_edges.edges_by_end, frm) - if not key: - print("{} not found".format(frm)) - return - print("key: '{}'".format(key)) - - seen = set() - current_item = deque([key]) - next_item = deque([]) - depth = 0 - - edges = graph.nt_edges.edges_by_start - print("depth: {}".format(depth)) - while len(current_item) != 0 and depth <= max_depth: - now = current_item.popleft() - print("now: '{}'".format(now)) - - for nxt in edges[now]: - if isinstance(nxt, Edge): - # print("nxt.start: {}".format(nxt.start)) - nxt = nxt.end - nxt = nxt.strip() - if nxt in EXCLUDE_TOPICS: - continue - nxt = find_key(edges, nxt) - if nxt is None: - continue - if nxt in seen: - continue - seen.add(nxt) - next_item.append(nxt) - - # for n in edges[nxt]: - # if isinstance(n, Edge): - # # print("nxt.start: {}".format(nxt.start)) - # n = n.end - # n = n.strip() - # n = find_key(edges, n) - # if n in EXCLUDE_TOPICS: - # continue - # if n is None: - # continue - # if n in seen: - # continue - # next_item.append(n) - - if len(current_item) == 0: - depth += 1 - current_item = next_item - next_item = deque([]) - print("") - if depth <= max_depth: - print("depth: {}".format(depth)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("frm") - parser.add_argument("--depth", type=int, default=2) - parser.add_argument("--least-edges", type=int, default=200, - help="the least number of edges to wait for graph.update") - args = parser.parse_args() - main(args) diff --git a/src/analysis/graph_common.py b/src/analysis/graph_common.py deleted file mode 100644 index 81d17bfc..00000000 --- a/src/analysis/graph_common.py +++ /dev/null @@ -1,33 +0,0 @@ -import rclpy -from rosgraph2_impl import Graph - - -def get_graph(least_num, prints_edge=False): - node = rclpy.create_node("topic_traversal_node") - graph = Graph(node) - graph.update() - - cnt = 0 - while cnt < 10 and len(graph.nt_edges.edges_by_end) <= least_num: - cnt += 1 - graph = Graph(node) - graph.update() - - if len(graph.nt_edges.edges_by_end) <= least_num: - raise Exception("not enough edges") - - print("found {} keys".format(len(graph.nt_edges.edges_by_end.keys()))) - - if prints_edge: - for k in graph.nt_edges.edges_by_end.keys(): - print(" '{}'".format(k)) - - return graph - - -def find_key(edges, name): - for key in edges: - if name in key and 0 <= len(key) - len(name) <= 2: - # print("find_key: {} -> {}".format(name, key)) - return key - return None diff --git a/src/analysis/message_tracking_tag.py b/src/analysis/message_tracking_tag.py deleted file mode 100644 index e79286dc..00000000 --- a/src/analysis/message_tracking_tag.py +++ /dev/null @@ -1,131 +0,0 @@ -def time2str(t): - return f"{t.sec}.{t.nanosec:09d}" - - -class TopicInfo(object): - def __init__(self, topic, has_stamp, stamp): - self.topic = topic - self.has_stamp = has_stamp - self.stamp = stamp - - def __str__(self): - stamp_s = time2str(self.stamp) if self.has_stamp else "NA" - return f"TopicInfo(topic={self.topic}, stamp={stamp_s})" - - -class Message_Tracking_Tag(object): - """ - Message_Tracking_Tag. - - - out_info = TopicInfo - - in_infos = {topic_name => list of TopicInfo} - """ - - def __init__(self, out_topic, out_stamp): - self.out_info = TopicInfo(out_topic, True, out_stamp) - # topic name vs TopicInfo - self.in_infos = {} - - def add_input_info(self, in_topic, has_stamp, stamp): - self.in_infos.setdefault(in_topic, []).append(TopicInfo(in_topic, has_stamp, stamp)) - - def __str__(self): - s = "Message_Tracking_Tag: \n" - s += f" out_info={self.out_info}\n" - for _, ti in self.in_infos.items(): - s += f" in_infos={ti}\n" - return s - - @property - def out_topic(self): - return self.out_info.topic - - -class Message_Tracking_Tags(object): - """ - Topic vs Message_Tracking_Tag. - - We have double-key dictionary internally, i.e. - we can get Message_Tracking_Tag by topic_vs_message_tracking_tag[topic_name][stamp]. - """ - - def __init__(self): - self.topic_vs_message_tracking_tags = {} - - def add(self, message_tracking_tag): - out_topic = message_tracking_tag.out_info.topic - out_stamp = time2str(message_tracking_tag.out_info.stamp) - if out_topic not in self.topic_vs_message_tracking_tags.keys(): - self.topic_vs_message_tracking_tags[out_topic] = {} - infos = self.topic_vs_message_tracking_tags[out_topic] - - if out_stamp not in infos.keys(): - infos[out_stamp] = {} - - infos[out_stamp] = message_tracking_tag - - def topics(self): - return list(self.topic_vs_message_tracking_tags.keys()) - - def stamps(self, topic): - """Return List[stamps].""" - if topic not in self.topic_vs_message_tracking_tags.keys(): - return [] - - return list(self.topic_vs_message_tracking_tags[topic].keys()) - - def get(self, topic, stamp=None, idx=None): - """ - Get corresponding Message_Tracking_Tag. - - topic: str - stamp: str - """ - ret = None - if topic not in self.topic_vs_message_tracking_tags.keys(): - return None - - if stamp is None and idx is None: - return list(self.topic_vs_message_tracking_tags[topic].values())[0] - - infos = self.topic_vs_message_tracking_tags[topic] - - if stamp in infos.keys(): - ret = infos[stamp] - - if idx is not None: - if len(infos.keys()) <= idx: - print("args.idx too large, should be < {len(info.kens())}") - else: - key = sorted(infos.keys())[idx] - ret = infos[key] - - return ret - - def in_topics(self, topic): - """ - Gather input topics by ignoring stamps. - - return set of topic name - """ - ret = set() - - if topic not in self.topic_vs_message_tracking_tags.keys(): - return ret - - for message_tracking_tag in self.topic_vs_message_tracking_tags[topic].values(): - for t in message_tracking_tag.in_infos.keys(): - ret.add(t) - return ret - - def all_topics(self): - out = set() - - lhs = self.topics() - for t in lhs: - out.add(t) - - rhs = self.in_topics(t) - for t in rhs: - out.add(t) - return out diff --git a/src/analysis/parse_message_tracking_tag.py b/src/analysis/parse_message_tracking_tag.py deleted file mode 100755 index 6ff1c757..00000000 --- a/src/analysis/parse_message_tracking_tag.py +++ /dev/null @@ -1,84 +0,0 @@ -#!/usr/bin/python3 - -import pickle -import argparse - -import rosbag2_py -from rosidl_runtime_py.utilities import get_message -from rclpy.serialization import deserialize_message - -from message_tracking_tag import Message_Tracking_Tag, Message_Tracking_Tags - - -def get_rosbag_options(path, serialization_format='cdr'): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format) - - return storage_options, converter_options - - -def main(args): - bag_path = args.bag_path - - storage_options, converter_options = get_rosbag_options(bag_path) - - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - cnt = 0 - - # topic => list of record - out_per_topic = Message_Tracking_Tags() - - skip_topic_vs_count = {} - - while reader.has_next() and cnt <= args.cnt: - (topic, data, t) = reader.read_next() - # TODO: need more accurate check - if "/message_tracking_tag" not in topic: - continue - - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - - out_topic = msg.output_info.topic_name - out_stamp = msg.output_info.header_stamp - - message_tracking_tag = Message_Tracking_Tag(out_topic, out_stamp) - - for input_info in msg.input_infos: - in_topic = input_info.topic_name - in_has_stamp = input_info.has_header_stamp - if not in_has_stamp: - if in_topic not in skip_topic_vs_count.keys(): - print(f"skip {in_topic} as no header.stamp, out_topic = {out_topic}") - skip_topic_vs_count[in_topic] = 1 - else: - skip_topic_vs_count[in_topic] += 1 - continue - in_stamp = input_info.header_stamp - - message_tracking_tag.add_input_info(in_topic, in_has_stamp, in_stamp) - - out_per_topic.add(message_tracking_tag) - - pickle.dump(out_per_topic, open("topic_infos.pkl", "wb"), protocol=pickle.HIGHEST_PROTOCOL) - - for topic, count in skip_topic_vs_count.items(): - print(f"skipped {topic} {count} times") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("bag_path") - parser.add_argument("--cnt", type=int, default=10) - args = parser.parse_args() - - main(args) diff --git a/src/analysis/pubinfo_traverse.py b/src/analysis/pubinfo_traverse.py deleted file mode 100755 index 7d5c8ac7..00000000 --- a/src/analysis/pubinfo_traverse.py +++ /dev/null @@ -1,206 +0,0 @@ -#!/usr/bin/python3 - -import pickle -import argparse -from collections import deque, defaultdict -import time - -from rclpy.time import Time - -from message_tracking_tag import time2str - - -def str_stamp2time(timestamp: str): - sec, nanosec = timestamp.split(".") - return Time(seconds=int(sec), nanoseconds=int(nanosec)) - - -class InputSensorStampSolver(object): - def __init__(self): - # {topic: {stamp: {sensor_topic: [stamps]}}} - self.topic_stamp_to_sensor_stamp = {} - - def solve(self, message_tracking_tags, tgt_topic, tgt_stamp): - """ - Solve. - - topic: target topic - stamp: target stamp(str) - - return list of {sensor: oldest stamp} - - Calculate topic_stamp_to_sensor_stamp internally. - """ - graph = TopicGraph(message_tracking_tags) - path_bfs = graph.bfs_rev(tgt_topic) - is_leaf = {t: b for (t, b) in path_bfs} - - stamp = tgt_stamp - - # dists[topic][stamp] - dists = defaultdict(lambda: defaultdict(lambda: -1)) - queue = deque() - parentQ = deque() - - dists[tgt_topic][tgt_stamp] = 1 - queue.append((tgt_topic, tgt_stamp)) - parentQ.append("") - - st = time.time() - start = None - while len(queue) != 0: - topic, stamp = queue.popleft() - parent = parentQ.popleft() - is_leaf_s = "looks_sensor" if is_leaf[topic] else "" - - if start is None: - start = str_stamp2time(stamp) - dur = start - str_stamp2time(stamp) - dur_ms = dur.nanoseconds // 10**6 - - print(f"{topic:80} {stamp:>20} {dur_ms:4} ms {is_leaf_s} {parent}") - - # NDT-EKF has loop, so skip - if topic == "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias": - continue - - # get next edges - next_message_tracking_tag = message_tracking_tags.get(topic, stamp) - if not next_message_tracking_tag: - continue - - for in_infos in next_message_tracking_tag.in_infos.values(): - for in_info in in_infos: - nx_topic = in_info.topic - nx_stamp = time2str(in_info.stamp) - if dists[nx_topic][nx_stamp] > 0: - continue - dists[nx_topic][nx_stamp] = dists[topic][stamp] + 1 - queue.append((nx_topic, nx_stamp)) - parentQ.append(topic) - - et = time.time() - print(f"solve internal: {(et-st)*1000} [ms]") - - def append(self, topic, stamp, sensor_topic, sensor_stamp): - dic = self.topic_stamp_to_sensor_stamp - if topic not in dic.keys(): - dic[topic] = {} - if stamp not in dic[topic].keys(): - dic[topic][stamp] = {} - if sensor_topic not in dic[topic][stamp].keys(): - dic[topic][stamp][sensor_topic] = [] - - dic[topic][stamp][sensor_topic].append(sensor_stamp) - - -class TopicGraph(object): - """Construct topic graph by ignoring stamps.""" - - def __init__(self, message_tracking_tags): - self.topics = sorted(message_tracking_tags.all_topics()) - self.t2i = {t: i for i, t in enumerate(self.topics)} - n = len(self.topics) - - # from sub -> pub - self.topic_edges = [set() for _ in range(n)] - # from out -> in - self.rev_edges = [set() for _ in range(n)] - for out_topic in self.topics: - in_topics = message_tracking_tags.in_topics(out_topic) - - out_id = self.t2i[out_topic] - for in_topic in in_topics: - in_id = self.t2i[in_topic] - self.topic_edges[in_id].add(out_id) - self.rev_edges[out_id].add(in_id) - - def rev_topics(self, topic): - """ - Get input topics. - - return List[Topic] - """ - input_topics = self.t2i[topic] - return [self.topics[i] for i in input_topics.rev_edges] - - def dfs_rev(self, start_topic): - """ - Traverse topic graph reversely from start_topic. - - return topic names in appearance order - """ - edges = self.rev_edges - n = len(edges) - seen = [False for _ in range(n)] - sid = self.t2i[start_topic] - - ret = [] - - def dfs(v): - ret.append(self.topics[v]) - seen[v] = True - for nxt in edges[v]: - if seen[nxt]: - continue - dfs(nxt) - dfs(sid) - - return ret - - def bfs_rev(self, start_topic): - """Return list of (topic, is_leaf).""" - edges = self.rev_edges - n = len(edges) - dist = [-1 for _ in range(n)] - queue = deque() - - sid = self.t2i[start_topic] - dist[sid] = 0 - queue.append(sid) - paths = [] - - while len(queue) != 0: - v = queue.popleft() - paths.append((self.topics[v], len(edges[v]) == 0)) - for nv in edges[v]: - if dist[nv] >= 0: - continue - dist[nv] = dist[v] + 1 - queue.append(nv) - - return paths - - -def main(args): - pickle_file = args.pickle_file - message_tracking_tags = pickle.load(open(pickle_file, "rb")) - - tgt_topic = args.topic - tgt_stamp = sorted(message_tracking_tags.stamps(tgt_topic))[args.stamp_index] - - # graph = TopicGraph(message_tracking_tags) - # bfs_path = graph.bfs_rev(tgt_topic) - # print(f"BFS from {tgt_topic}") - # for p, is_leaf in bfs_path: - # print(f" {p} {is_leaf}") - # print("") - - st = time.time() - solver = InputSensorStampSolver() - solver.solve(message_tracking_tags, tgt_topic, tgt_stamp) - et = time.time() - - print(f"solve {(et-st) * 1000} [ms]") - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("pickle_file") - parser.add_argument("stamp_index", type=int, default=0, - help="header stamp index") - parser.add_argument("topic", default="/sensing/lidar/no_ground/pointcloud", nargs="?") - - args = parser.parse_args() - - main(args) diff --git a/src/analysis/rosgraph2_impl.py b/src/analysis/rosgraph2_impl.py deleted file mode 100644 index 78b43da8..00000000 --- a/src/analysis/rosgraph2_impl.py +++ /dev/null @@ -1,589 +0,0 @@ -# Software License Agreement (BSD License) -# -# Copyright (c) 2008, Willow Garage, Inc. -# All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# * Redistributions in binary form must reproduce the above -# copyright notice, this list of conditions and the following -# disclaimer in the documentation and/or other materials provided -# with the distribution. -# * Neither the name of Willow Garage, Inc. nor the names of its -# contributors may be used to endorse or promote products derived -# from this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; -# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -# Revision $Id$ -# Adopted from the ROS1 ros_comm repository -# https://github.com/ros/ros_comm/blob/5de058ad1bbf3a525ae3f5288a76e59ec0dd62d0/tools/rosgraph/src/rosgraph/impl/graph.py - -# flake8: noqa - -from __future__ import print_function - -""" -Data structures and library for representing ROS Computation Graph state. -""" - -import time -import itertools -import random - -from collections import defaultdict - -from python_qt_binding.QtCore import qDebug, qWarning -from rclpy.qos import qos_check_compatible -from rclpy.qos import QoSCompatibility - - -_ROS_NAME = '/rosviz' - - -def topic_node(topic): - """ - Write me. - - In order to prevent topic/node name aliasing, we have to remap - topic node names. Currently we just prepend a space, which is - an illegal ROS name and thus not aliased. - @return str: topic mapped to a graph node name. - """ - return ' ' + topic - - -def node_topic(node): - """ - Write me. - - Inverse of topic_node - @return str: undo topic_node() operation - """ - return node[1:] - - -class BadNode(object): - """ - Write me. - - Data structure for storing info about a 'bad' node - """ - - # no connectivity - DEAD = 0 - # intermittent connectivity - WONKY = 1 - - def __init__(self, name, node_type, reason): - """ - Write me. - - @param type: DEAD | WONKY - @type type: int - """ - self.name = name - self.reason = reason - self.type = node_type - - -class EdgeList(object): - """ - Write me. - - Data structure for storing Edge instances - """ - - __slots__ = ['edges_by_start', 'edges_by_end'] - - def __init__(self): - # in order to make it easy to purge edges, we double-index them - self.edges_by_start = {} - self.edges_by_end = {} - - def __iter__(self): - return itertools.chain(*[v for v in self.edges_by_start.values()]) # NOLINT - - def has(self, edge): - return edge in self - - def __contains__(self, edge): - """ - Write me. - - @return: True if edge is in edge list - @rtype: bool - """ - key = edge.key - return key in self.edges_by_start and edge in self.edges_by_start[key] - - def add(self, edge): - """ - Add an edge to our internal representation. not multi-thread safe. - - @param edge: edge to add - @type edge: Edge - """ - # see note in __init__ - def update_map(edge_map, key, edge): - if key in edge_map: - edges = edge_map[key] - if edge not in edges: - edges.append(edge) - return True - else: - return False - else: - edge_map[key] = [edge] - return True - - updated = update_map(self.edges_by_start, edge.key, edge) - updated = update_map(self.edges_by_end, edge.r_key, edge) or updated - return updated - - def add_edges(self, start, dest, direction, label='', qos=None): - """ - Write me. - - Create Edge instances for args and add resulting edges to edge - list. Convenience method to avoid repetitive logging, etc... - @param edge_list: data structure to add edge to - @type edge_list: EdgeList - @param start: name of start node. If None, warning will be logged and add fails - @type start: str - @param dest: name of start node. If None, warning will be logged and add fails - @type dest: str - @param direction: direction string (i/o/b) - @type direction: str - @return: True if update occurred - @rtype: bool - """ - # the warnings should generally be temporary, occurring of the - # master/node information becomes stale while we are still - # doing an update - updated = False - if not start: - qWarning("cannot add edge: cannot map start [%s] to a node name", start) - elif not dest: - qWarning("cannot add edge: cannot map dest [%s] to a node name", dest) - else: - for args in edge_args(start, dest, direction, label, qos): - updated = self.add(Edge(*args)) or updated - return updated - - def delete_all(self, node): - """ - Delete all edges that start or end at node. - - @param node: name of node - @type node: str - """ - def matching(map_, pref): - return [map_[k] for k in map.keys() if k.startswith(pref)] - - pref = node+"|" - edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_end, pref) - for el in edge_lists: - for e in el: - self.delete(e) - - def delete(self, edge): - # see note in __init__ - def update_map(map, key, edge): # NOLINT - if key in map: - edges = map[key] - if edge in edges: - edges.remove(edge) - return True - update_map(self.edges_by_start, edge.key, edge) - update_map(self.edges_by_end, edge.r_key, edge) - - -class Edge(object): - """Data structure for representing ROS node graph edge.""" - - __slots__ = ['start', 'end', 'label', 'key', 'r_key', 'qos'] - - def __init__(self, start, end, label='', qos=None): - self.start = start - self.end = end - self.label = label - self.qos = qos - self.key = "%s|%s" % (self.start, self.label) - # reverse key, indexed from end - self.r_key = "%s|%s" % (self.end, self.label) - - def __ne__(self, other): - return self.start != other.start or self.end != other.end - - def __str__(self): - return "%s->%s" % (self.start, self.end) - - def __eq__(self, other): - return self.start == other.start and self.end == other.end and \ - self.label == other.label and self.qos == other.qos - - -def edge_args(start, dest, direction, label, qos): - """ - Compute argument ordering for Edge constructor based on direction flag. - - @param direction str: i, o, or b (in/out/bi_dir) relative to start - @param start str: name of starting node - @param start dest: name of destination node - """ - edge_args = [] - if direction in ['o', 'b']: - edge_args.append((start, dest, label, qos)) - if direction in ['i', 'b']: - edge_args.append((dest, start, label, qos)) - return edge_args - - -class Graph(object): - """ - Utility class for polling ROS statistics from running ROS graph. - - Not multi-thread-safe - """ - - def __init__(self, node, node_ns='/', topic_ns='/'): - self._node = node - self.node_ns = node_ns or '/' - self.topic_ns = topic_ns or '/' - - # ROS nodes - self.nn_nodes = set([]) # NOLINT - # ROS topic nodes - self.nt_nodes = set([]) # NOLINT - - # ROS nodes that aren't responding quickly - self.bad_nodes = {} - import threading - self.bad_nodes_lock = threading.Lock() - - # ROS services - self.srvs = set([]) - # ROS node->node transport connections - self.nn_edges = EdgeList() - # ROS node->topic connections - self.nt_edges = EdgeList() - # ROS all node->topic connections, including empty - self.nt_all_edges = EdgeList() - - # node names to URI map - self.node_uri_map = {} # { node_name_str : uri_str } - # reverse map URIs to node names - self.uri_node_map = {} # { uri_str : node_name_str } - - # time we last updated the graph - self.last_graph_refresh = 0 - self.last_node_refresh = {} - - self.graph_stale = 5.0 - # time we last communicated with node - # seconds until node data is considered stale - self.node_stale = 5.0 # seconds - - # dictionary with qos incompatibilities found - # {topic_name: {publisher_node_name: [subscription_node_name, ...], ...}, ...} - self.topic_with_qos_incompatibility = defaultdict(lambda: defaultdict(list)) - - def set_node_stale(self, stale_secs): - """ - Write me. - - @param stale_secs: seconds that data is considered fresh - @type stale_secs: double - """ - self.node_stale = stale_secs - - def _graph_refresh(self): - """ - Write me. - - @return: True if nodes information was updated - @rtype: bool - """ - updated = False - publishers = defaultdict(list) - subscriptions = defaultdict(list) - servers = defaultdict(list) - - publisher_topic_names = set() - subscriber_topic_names = set() - - for name, namespace in self._node.get_node_names_and_namespaces(): - node_name = namespace + name if namespace.endswith('/') else namespace + '/' + name - - for topic_name, topic_type in \ - self._node.get_publisher_names_and_types_by_node(name, namespace): - publishers[topic_name].append(node_name) - publisher_topic_names.add(topic_name) - - for topic_name, topic_type in \ - self._node.get_subscriber_names_and_types_by_node(name, namespace): - subscriptions[topic_name].append(node_name) - subscriber_topic_names.add(topic_name) - - for service_name, service_type in \ - self._node.get_service_names_and_types_by_node(name, namespace): - servers[service_name].append(node_name) - - publisher_qos_lists = defaultdict(lambda: defaultdict(list)) - for topic_name in publisher_topic_names: - for topic_endpoint_info in self._node.get_publishers_info_by_topic(topic_name): - node_name = topic_endpoint_info.node_namespace - if not node_name.endswith('/'): - node_name += '/' - node_name += topic_endpoint_info.node_name - publisher_qos_lists[topic_name][node_name].append(topic_endpoint_info.qos_profile) - subscriber_qos_lists = defaultdict(lambda: defaultdict(list)) - for topic_name in subscriber_topic_names: - for topic_endpoint_info in self._node.get_subscriptions_info_by_topic(topic_name): - node_name = topic_endpoint_info.node_namespace - if not node_name.endswith('/'): - node_name += '/' - node_name += topic_endpoint_info.node_name - subscriber_qos_lists[topic_name][node_name].append(topic_endpoint_info.qos_profile) - - for topic, node_qos_profiles_dict in publisher_qos_lists.items(): - for pub_node, pub_qos_list in node_qos_profiles_dict.items(): - for sub_node, sub_qos_list in subscriber_qos_lists[topic].items(): - if any( - qos_check_compatible(pub_qos, sub_qos)[0] == QoSCompatibility.ERROR - for pub_qos in pub_qos_list - for sub_qos in sub_qos_list - ): - self.topic_with_qos_incompatibility[topic][pub_node].append(sub_node) - - pubs = list(publishers.items()) - subs = list(subscriptions.items()) - srvs = list(servers.items()) - - nodes = [] - nt_all_edges = EdgeList() - nt_nodes = self.nt_nodes - for state, direction in ((pubs, 'o'), (subs, 'i')): - for topic, l in state: - if topic.startswith(self.topic_ns): - nodes.extend([n for n in l if n.startswith(self.node_ns)]) - nt_nodes.add(topic_node(topic)) - for node in l: - if direction == 'o': - qos_list = publisher_qos_lists[topic][node] - elif direction == 'i': - qos_list = subscriber_qos_lists[topic][node] - else: - qos_list = {None} - for qos in qos_list: - updated |= nt_all_edges.add_edges( - node, topic_node(topic), direction, qos=qos) - self.nt_nodes = nt_nodes - self.nt_all_edges = nt_all_edges - self.nt_edges = nt_all_edges - - nn_edges = EdgeList() - for topic, pub_nodes in publishers.items(): - for pub_node, sub_node in itertools.product(pub_nodes, subscriptions.get(topic, [])): - updated = nn_edges.add_edges(pub_node, sub_node, 'o', topic) or updated - self.nn_edges = nn_edges - - nodes = set(nodes) - - srvs = set([s for s, _ in srvs]) - purge = None - if nodes ^ self.nn_nodes: - purge = self.nn_nodes - nodes - self.nn_nodes = nodes - updated = True - if srvs ^ self.srvs: - self.srvs = srvs - updated = True - - if purge: - qDebug("following nodes and related edges will be purged: %s", ','.join(purge)) - for p in purge: - self.nn_edges.delete_all(p) - self.nt_edges.delete_all(p) - self.nt_all_edges.delete_all(p) - - qDebug("Graph refresh: done, updated[%s]" % updated) - return updated - - def _mark_bad_node(self, node, reason): - try: - # bad nodes are updated in a separate thread, so lock - self.bad_nodes_lock.acquire() - if node in self.bad_nodes: - self.bad_nodes[node].type = BadNode.DEAD - else: - self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason)) - finally: - self.bad_nodes_lock.release() - - def _unmark_bad_node(self, node, reason): - """Promotes bad node to 'wonky' status.""" - try: - # bad nodes are updated in a separate thread, so lock - self.bad_nodes_lock.acquire() - bad = self.bad_nodes[node] - bad.type = BadNode.WONKY - finally: - self.bad_nodes_lock.release() - - def _node_refresh_bus_info(self, node, bad_node=False): - """ - Retrieve bus info from the node and update nodes and edges as appropriate. - - @param node: node name - @type node: str - @param api: XML-RPC proxy - @type api: ServerProxy - @param bad_node: If True, node has connectivity issues and - should be treated differently - @type bad_node: bool - """ - if bad_node: - self._mark_bad_node(node, 'Not found during discovery') - return True - - def _node_refresh(self, node, bad_node=False): - """ - Contact node for stats/connectivity information. - - @param node: name of node to contact - @type node: str - @param bad_node: if True, node has connectivity issues - @type bad_node: bool - @return: True if node was successfully contacted - @rtype bool - """ - # TODO: I'd like for master to provide this information in - # getSystemState() instead to prevent the extra connection per node - updated = False - uri = self._node_uri_refresh(node) - - bad_node = (uri is None) - updated = self._node_refresh_bus_info(node, bad_node) - return updated - - def _node_uri_refresh(self, node): - current_nodes = { - namespace + ('' if namespace.endswith('/') else '/') + name - for name, namespace in self._node.get_node_names_and_namespaces()} - if node not in current_nodes: - qWarning('Node "{}" does not exist'.format(node)) - return None - return node - - def bad_update(self): - """ - Write me. - - Update loop for nodes with bad connectivity. We box them separately - so that we can maintain the good performance of the normal update loop. - Once a node is on the bad list it stays there. - """ - last_node_refresh = self.last_node_refresh - - # nodes left to check - try: - self.bad_nodes_lock.acquire() - # make copy due to multithreading - update_queue = self.bad_nodes.values()[:] - finally: - self.bad_nodes_lock.release() - - # return value. True if new data differs from old - updated = False - # number of nodes we checked - num_nodes = 0 - - start_time = time.time() - while update_queue: - # figure out the next node to contact - next = update_queue.pop() - # rate limit talking to any particular node - if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): - updated = self._node_refresh(next.name, True) or updated - # include in random offset (max 1/5th normal update interval) - # to help spread out updates - last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) - num_nodes += 1 - - # small yield to keep from torquing the processor - time.sleep(0.01) - end_time = time.time() - qDebug("ROS stats (bad nodes) update took %ss" % (end_time - start_time)) - return updated - - def update(self): - """ - Write me. - - Update all the stats. This method may take awhile to complete as it will - communicate with all nodes + master. - """ - last_node_refresh = self.last_node_refresh - - # nodes left to check - update_queue = None - # True if there are still more stats to fetch this cycle - work_to_do = True - # return value. True if new data differs from old - updated = False - # number of nodes we checked - num_nodes = 0 - - start_time = time.time() - while work_to_do: - - # each time through the loop try to talk to either the master - # or a node. stop when we have talked to everybody. - - # get a new node list from the master - if time.time() > (self.last_graph_refresh + self.graph_stale): - updated = self._graph_refresh() - self.last_graph_refresh = time.time() - # contact the nodes for stats - else: - # initialize update_queue based on most current nodes list - if update_queue is None: - update_queue = list(self.nn_nodes) - # no nodes left to contact - elif not update_queue: - work_to_do = False - # contact next node - else: - # figure out the next node to contact - next = update_queue.pop() - # rate limit talking to any particular node - if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): - updated = self._node_refresh(next) or updated - # include in random offset (max 1/5th normal update interval) - # to help spread out updates - last_node_refresh[next] = \ - time.time() + (random.random() * self.node_stale / 5.0) - num_nodes += 1 - - # small yield to keep from torquing the processor - time.sleep(0.01) - end_time = time.time() - qDebug("ROS stats update took %ss" % (end_time - start_time)) - return updated diff --git a/src/analysis/topic_times.py b/src/analysis/topic_times.py deleted file mode 100755 index 818df908..00000000 --- a/src/analysis/topic_times.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/python3 - -import argparse - -from rclpy.serialization import deserialize_message -import rosbag2_py -from rosidl_runtime_py.utilities import get_message - - -def get_rosbag_options(path, serialization_format='cdr'): - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format) - - return storage_options, converter_options - - -def main(args): - bag_path = args.bag_path - topic = args.topic - - storage_options, converter_options = get_rosbag_options(bag_path) - - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} - - storage_filter = rosbag2_py.StorageFilter(topics=[topic]) - reader.set_filter(storage_filter) - - print("topic: {}".format(topic)) - cnt = 0 - - while reader.has_next() and cnt <= args.cnt: - (topic, data, t) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - cnt += 1 - - if args.types_only: - print(msg_type) - continue - - if not hasattr(msg, "header"): - print("{} does not have header".format(msg_type)) - print(msg) - continue - - print("{}.{}: frame_id: {} msg_type: {}".format( - msg.header.stamp.sec, - msg.header.stamp.nanosec, - msg.header.frame_id, - msg_type)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("bag_path") - parser.add_argument("topic") - parser.add_argument("--types-only", action="store_true") - parser.add_argument("--cnt", type=int, default=10) - args = parser.parse_args() - - main(args) diff --git a/src/analysis/topic_traversal.py b/src/analysis/topic_traversal.py deleted file mode 100755 index 73ee5104..00000000 --- a/src/analysis/topic_traversal.py +++ /dev/null @@ -1,100 +0,0 @@ -#!/usr/bin/env python3 -# traversal topic -# given path: topic1 -> node1 -> topic2 -> node2 -> topic3 -> node3 -> topic4 -# traverse topic2 to topic4, then "topic2 -> node2 -> topic3 -> node3 -> topic4" printed - -import argparse - -import rclpy -from rosgraph2_impl import Edge, Graph - -EXCLUDE_TOPICS = [ - "/parameter_events", - "/rosout", - "/clock", - ] - - -def main(args): - rclpy.init() - node = rclpy.create_node("topic_traversal_node") - graph = Graph(node) - graph.update() - - cnt = 0 - while cnt < 10 and len(graph.nt_edges.edges_by_end) <= len(EXCLUDE_TOPICS): - cnt += 1 - graph = Graph(node) - graph.update() - - if len(graph.nt_edges.edges_by_end) <= len(EXCLUDE_TOPICS): - print("not enough edges") - return - - print("found {} keys".format(len(graph.nt_edges.edges_by_end.keys()))) - for k in graph.nt_edges.edges_by_end.keys(): - print(" '{}'".format(k)) - - def find_key(name): - for key in graph.nt_edges.edges_by_end: - if name in key and 0 <= len(key) - len(name) <= 2: - # print("find_key: {} -> {}".format(name, key)) - return key - return None - - name_from = find_key(args.frm) - name_to = find_key(args.to) - - print("") - print("args: {} -> {}".format(args.frm, args.to)) - print("graph: {} -> {}".format(name_from, name_to)) - print("") - - seen = set() - path = [] - - def traverse(now): - # print("path: {}, traverse {}".format(path, now)) - if now is None: - return False - if now in seen: # basically, assume now is not in seen - return False - - seen.add(now) - path.append(now) - if now == name_from: - return True - - for nxt in graph.nt_edges.edges_by_end[now]: - if isinstance(nxt, Edge): - # print("nxt.start: {}".format(nxt.start)) - nxt = nxt.start - nxt = nxt.strip() - if nxt in EXCLUDE_TOPICS: - continue - # print("nxt: '{}'".format(nxt)) - nxt = find_key(nxt) - if nxt in seen: - continue - # print("nxt: {}".format(nxt)) - if traverse(nxt): - return True - path.pop() - - return False - - if traverse(name_to): - print("Found") - path = path[::-1] - # path = path[::2] - - for i in path: - print(" '{}'".format(i)) - - -if __name__ == "__main__": - parser = argparse.ArgumentParser() - parser.add_argument("frm") - parser.add_argument("to") - args = parser.parse_args() - main(args) diff --git a/src/autoware_common/CODE_OF_CONDUCT.md b/src/autoware_common/CODE_OF_CONDUCT.md deleted file mode 100644 index c493cad4..00000000 --- a/src/autoware_common/CODE_OF_CONDUCT.md +++ /dev/null @@ -1,132 +0,0 @@ -# Contributor Covenant Code of Conduct - -## Our Pledge - -We as members, contributors, and leaders pledge to make participation in our -community a harassment-free experience for everyone, regardless of age, body -size, visible or invisible disability, ethnicity, sex characteristics, gender -identity and expression, level of experience, education, socio-economic status, -nationality, personal appearance, race, caste, color, religion, or sexual -identity and orientation. - -We pledge to act and interact in ways that contribute to an open, welcoming, -diverse, inclusive, and healthy community. - -## Our Standards - -Examples of behavior that contributes to a positive environment for our -community include: - -- Demonstrating empathy and kindness toward other people -- Being respectful of differing opinions, viewpoints, and experiences -- Giving and gracefully accepting constructive feedback -- Accepting responsibility and apologizing to those affected by our mistakes, - and learning from the experience -- Focusing on what is best not just for us as individuals, but for the overall - community - -Examples of unacceptable behavior include: - -- The use of sexualized language or imagery, and sexual attention or advances of - any kind -- Trolling, insulting or derogatory comments, and personal or political attacks -- Public or private harassment -- Publishing others' private information, such as a physical or email address, - without their explicit permission -- Other conduct which could reasonably be considered inappropriate in a - professional setting - -## Enforcement Responsibilities - -Community leaders are responsible for clarifying and enforcing our standards of -acceptable behavior and will take appropriate and fair corrective action in -response to any behavior that they deem inappropriate, threatening, offensive, -or harmful. - -Community leaders have the right and responsibility to remove, edit, or reject -comments, commits, code, wiki edits, issues, and other contributions that are -not aligned to this Code of Conduct, and will communicate reasons for moderation -decisions when appropriate. - -## Scope - -This Code of Conduct applies within all community spaces, and also applies when -an individual is officially representing the community in public spaces. -Examples of representing our community include using an official e-mail address, -posting via an official social media account, or acting as an appointed -representative at an online or offline event. - -## Enforcement - -Instances of abusive, harassing, or otherwise unacceptable behavior may be -reported to the community leaders responsible for enforcement at -conduct@autoware.org. -All complaints will be reviewed and investigated promptly and fairly. - -All community leaders are obligated to respect the privacy and security of the -reporter of any incident. - -## Enforcement Guidelines - -Community leaders will follow these Community Impact Guidelines in determining -the consequences for any action they deem in violation of this Code of Conduct: - -### 1. Correction - -**Community Impact**: Use of inappropriate language or other behavior deemed -unprofessional or unwelcome in the community. - -**Consequence**: A private, written warning from community leaders, providing -clarity around the nature of the violation and an explanation of why the -behavior was inappropriate. A public apology may be requested. - -### 2. Warning - -**Community Impact**: A violation through a single incident or series of -actions. - -**Consequence**: A warning with consequences for continued behavior. No -interaction with the people involved, including unsolicited interaction with -those enforcing the Code of Conduct, for a specified period of time. This -includes avoiding interactions in community spaces as well as external channels -like social media. Violating these terms may lead to a temporary or permanent -ban. - -### 3. Temporary Ban - -**Community Impact**: A serious violation of community standards, including -sustained inappropriate behavior. - -**Consequence**: A temporary ban from any sort of interaction or public -communication with the community for a specified period of time. No public or -private interaction with the people involved, including unsolicited interaction -with those enforcing the Code of Conduct, is allowed during this period. -Violating these terms may lead to a permanent ban. - -### 4. Permanent Ban - -**Community Impact**: Demonstrating a pattern of violation of community -standards, including sustained inappropriate behavior, harassment of an -individual, or aggression toward or disparagement of classes of individuals. - -**Consequence**: A permanent ban from any sort of public interaction within the -community. - -## Attribution - -This Code of Conduct is adapted from the [Contributor Covenant][homepage], -version 2.1, available at -[https://www.contributor-covenant.org/version/2/1/code_of_conduct.html][v2.1]. - -Community Impact Guidelines were inspired by -[Mozilla's code of conduct enforcement ladder][mozilla coc]. - -For answers to common questions about this code of conduct, see the FAQ at -[https://www.contributor-covenant.org/faq][faq]. Translations are available at -[https://www.contributor-covenant.org/translations][translations]. - -[homepage]: https://www.contributor-covenant.org -[v2.1]: https://www.contributor-covenant.org/version/2/1/code_of_conduct.html -[mozilla coc]: https://github.com/mozilla/diversity -[faq]: https://www.contributor-covenant.org/faq -[translations]: https://www.contributor-covenant.org/translations diff --git a/src/autoware_common/CONTRIBUTING.md b/src/autoware_common/CONTRIBUTING.md deleted file mode 100644 index 22c7ee28..00000000 --- a/src/autoware_common/CONTRIBUTING.md +++ /dev/null @@ -1,3 +0,0 @@ -# Contributing - -See . diff --git a/src/autoware_common/CPPLINT.cfg b/src/autoware_common/CPPLINT.cfg deleted file mode 100644 index ba6bdf08..00000000 --- a/src/autoware_common/CPPLINT.cfg +++ /dev/null @@ -1,14 +0,0 @@ -# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120 -set noparent -linelength=100 -includeorder=standardcfirst -filter=-build/c++11 # we do allow C++11 -filter=-build/namespaces_literals # we allow using namespace for literals -filter=-runtime/references # we consider passing non-const references to be ok -filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions -filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space -filter=-whitespace/parens # we allow closing parenthesis to be on the next line -filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon -filter=-build/header_guard # we automatically fix the names of header guards using pre-commit -filter=-build/include_order # we use the custom include order -filter=-build/include_subdir # we allow the style of "foo.hpp" diff --git a/src/autoware_common/DISCLAIMER.md b/src/autoware_common/DISCLAIMER.md deleted file mode 100644 index 1b5a9bbe..00000000 --- a/src/autoware_common/DISCLAIMER.md +++ /dev/null @@ -1,46 +0,0 @@ -DISCLAIMER - -“Autoware” will be provided by The Autoware Foundation under the Apache License 2.0. -This “DISCLAIMER” will be applied to all users of Autoware (a “User” or “Users”) with -the Apache License 2.0 and Users shall hereby approve and acknowledge all the contents -specified in this disclaimer below and will be deemed to consent to this -disclaimer without any objection upon utilizing or downloading Autoware. - -Disclaimer and Waiver of Warranties - -1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, - EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) - including but not limited to any representation or warranty (i) of fitness or - suitability for a particular purpose contemplated by the Users, (ii) of the - expected functions, commercial value, accuracy, or usefulness of the Service, - (iii) that the use by the Users of the Service complies with the laws and - regulations applicable to the Users or any internal rules established by - industrial organizations, (iv) that the Service will be free of interruption or - defects, (v) of the non-infringement of any third party's right and (vi) the - accuracy of the content of the Services and the software itself. - -2. The Autoware Foundation shall not be liable for any damage incurred by the - User that are attributable to the Autoware Foundation for any reasons - whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR - INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. - -3. A User shall be entirely responsible for the content posted by the User and - its use of any content of the Service or the Website. If the User is held - responsible in a civil action such as a claim for damages or even in a criminal - case, the Autoware Foundation and member companies, governments and academic & - non-profit organizations and their directors, officers, employees and agents - (collectively, the “Indemnified Parties”) shall be completely discharged from - any rights or assertions the User may have against the Indemnified Parties, or - from any legal action, litigation or similar procedures. - -Indemnity - -A User shall indemnify and hold the Indemnified Parties harmless from any of -their damages, losses, liabilities, costs or expenses (including attorneys' -fees or criminal compensation), or any claims or demands made against the -Indemnified Parties by any third party, due to or arising out of, or in -connection with utilizing Autoware (including the representations and -warranties), the violation of applicable Product Liability Law of each country -(including criminal case) or violation of any applicable laws by the Users, or -the content posted by the User or its use of any content of the Service or the -Website. diff --git a/src/autoware_common/LICENSE b/src/autoware_common/LICENSE deleted file mode 100644 index 261eeb9e..00000000 --- a/src/autoware_common/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - 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. diff --git a/src/autoware_common/README.md b/src/autoware_common/README.md deleted file mode 100644 index efe11f4c..00000000 --- a/src/autoware_common/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# caret_common - -common utility for CARET repositories diff --git a/src/autoware_common/caret_lint_common/CMakeLists.txt b/src/autoware_common/caret_lint_common/CMakeLists.txt deleted file mode 100644 index b068de75..00000000 --- a/src/autoware_common/caret_lint_common/CMakeLists.txt +++ /dev/null @@ -1,11 +0,0 @@ -cmake_minimum_required(VERSION 3.14) - -project(caret_lint_common NONE) - -find_package(ament_cmake_core REQUIRED) -find_package(ament_cmake_export_dependencies REQUIRED) -find_package(ament_cmake_test REQUIRED) - -ament_package_xml() -ament_export_dependencies(${${PROJECT_NAME}_EXEC_DEPENDS}) -ament_package() diff --git a/src/autoware_common/caret_lint_common/README.md b/src/autoware_common/caret_lint_common/README.md deleted file mode 100644 index 1b906750..00000000 --- a/src/autoware_common/caret_lint_common/README.md +++ /dev/null @@ -1,44 +0,0 @@ -# caret_lint_common - -A custom version of [ament_lint_common](https://github.com/ament/ament_lint/tree/master/ament_lint_common) for [CARET](https://github.com/tier4/caret). - -## Usage - -Add dependencies of `ament_lint_auto` and `caret_lint_common` to your package as below. - -`package.xml`: - -```xml -ament_lint_auto -caret_lint_common -``` - -`CMakeLists.txt`: - -```cmake -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() -``` - -Then, the following linters will run during `colcon test`. - -- [ament_cmake_copyright](https://github.com/ament/ament_lint/blob/master/ament_cmake_copyright/doc/index.rst) -- [ament_cmake_cppcheck](https://github.com/ament/ament_lint/blob/master/ament_cmake_cppcheck/doc/index.rst) -- [ament_cmake_lint_cmake](https://github.com/ament/ament_lint/blob/master/ament_cmake_lint_cmake/doc/index.rst) -- [ament_cmake_xmllint](https://github.com/ament/ament_lint/blob/master/ament_cmake_xmllint/doc/index.rst) - -## Design - -The original `ament_lint_common` contains other formatters/linters like `ament_cmake_uncrustify`, `ament_cmake_cpplint` and `ament_cmake_flake8`. -However, we don't include them because it's more useful to run them with `pre-commit` as [MoveIt](https://github.com/ros-planning/moveit2) does. - -For example, the benefits are: - -- We can use any version of tools independent of ament's version. -- We can easily integrate into IDE. -- We can easily check all the files in the repository without writing `test_depend` in each package. -- We can run formatters/linters without building, which makes error detection faster. - -Ideally, we think other linters should be moved to `pre-commit` as well, so we'll try to support them in the future. diff --git a/src/autoware_common/caret_lint_common/package.xml b/src/autoware_common/caret_lint_common/package.xml deleted file mode 100644 index bf0cffb9..00000000 --- a/src/autoware_common/caret_lint_common/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - caret_lint_common - 0.1.0 - The list of commonly used linters in CARET - Takayuki Akamine - Apache License 2.0 - - ament_cmake_core - ament_cmake_export_dependencies - ament_cmake_test - - ament_cmake_core - ament_cmake_test - - ament_cmake_copyright - ament_cmake_cppcheck - ament_cmake_lint_cmake - ament_cmake_xmllint - - - ament_cmake - - diff --git a/src/autoware_common/setup.cfg b/src/autoware_common/setup.cfg deleted file mode 100644 index 5214751c..00000000 --- a/src/autoware_common/setup.cfg +++ /dev/null @@ -1,15 +0,0 @@ -[flake8] -# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini -extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 -import-order-style = pep8 -max-line-length = 100 -show-source = true -statistics = true - -[isort] -profile=black -line_length=100 -force_sort_within_sections=true -force_single_line=true -reverse_relative=true -known_third_party=launch diff --git a/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md b/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md deleted file mode 100644 index 1a13881f..00000000 --- a/src/autoware_msg/autoware_auto_msgs/DISCLAIMER.md +++ /dev/null @@ -1,48 +0,0 @@ -DISCLAIMER - -The open-source software for self-driving vehicles, Autoware.AI, Autoware.IO, -Autoware.Auto. (collectively, referred to as “Autoware”) will be provided by -The Autoware Foundation under the Apache 2.0 License. This “DISCLAIMER” will be -applied to all users of Autoware (a “User” or “Users”) with the Apache 2.0 -License and Users shall hereby approve and acknowledge all the contents -specified in this disclaimer below and will be deemed to consent to this -disclaimer without any objection upon utilizing or downloading Autoware. - -Disclaimer and Waiver of Warranties - -1. AUTOWARE FOUNDATION MAKES NO REPRESENTATION OR WARRANTY OF ANY KIND, - EXPRESS OR IMPLIED, WITH RESPECT TO PROVIDING AUTOWARE (the “Service”) - including but not limited to any representation or warranty (i) of fitness or - suitability for a particular purpose contemplated by the Users, (ii) of the - expected functions, commercial value, accuracy, or usefulness of the Service, - (iii) that the use by the Users of the Service complies with the laws and - regulations applicable to the Users or any internal rules established by - industrial organizations, (iv) that the Service will be free of interruption or - defects, (v) of the non-infringement of any third party's right and (vi) the - accuracy of the content of the Services and the software itself. - -2. The Autoware Foundation shall not be liable for any damage incurred by the - User that are attributable to the Autoware Foundation for any reasons - whatsoever. UNDER NO CIRCUMSTANCES SHALL THE AUTOWARE FOUNDATION BE LIABLE FOR - INCIDENTAL, INDIRECT, SPECIAL OR FUTURE DAMAGES OR LOSS OF PROFITS. - -3. A User shall be entirely responsible for the content posted by the User and - its use of any content of the Service or the Website. If the User is held - responsible in a civil action such as a claim for damages or even in a criminal - case, the Autoware Foundation and member companies, governments and academic & - non-profit organizations and their directors, officers, employees and agents - (collectively, the “Indemnified Parties”) shall be completely discharged from - any rights or assertions the User may have against the Indemnified Parties, or - from any legal action, litigation or similar procedures. - -Indemnity - -A User shall indemnify and hold the Indemnified Parties harmless from any of -their damages, losses, liabilities, costs or expenses (including attorneys' -fees or criminal compensation), or any claims or demands made against the -Indemnified Parties by any third party, due to or arising out of, or in -connection with utilizing Autoware (including the representations and -warranties), the violation of applicable Product Liability Law of each country -(including criminal case) or violation of any applicable laws by the Users, or -the content posted by the User or its use of any content of the Service or the -Website. diff --git a/src/autoware_msg/autoware_auto_msgs/LICENSE b/src/autoware_msg/autoware_auto_msgs/LICENSE deleted file mode 100644 index b0d61354..00000000 --- a/src/autoware_msg/autoware_auto_msgs/LICENSE +++ /dev/null @@ -1,201 +0,0 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "{}" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright 2018 ApexAI - - 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. diff --git a/src/autoware_msg/autoware_auto_msgs/README.md b/src/autoware_msg/autoware_auto_msgs/README.md deleted file mode 100644 index 35249982..00000000 --- a/src/autoware_msg/autoware_auto_msgs/README.md +++ /dev/null @@ -1,77 +0,0 @@ -This is a Tier4 forked version of [autoware_auto_msgs](https://gitlab.com/autowarefoundation/autoware.auto/autoware_auto_msgs) - -# autoware_auto_msgs - -Interfaces between core Autoware.Auto components. - -## Conventions - -### Comments - -Add a comment to describe the meaning of the field. - -### Default Value - -Prefer a meaningful default value. Otherwise, the field is uninitialized. - -### Optional parameters - -There is nothing like `std::optional` in IDL, unfortunately. To accomodate the common use case of a -fixed message where some variables are not always filled, add an additional boolean variable with a -prefix `has_` and a default value of `FALSE`. - -### Units - -If a quantity described by a field has an associated unit of measurement, the following rules apply to determine the field name: - -1. If the unit is as base or [derived SI unit](https://en.wikipedia.org/wiki/International_System_of_Units#Derived_units), do not add a suffix and assume the default from [REP-103](https://www.ros.org/reps/rep-0103.html). -1. If the unit is a multiple of a base or derived SI unit, apply a suffix according to the table below. -1. If the unit is composed of non-SI units, apply a suffix according to the table below. - -Only deviate from the SI units when absolutely necessary and with justification. - -| Quantity | Unit | Suffix | Notes | -| --------------- | --------------------------- | ------- | ------------------------------------------------------- | -| distance | meters | None | | -| | micrometers | `_um` | | -| | millimeters | `_mm` | | -| | kilometers | `_km` | | -| speed, velocity | meters / second | None | Use speed for scalar and velocity for vector quantities | -| | kilometers / hour | `_kmph` | | -| acceleration | meters / second2 | None | | -| radial velocity | radians / second | None | | -| time | second | None | | -| | microsecond | `_us` | | -| | nanosecond | `_ns` | | - -#### Examples - -The first alternative is recommended, the second discouraged: - -1. `float elapsed_time` vs `float_elapsed_time_s` -1. `float distance_travelled` vs `float distance_travelled_m` -1. `int32 time_step_ms` vs `int32 time_step` -1. `float speed_mps` vs `float speed` - -### Minimal Example - -```idl -struct Foo { - @verbatim (language="comment", text= - " A multiline" "\n" - " comment") - @default (value=0.0) - float bar_speed; - - @verbatim (language="comment", text= - " Another multiline" "\n" - " comment") - @default (value=FALSE) - boolean has_bar_speed; - - @verbatim (language="comment", text= - " Describe the time stamp") - @default (value=0) - int32 timestamp_ns; -}; -``` diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt deleted file mode 100644 index 6cc556a6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_control_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AckermannControlCommand.idl" - "msg/AckermannLateralCommand.idl" - "msg/HighLevelControlCommand.idl" - "msg/LongitudinalCommand.idl" - DEPENDENCIES - "builtin_interfaces" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl deleted file mode 100644 index 916b1305..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannControlCommand.idl +++ /dev/null @@ -1,16 +0,0 @@ -#include "autoware_auto_control_msgs/msg/AckermannLateralCommand.idl" -#include "autoware_auto_control_msgs/msg/LongitudinalCommand.idl" -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_control_msgs { - module msg { - @verbatim (language="comment", text= - " Lateral and longitudinal control message for Ackermann-style platforms") - struct AckermannControlCommand { - builtin_interfaces::msg::Time stamp; - - autoware_auto_control_msgs::msg::AckermannLateralCommand lateral; - autoware_auto_control_msgs::msg::LongitudinalCommand longitudinal; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl deleted file mode 100644 index e1df7fb0..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/AckermannLateralCommand.idl +++ /dev/null @@ -1,25 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_control_msgs { - module msg { - @verbatim (language="comment", text= - " Lateral control message for Ackermann-style platforms" "\n" - " Note regarding tires: If the platform has multiple steering tires, the commands" - " given here are for a virtual tire at the average lateral position of the steering tires.") - - struct AckermannLateralCommand { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Desired angle of the steering tire in radians left (positive)" - " or right (negative) of center (0.0)") - @default (value=0.0) - float steering_tire_angle; - - @verbatim (language="comment", text= - " Desired rate of change of the steering tire angle in radians per second") - @default (value=0.0) - float steering_tire_rotation_rate; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl deleted file mode 100644 index 4fb6fbe6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/HighLevelControlCommand.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_control_msgs { - module msg { - struct HighLevelControlCommand { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " should be negative when reversed") - @default (value=0.0) - float velocity_mps; - - @verbatim (language="comment", text= - " units of inverse meters") - float curvature; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl deleted file mode 100644 index 4549bf0b..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/msg/LongitudinalCommand.idl +++ /dev/null @@ -1,30 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_control_msgs { - module msg { - @verbatim (language="comment", text= - " Longitudinal control message for all vehicle types") - - struct LongitudinalCommand { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Desired platform speed in meters per second." - " A positive value indicates movement in the positive X direction of the vehicle " - " while a negative value indicates movement in the negative X direction of the vehicle.") - @default (value=0.0) - float speed; - - @verbatim (language="comment", text= - " Desired platform acceleration in meters per second squared." - " A positive value indicates acceleration while a negative value indicates deceleration.") - @default (value=0.0) - float acceleration; - - @verbatim (language="comment", text= - " Desired platform jerk in meters per second cubed") - @default (value=0.0) - float jerk; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml deleted file mode 100644 index cfddca34..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_control_msgs/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - autoware_auto_control_msgs - 1.0.0 - Interfaces between core Autoware.Auto control components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - builtin_interfaces - std_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl deleted file mode 100644 index dbf8b94f..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Complex32.idl +++ /dev/null @@ -1,20 +0,0 @@ -module autoware_auto_geometry_msgs { - module msg { - @verbatim (language="comment", text= - " Complex32.msg" "\n" - " Can be used to represent yaw angle for trajectories" "\n" - " To convert back to a yaw angle, see" "\n" - " https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles") - struct Complex32 { - @verbatim (language="comment", text= - " cos(yaw / 2)") - @default (value=1.0) - float real; - - @verbatim (language="comment", text= - " sin(yaw / 2)") - @default (value=0.0) - float imag; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl deleted file mode 100644 index 1535d57c..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/Quaternion32.idl +++ /dev/null @@ -1,19 +0,0 @@ -module autoware_auto_geometry_msgs { - module msg { - @verbatim (language="comment", text= - " Represents rotation, but with less precision than the message in common_interfaces") - struct Quaternion32 { - @default (value=0.0) - float x; - - @default (value=0.0) - float y; - - @default (value=0.0) - float z; - - @default (value=1.0) - float w; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl deleted file mode 100644 index 9c79bcdb..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_geometry_msgs/msg/RelativePositionWithCovarianceStamped.idl +++ /dev/null @@ -1,16 +0,0 @@ -#include "geometry_msgs/msg/Point.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_geometry_msgs { - module msg { - @verbatim (language="comment", text= - " This message is a generalized representation of a 3D pose without" - " orientation of the origin of one frame within a child frame.") - struct RelativePositionWithCovarianceStamped { - std_msgs::msg::Header header; - string child_frame_id; - geometry_msgs::msg::Point position; - double covariance[9]; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt deleted file mode 100644 index 6c060600..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_mapping_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/HADMapBin.idl" - "msg/HADMapSegment.idl" - "msg/MapPrimitive.idl" - "srv/HADMapService.idl" - DEPENDENCIES - "std_msgs" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl deleted file mode 100644 index 35594899..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapBin.idl +++ /dev/null @@ -1,32 +0,0 @@ -#include "std_msgs/msg/Header.idl" - -module autoware_auto_mapping_msgs { - module msg { - module HADMapBin_Constants { - const uint8 MAP_FORMAT_LANELET2 = 0; - }; - @verbatim(language = "comment", text = - "HADMap contents in binary blob format") - struct HADMapBin - { - std_msgs::msg::Header header; - @verbatim(language = "comment", text = - "HADMap format identifier, allows supporting multiple map formats") - uint8 map_format; - - @verbatim(language = "comment", text = - "Version of map format. Keep as empty string if unnecssary") - @default(value = "") - string format_version; - - @verbatim(language = "comment", text = - "Version of map. Keep as empty empty if unnecessary") - @default(value = "") - string map_version; - - @verbatim(language = "comment", text = - "Binary map data") - sequence < uint8 > data; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl deleted file mode 100644 index 52ed8208..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/HADMapSegment.idl +++ /dev/null @@ -1,12 +0,0 @@ -#include "autoware_auto_mapping_msgs/msg/MapPrimitive.idl" - -module autoware_auto_mapping_msgs { - module msg { - @verbatim (language="comment", text= - " A segment of an HADMap which contains one or more MapPrimitives.") - struct HADMapSegment { - sequence primitives; - int64 preferred_primitive_id; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl deleted file mode 100644 index 6dea38bc..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/msg/MapPrimitive.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "std_msgs/msg/Header.idl" - -module autoware_auto_mapping_msgs { - module msg { - @verbatim(language = "comment", text = - "Map primitive information") - struct MapPrimitive - { - int64 id; - - @verbatim(language = "comment", text = - "Type of primitive, such as lane, polygon, line.") - @default(value = "") - string primitive_type; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml deleted file mode 100644 index 7e763458..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - autoware_auto_mapping_msgs - 1.0.0 - Interfaces between core Autoware.Auto mapping components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - std_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl deleted file mode 100644 index 012cdac6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_mapping_msgs/srv/HADMapService.idl +++ /dev/null @@ -1,39 +0,0 @@ -#include "autoware_auto_mapping_msgs/msg/HADMapBin.idl" - -module autoware_auto_mapping_msgs { - module srv { - // enum type not working yet on ROS2 implementation of idl - // enum HADPrimitive - // { - // FullMap, - // AllPrimitives, - // DriveableGeometry, - // RegulatoryElements, - // StaticObjects - // }; - - module HADMapService_Request_Constants { - const uint8 FULL_MAP = 0; - const uint8 ALL_PRIMITIVES = 1; - const uint8 DRIVEABLE_GEOMETRY = 2; - const uint8 REGULATORY_ELEMENTS = 3; - const uint8 STATIC_OBJECTS = 4; - }; - - struct HADMapService_Request - { - sequence < uint8 > requested_primitives; - @verbatim(language = "comment", text = - "Geometric upper bound of map data requested") - sequence < double, 3 > geom_upper_bound; - @verbatim(language = "comment", text = - "Geometric upper bound of map data requested") - sequence < double, 3 > geom_lower_bound; - }; - struct HADMapService_Response - { - autoware_auto_mapping_msgs::msg::HADMapBin map; - int32 answer; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst deleted file mode 100644 index 7e4ae3ea..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CHANGELOG.rst +++ /dev/null @@ -1,57 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Changelog for package autoware_auto_msgs -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ - -1.0.0 (2021-01-05) ------------------- -* Merge branch 'BoundingBoxArray-design' into 'master' - Add design doc for bounding-box message - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!12 `_ -* Working around https://github.com/ros-tooling/libstatistics_collector/issues/51. -* Fixing constants in AutonomyModeChange. -* Making AutonomyModeChange return empty. -* Merge branch '657-autonomy-service' into 'master' - Adding AutonomyModeChange service definition. - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!11 `_ -* Adding velocity_mps to VehicleControlCommand. -* Merge branch '474-add-modify-trajectory-service' into 'master' - Adding ModifyTrajectory service definition. - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!10 `_ -* Merge branch 'modify-plan-trajectory-action' into 'master' - modify PlanTrajectory Action to return trajectory in Result - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!9 `_ -* Merge branch 'fix/plan_trajectory_action' into 'master' - fix include file and namespace of constants - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!8 `_ -* Merge branch 'add-actions-dependency' into 'master' - Cleaning up package.xml and adding action_msgs dependency. - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!7 `_ -* Merge branch 'add-plan-trajectory-action' into 'master' - add PlanTrajectory action - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!6 `_ -* Contributors: Frederik Beaujean, Joshua Whitley, mitsudome-r - -0.1.0 (2020-08-07) ------------------- -* Merge branch 'add-hadmap-msgs' into 'master' - had map msgs - respect idl constant naming conventions - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!5 `_ -* Merge branch 'add-hadmap-msgs' into 'master' - Add hadmap msgs - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!4 `_ -* Add hadmap msgs -* Merge branch '3-convert-messages-from-rosmsg-to-idl' into 'master' - Resolve "Convert messages from ROSmsg to IDL" - Closes `#3 `_ - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!2 `_ -* Resolve "Convert messages from ROSmsg to IDL" -* Contributors: Esteve Fernandez, Joshua Whitley, simon-t4 - -0.0.2 (2020-03-02) ------------------- -* Merge branch '1-import-autoware_auto_msgs-from-autoware-auto' into 'master' - Resolve "Import autoware_auto_msgs from Autoware.Auto" - Closes `#1 `_ - See merge request `autowarefoundation/autoware.auto/autoware_auto_msgs!1 `_ -* Initial import -* Contributors: Esteve Fernandez, Geoffrey Biggs diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt deleted file mode 100644 index df1cc27d..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/CMakeLists.txt +++ /dev/null @@ -1,16 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md deleted file mode 100644 index a6558634..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_internal_msgs-design.md +++ /dev/null @@ -1,65 +0,0 @@ -# autoware_auto_msgs internal message design - -[TOC] - -This document is intended to track the design and rationale of messages internal to individual -components/stacks. - -# Perception - -This supersection tracks messages relating to the perception components of the autonomous driving -stack. - -## Object Detection - -This section tracks internal messages needed for the implementation of object detection stacks -that result in the emission of the `BoundingBoxArray` type as a common object list representation. - -### Classical 3d object detection - -This section covers the internal messages needed by a classical 3d object detection stack, -consisting of ground filtering, downsampling, clustering, and finally hull formation. - -#### PointClusters - -``` -sensor_msgs/PointCloud2[] clusters -``` - -This message represents a set of point clusters as a result of object detection or clustering. -`PointCloud2` was used as a cluster can be conceived as a subset of a point cloud, and -`PointCloud2` is the standard representation for point clouds. - -## Tracking - -This section tracks messages internal to specific implementation of tracking stacks. - -# Localization - -This supersection tracks messages relating to the implementation of localization/mapping -components in the autonomous driving stack. - -# Planning - -This supersection tracks messages relating to the implementation of planning components in the -autonomous driving stack. - -## Global Planning - -This section tracks internal messages needed for the implementation of specific global planning -stacks. - -## Behavior Planning - -This section tracks internal messages needed for the implementation of specific behavior planning -stacks. - -## Motion Planning - -This section tracks internal messages needed for the implementation of specific motion planning -stacks. - -## Control - -This section tracks internal messages needed for the implementation of specific controller -stacks. diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md deleted file mode 100644 index 78f89f93..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/autoware_auto_msgs-design.md +++ /dev/null @@ -1,516 +0,0 @@ -# autoware_auto_msgs - -[TOC] - -This document contains the intended sources, recipients and rationale for each of the custom -message types. - -# Helper Types - -The following are helper types that are composed into messages that software components use. - -## Complex32 - -``` -float32 real -float32 imag -``` - -## BoundingBox - -``` -geometry_msgs/Point32 centroid -geometry_msgs/Point32 size -geometry_msgs/Quaternion orientation -geometry_msgs/Point32[4] corners -float32 value -uint32 label -``` - -See the [design document](bounding-box-design.md) for further details. - -## BoundingBoxArray - -``` -std_msgs/Header header -BoundingBox[256] boxes -uint32 size -uint32 CAPACITY=256 -``` - - - -## DiagnosticHeader - -``` -string name -builtin_interfaces/Time data_stamp -builtin_interfaces/Time computation_start -builtin_interfaces/Duration runtime -uint32 iterations -``` - -## TrajectoryPoint - -``` -builtin_interfaces/Duration time_from_start -float32 x -float32 y -Complex32 heading -float32 longitudinal_velocity_mps 0.0 -float32 lateral_velocity_mps 0.0 -float32 acceleration_mps2 -float32 heading_rate_rps -float32 front_wheel_angle_rad 0.0 -float32 rear_wheel_angle_rad 0.0 -``` - -And the heading field has the following form: - -A zero heading corresponds to the positive x direction in the given coordinate frame. - -**Default Values**: All trajectory point and trajectory fields should default to 0 - -**Default Values**: All complex numbers should default to the pair (1, 0), which corresponds to a -zero heading. - -**Extensions**: In the future, this message may include higher derivative information. - -**Rationale**: Trajectory points mirror, but are not the same as a standard JointTrajectory message. -This is because the standard message allows room for ambiguities whereas this message does not. - -**Rationale**: Higher derivative information (e.g. acceleration, heading rate) is available to -provide a more expressive language for controllers and trajectory planners. If the information -is not needed, a value of zero is provided, which is semantically consistent for controllers and -motion planners - -**Rationale**: Complex numbers are used to represent heading rather than an angle because it reduces -ambiguities between angles, and angle distances. In addition, it conveniently represents a -precomputed sine and cosine values, obviating the need for additional computations. - -For more details, see [Controller design](@ref controller-design) - -# Interface Messages - -## Control Diagnostic - -``` -DiagnosticHeader diag_header -bool new_trajectory -string trajectory_source -string pose_source -float32 lateral_error_m -float32 longitudinal_error_m -float32 velocity_error_mps -float32 acceleration_error_mps2 -float32 yaw_error_rad -float32 yaw_rate_error_rps -``` - -**Source**: Controller - -**Recipient(s)**: - -**Rationale**: Diagnostic information is helpful to detect if a fault occurred, or some incorrect -behavior leading up to a fault. It can also be used for infotainment applications. - -For more details, see [Controller design](@ref controller-design) - -## Trajectory - -``` -std_msgs/Header header -TrajectoryPoint[<=100] points -``` - -**Source**: Motion planner - -**Recipient(s)**: Controller - -**Rationale**: A bounded number of points is provided since a new trajectory should be provided -at a regular rate. Bounded data structures also imply more deterministic communication times -due to having a bounded size. - -For more details, see [Controller design](@ref controller-design) - -### RawControlCommand - -This message type is defined as follows: - -``` -builtin_interfaces/Time stamp -uint32 throttle -uint32 brake -int32 front_steer -int32 rear_steer -``` - -This type is intended to be used when direct control of the vehicle hardware is required. As such, -no assumptions about units are made. It is the responsibility of the system integrator to ensure -that the values used are appropriate for the vehicle platform. - -**Source**: Controller - -**Recipient(s)**: Vehicle Interface - -**Default Values**: The default value for all fields is 0 - -**Rationale**: A time stamp field is provided because a control command should be generated in -response to the input of a vehicle kinematic state. Retaining the time of the triggering data -should aid with diagnostics and traceability. - -**Rationale**: The rear wheel angle is also provided to enable support for rear drive vehicles, -such as forklifts. - -**Rationale**: A frame is not provided since it is implied that it is fixed to the vehicle frame. - -**Rationale**: Throttle and brake are separated as they are fundamentally controlled by two -different actuators - -**Rationale**: This is intended to be compatible with using the triggers of a gamepad as a first -pass for vehicle platform validation. - -**Rationale**: Integer values are used to be more compatible with discrete, or byte-level -representation that is more common with CAN interfaces. - -**Rationale**: Units are not provided since byte-level representations are generally platform- -specific. - -For more details see [vehicle interface design](). - -### HighLevelControlCommand - -This message type is defined as follows: - -``` -builtin_interfaces/Time stamp -# should be negative when reversed -float32 velocity_mps 0.0 -float32 curvature -``` - -This type is intended to be used as an interface when only simple control is required, and thus -abstracts away more direct control of the vehicle. - -**Source**: Controller - -**Recipient(s)**: Vehicle Interface - -**Default Values**: The default value for all fields is 0 - -**Rationale**: A time stamp field is provided because a control command should be generated in -response to the input of a vehicle kinematic state. Retaining the time of the triggering data -should aid with diagnostics and traceability. - -**Rationale**: A frame is not provided since it is implied that it is fixed to the vehicle frame. - -**Rationale**: Curvature is used as a control target as it does not require any vehicle-specific -information. This is inspired by the AutonomousStuff Low Level Controller - -**Rationale**: Velocity is used as a longitudinal control target to allow the vehicle to minimally -follow a geometric reference path. - -For more details see [vehicle interface design](). - -## Vehicle Control Command - -``` -builtin_interfaces/Time stamp -float32 long_accel_mps2 -float32 front_wheel_angle_rad -float32 rear_wheel_angle_rad -``` - -This message type is intended to provide a proxy for a foot on the accelerator and brake pedal, -and a pair of hands on the steering wheel, while also providing a convenient representation -for controller developers. - -These wheel angles are counter-clockwise positive, where 0 corresponds to a neutral, or -forward-facing orientation. - -**Source**: Controller - -**Recipient(s)**: Vehicle Interface - -**Default Values**: The default value for all fields is 0 - -**Rationale**: A time stamp field is provided because a control command should be generated in -response to the input of a vehicle kinematic state. Retaining the time of the triggering data -should aid with diagnostics and traceability. - -**Rationale**: A single acceleration field is provided because controllers typically plan a -longitudinal acceleration and turning angle for the rigid body. Braking is baked into this single -command for simplicity of controller development, and because it is generally not expected for both -the brake to be depressed and the accelerator at the same time. - -**Rationale**: Wheel steering angles are provided since it allows the controller to only require a -simplified view of the vehicle platform, such as knowing the wheelbase length. Depending on the -implementation of a drive-by-wire interface, producing a given wheel angle may require knowledge -of the mapping between steering wheel angle to wheel angle. - -**Rationale**: The rear wheel angle is also provided to enable support for rear drive vehicles, -such as forklifts. - -**Rational**: A frame is not provided since it is implied that it is fixed to the vehicle frame. - -For more details, see [Controller design](@ref controller-design) - -## Vehicle Kinematic State - -**Source**: Vehicle interface - -**Recipient(s)**: Behavior Planner - -``` -std_msgs/Header header -TrajectoryPoint state -geometry_msgs/Transform delta -``` - -The `TrajectoryPoint::time_from_start` field should also be filled with the time difference between -the current state and the previous state message. - -The position fields of the `TrajectoryPoint` member should contain the position of the ego vehicle -with respect to a fixed frame, if available. If the frame is a dynamic frame (e.g. `base_link`), -then this position field should be taken as unavailable or invalid. The further kinematics of this -member should always be available, and populated according to the appropriate coordinate frame, -making the additional assumption that the coordinate frame is static. - -The delta member is the transform of the message should represent the positional update of the -ego vehicle's coordinate frame relative to it's last position and orientation. - -**Source**: Vehicle State Estimator - -**Recipient(s)**: Planning stack (Route planner, Behavior Planner, Motion Planner, Controller), -Tracker - -**Default Values**: The trajectory point should be appropriately default initialized - -**Default Values**: The delta transform field should have zero translation and the unit quaternion -by default. - -**Extensions**: State and transform uncertainty may be added as variances or covariance matrices. -This may be necessary for probabilistic algorithms (e.g. robust MPC) - -**Rationale**: An underlying trajectory point is used because kinematic information may be needed -by controllers. Further, representing the vehicle state in a manner that mirrors the trajectory -fits with the semantic purpose of a controller: matching the current state to a state sequence - -**Rationale**: A transform relative to the previous position and orientation of the ego vehicle is -provided because while absolute position may not always be available with accuracy, this relative -transform should be available with some degree of accuracy (e.g. via IMU information, odometry -information, etc.). The availability of this information allows for planning and tracking within a -local coordinate frame. For example, in a level 3 highway driving use case, a trajectory may be -rooted at the ego vehicle's position at some time stamp. The accumulation of these relative -transforms would allow for a controller to properly follow the trajectory. - -**Rationale**: The kinematics are populated with a fixed frame assumption to simplify the updates -and usage of this field. - -For more details, see [Controller design](@ref controller-design) - -## Vehicle Odometry - -``` -builtin_interfaces/Time stamp -float32 velocity_mps -float32 front_wheel_angle_rad -float32 rear_wheel_angle_rad -``` - -This message reports the kinematic state of the vehicle as the vehicle itself reports. The intended -use case for this message could be in motion planning for initial conditions, or dead reckoning. - -**Source**: Vehicle interface - -**Recipient(s)**: Vehicle State Estimator - -**Default Values**: The default value for this message should be 0 in all fields. - -**Rationale**: This message is separate from the vehicle state report since they are typically for -distinct use cases (e.g. behavior planning vs dead reckoning) - -**Rationale**: A vehicle is expected to have encoders which provide velocity, and steering angle. -Acceleration and other fields may not be available on all vehicle platforms. - -**Rationale**: A stamp is expected here because this is timely information. In this sense, the -vehicle interface is acting as a sensor driver. - -**Rationale**: A frame id is not provided because these are assumed to be fixed to the vehicle -frame, and heading information is not available. - -## Vehicle State Command - -``` -builtin_interfaces/Time stamp -uint8 blinker -uint8 headlight -uint8 wiper -uint8 gear -uint8 mode -bool hand_brake -bool horn -bool autonomous - -### Definitions -# Blinker -uint8 BLINKER_NO_COMMAND = 0; -uint8 BLINKER_OFF = 1; -uint8 BLINKER_LEFT = 2; -uint8 BLINKER_RIGHT = 3; -uint8 BLINKER_HAZARD = 4; -# Headlight -uint8 HEADLIGHT_NO_COMMAND = 0; -uint8 HEADLIGHT_OFF = 1; -uint8 HEADLIGHT_ON = 2; -uint8 HEADLIGHT_HIGH = 3; -# Wiper -uint8 WIPER_NO_COMMAND = 0; -uint8 WIPER_OFF = 1; -uint8 WIPER_LOW = 2; -uint8 WIPER_HIGH = 3; -uint8 WIPER_CLEAN = 4; -# Gear -uint8 GEAR_NO_COMMAND = 0 -uint8 GEAR_DRIVE = 1 -uint8 GEAR_REVERSE = 2 -uint8 GEAR_PARK = 3 -uint8 GEAR_LOW = 4 -uint8 GEAR_NEUTRAL = 5 -# Mode -uint8 MODE_NO_COMMAND = 0; -uint8 MODE_AUTONOMOUS = 1; -uint8 MODE_MANUAL = 2; -``` - -This message is intended to control the remainder of the vehicle state, e.g. those not required for -minimal collision-free driving. - -**Source**: Behavior Planner - -**Recipient(s)**: Vehicle interface - -**Default Values**: The default value for all fields should be zero. - -**Rationale**: Hazard lights superseded a left/right signal, and as such are an exclusive state - -**Rationale**: Cleaning might be necessary to ensure adequate operation of sensors mounted behind -the wind shield - -**Rationale**: A horn might be required to signal to other drivers - -**Rationale**: Autonomous is a flag because this is a command message: true requests a transition -to autonomous, false requests a disengagement to manual. - -**Rationale**: While additional states are possible for other fields (e.g. headlights, wipers, -gears, etc.), this message only prescribes the minimal set of states that most or all vehicles -can satisfy. - -**Rationale**: Since commands here represent a state transition, a command of NONE denotion "no -state transition" is also valid - -## Vehicle State Report - -``` -builtin_interfaces/Time stamp -uint8 fuel # 0 to 100 -uint8 blinker -uint8 headlight -uint8 wiper -uint8 gear -uint8 mode -bool hand_brake -bool horn - -### Definitions -# Blinker -uint8 BLINKER_OFF = 1 -uint8 BLINKER_LEFT = 2 -uint8 BLINKER_RIGHT = 3 -uint8 BLINKER_HAZARD = 4 -# Headlight -uint8 HEADLIGHT_OFF = 1 -uint8 HEADLIGHT_ON = 2 -uint8 HEADLIGHT_HIGH = 3 -# Wiper -uint8 WIPER_OFF = 1 -uint8 WIPER_LOW = 2 -uint8 WIPER_HIGH = 3 -uint8 WIPER_CLEAN = 4 -# Gear -uint8 GEAR_DRIVE = 1 -uint8 GEAR_REVERSE = 2 -uint8 GEAR_PARK = 3 -uint8 GEAR_LOW = 4 -uint8 GEAR_NEUTRAL = 5 -# Autonomous -uint8 MODE_AUTONOMOUS = 1 -uint8 MODE_MANUAL = 2 -uint8 MODE_DISENGAGED = 3 -uint8 MODE_NOT_READY = 4 -``` - -**Source**: Vehicle interface - -**Recipient(s)**: Behavior Planner - -**Default Value**: N/A. All fields should be populatable by the vehicle interface. - -**Rationale**: A discrete fuel range is provided as finer granularity is likely unnecessary. - -**Rationale**: A simple state machine is provided to ensure that disambiguate between intentionally -manual, and two modes of unintentionally being in manual mode: due to preconditions not being met, -or due to some failure of the autonomous driving stack. - -**Rationale**: Constants are kept the same as VehicleStateCommand to prevent subtle bugs from being -introduced. The constant 0 is reserved for NO_COMMAND in VehicleStateCommand. - -# References - -- AutowareAuto#62 - -## Related Message Types - -### ROS - -- [JointTrajectory.msg](https://github.com/ros2/common_interfaces/blob/master/trajectory_msgs/msg/JointTrajectoryPoint.msg) -- [PointStamped](http://docs.ros.org/lunar/api/geometry_msgs/html/msg/PointStamped.html) -- [PoseStamped](http://docs.ros.org/lunar/api/geometry_msgs/html/msg/PoseStamped.html) -- [TransformStamped](http://docs.ros.org/melodic/api/geometry_msgs/html/msg/TransformStamped.html) -- [NavSatFix](http://docs.ros.org/melodic/api/sensor_msgs/html/msg/NavSatFix.html) - -### Autoware.AI - -- [AccelCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/AccelCmd.msg) -- [BrakeCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/BrakeCmd.msg) -- [ControlCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/ControlCommand.msg) -- [IndicatorCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/IndicatorCmd.msg) -- [LampCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/LampCmd.msg) -- [RemoteCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/RemoteCmd.msg) -- [SteerCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/SteerCmd.msg) -- [VehicleCmd](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/VehicleCmd.msg) -- [VehicleStatus](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/VehicleStatus.msg) -- [Waypoint](https://github.com/CPFL/Autoware/blob/master/ros/src/msgs/autoware_msgs/msg/Waypoint.msg) - -### Apollo - -Controller/Interface Messages - -- [VehicleSignal](https://github.com/ApolloAuto/apollo/blob/fb12723bd6dcba88ecccb6123ea850da1e050171/modules/common/proto/vehicle_signal.proto) -- [Lexus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/canbus/proto/lexus.proto) -- [DriveState](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/common/proto/drive_state.proto) -- [VehicleState](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/common/vehicle_state/proto/vehicle_state.proto) -- [ControlCmd](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/control_cmd.proto) - -Configuration Messages - -- [LonControllerConf](https://github.com/ApolloAuto/apollo/blob/3d6f86e21a3c3ac43cf08423d4c3f92bc63ecac9/modules/control/proto/lon_controller_conf.proto) -- [LatControllerConf](https://github.com/ApolloAuto/apollo/blob/e9156ced04a8f0e372c781d803fd43428ab6c497/modules/control/proto/lat_controller_conf.proto) -- [ControlConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/control_conf.proto) -- [MPCControllerConf](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/control/proto/mpc_controller_conf.proto) - -Algorithm Status Messages - -- [PlanningStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_status.proto) -- [LocalizationStatus](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/localization/proto/localization_status.proto) -- [PlanningStats](https://github.com/ApolloAuto/apollo/blob/51651b9105e55c14e65cc2bd349b479e55fffa36/modules/planning/proto/planning_stats.proto) diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md deleted file mode 100644 index d109ae67..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/design/bounding-box-design.md +++ /dev/null @@ -1,211 +0,0 @@ -# BoundingBox message design {#bounding-box-design} - -# Motivation - -In order to ensure a modular system, it's assumed that object detectors are architecturally -delineated. In other words, software components which can produce instantaneous observations of -objects are delineated from other components in the autonomous driving stack. - -This assumption or decomposition allows us to interchangeably use various forms of object detectors -with various forms of multi-object tracking (by assignment) algorithms. If a tighter coupling -between object detection and tracking is required, specialized combined stacks can be developed. - -In order to facilitate the isolation of object detectors as a module, common messages types are -defined as an interface or output of the object detectors to other components. - -# Use cases - -The instantaneous detection of objects primarily has three use cases: - -1. Collision detection (as a part of planning) -2. Tracking (which may eventually be an input to planning) -3. Region of Interest identification (as a preprocessing step to localization/mapping) - -In addition, there is the implied use case that this message representation is used on a safety- -critical system. - -# Requirements - -## Safety-critical systems - -Safety-critical systems require real-time and static memory applications. As such, this implies -that the representation of the message must be upper bounded in size. - -For the purpose of performance, an optional requirement is added to message definitions such that -the maximum size is no more than `64kB`, which is also the maximum size of a single UDP packet. This -requirement ensures that latency is kept to a minimum if interprocess communication is necessary at -the interface of the object detection algorithms. - -## Collision detection - -In order to detect collisions, the representation must have: - -1. A position at least in 2-space (with the assumption that the object is on the same plane as the - ego) -2. A bounding volume representation which fully contains the detected object -3. A way to ensure the object representation is in a consistent coordinate frame with respect to the - ego -4. Optionally uncertainty representation of the above parameters - -## Tracking - -The purpose of tracking is to maintain a consistent identity of a detected object over time. -In addition, tracking may also produce state estimates over time, such as kinematic estimates, -shape estimates, or label estimates. - -To satisfy the fundamental requirement of tracking, the message representation must provide features -that can be used for assignment, or association of current observations with previous observations. -Features that can satisfy this requirement include: - -1. Position information -2. Shape information (e.g. convex hull, bounding ellipse/box, shape features, etc.) -3. Object information (e.g. color/intensity distribution, classification label, etc.) -4. Other sensor information (e.g. velocity, heading, turn rate (via FMCW sensors)) -5. Optionally uncertainty information for the above parameters - -Classically, position information is the most important association feature, assuming a continuous -world and a relatively high sampling frequency. - -The other features can be used to motivate various state estimates in the tracking stack. - -## Region of interest detection - -An object detector can also be used to identify regions of interest for various other stacks, such -as feature-based localization/mapping, or scene understanding (sign/scenario detection). - -The localization/mapping use case is largely similar to the tracking use case insofar as it requires -assignment or association of features. In contrast, scene understanding requires an identification -of the relevant objects and their position in a space that is compatible with the ego frame. - -As such, the features needed to satisfy this use case are similar to that of the tracking use case: - -1. Position information -2. Shape/object information -3. Optionally uncertainty information - -# Message definition - -Based on the requirements laid out, some assumptions and simplifications can be made, and a message -type can be defined. - -If any of the assumptions and simplifications must be broken for the proper operation of the -algorithm, then the choice of algorithm should be reconsidered, or a parallel construct which -encapsulates multiple software components should be made. - -In the case when a stack cannot populate certain fields of the interface, the field should be in -an identifiably non-normal state, such as zero or NaN. - -## Safety critical requirements - -To satisfy the minimal latency requirement, the representation must be bounded in size to `64kB`. - -If it's additionally assumed that `1kB` is used for secondary representation and there are -piggy-backed submessages in the communication sublayer, the maximum representation size is -`63kB`. - -Next, it's assumed that some maximum number of objects that are detected instantaneously. Based on -current observations of Apex.AI urban driving use cases, typically 50-70 objects are observed per -scene. Applying a healthy safety factor gives a maximum object bound of 256 objects per frame. - -These two assumptions combined lead us to have a maximum representation size of `250B`. - -## Position requirements - -Minimally, a 3D position is used to satisfy the position requirement inherent in all use cases. A 3D -position is used to be more general, and apply to other stacks, such as a vision/camera-based stack. - -To satisfy the compatibility of representational spaces, it's assumed that all observations are made -in a common frame. This frame can then be represented by a string in the aggregate object type. -It is assumed that it is the responsibility of the consuming algorithm to have and transform the -position space into the correct coordinate frame given this information. - -## Shape requirements - -Next, the shape of the object is represented as a bounding box in 3 interdependent ways: - -- A quaternion for orientation -- A 3D size parameter -- Four 3D positions for the corners of the bounding box - -A bounding box representation was used because many objects in the autonomous driving use case -are approximately box-shaped. In addition, many efficient algorithms exist in both the 2D and 3D -case to compute bounding boxes. Finally, bounding boxes are convenient, close-formed representations -of objects, as opposed to a convex hull which can be potentially unbounded in representation size. - -A quaternion is used to represent orientation. An oriented bounding box is used as opposed to -axis-aligned bounding boxes because it can better represent and fit to objects. A quaternion -is used to represent orientation in the SO(3) space because it is continuously varying without -singularities. In addition, it can be used to efficiently calculate sine and cosine values. - -Finally, the size and corners of the bounding box is represented. While the two -together represent redundant information, both parameters are byproducts of the bounding box -computation process, and communicating both forms of information downstream can reduce the -computational burden and repeated work of algorithms downstream. - -For example, size can be used as a coarse collision-checking step, whereas the corners are used -for fine-grained collision detection. Similarly, the size of the bounding may be treated as direct -extent observations in the tracking case, whereas the corners may be treated as proxy observations -of the centroid. - -## Object/Other requirements - -Of all the object features proposed, only a classification label is bounded in representation size -(assuming a finite set of classes). While the other features could be bounded, they would be -severely limited in their representational capabilities due to the size limitations. - -As such, only a classification label is provided with the current form: - -`vehicle_label` is an 8-bit integer depicting the classification label of the bounding box. It -can take any of the following default values: - -``` -NO_LABEL=0 -CAR=1 -PEDESTRIAN=2 -CYCLIST=3 -MOTORCYCLE=4 -``` - -The `signal_label` field contains the back signal state of the car and can take any of the following -default values: - -``` -NO_SIGNAL=0 -LEFT_SIGNAL=1 -RIGHT_SIGNAL=2 -BRAKE=3 -``` - -Next, additional kinematic fields can potentially be populated at minimal size cost. These include -velocity, heading, and heading rate, assuming a FMCW/doppler-effect sensor is available. The -inclusion of such fields can greatly improve the performance of tracking, or in some cases remove -the necessity of tracking (i.e. predictive collision detection). - -## Uncertainty requirements - -While uncertainty representation is not a strict requirement, the usage of uncertainty can greatly -improve the performance of downstream algorithms, both from the perspective of accuracy, and safety. - -Representing the full probabilistic state of the object is difficult due to size constraints, -as representing an 8-state covariance matrix would overrun the capacity, and -similarly representing the full classification distribution is potentially unbounded in size. - -As a compromise, several modeling simplifications and assumptions can be made for the bonus -representation of uncertainty. - -First, it can be assumed that the uncertainty of state variables are uncorrelated, implying all -off-diagonal elements of the full covariance matrix are zero, and thus need not be represented. -Second, the coarse modeling assumption of a uniformly distributed class assignment residual can -be used. As a result, class uncertainty can be represented with a single value. - -# Future extensions - -The currently proposed representation for bounding boxes takes up approximately `150B` of the total -size budget of `230B`. This leaves room for up to 18 floating point values. These could be used to -cover the other optional requirements as a vector of floating points, with an optional ID denoting -the intended interpretation. - -# Related issues - -- #3540: Update bounding box, add use cases -- #4069: Revise new articles for the 0.12.0 release diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml deleted file mode 100644 index 4a80c67c..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_msgs/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - autoware_auto_msgs - 1.0.0 - Interfaces between core Autoware.Auto components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - autoware_auto_control_msgs - autoware_auto_geometry_msgs - autoware_auto_mapping_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_system_msgs - autoware_auto_vehicle_msgs - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt deleted file mode 100644 index 2582b6e6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/CMakeLists.txt +++ /dev/null @@ -1,52 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_perception_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/BoundingBox.idl" - "msg/BoundingBoxArray.idl" - "msg/ClassifiedRoi.idl" - "msg/ClassifiedRoiArray.idl" - "msg/DetectedObject.idl" - "msg/DetectedObjectKinematics.idl" - "msg/DetectedObjects.idl" - "msg/LookingTrafficSignal.idl" - "msg/ObjectClassification.idl" - "msg/PointClusters.idl" - "msg/PointXYZIF.idl" - "msg/PredictedObject.idl" - "msg/PredictedObjectKinematics.idl" - "msg/PredictedObjects.idl" - "msg/PredictedPath.idl" - "msg/Shape.idl" - "msg/TrackedObject.idl" - "msg/TrackedObjectKinematics.idl" - "msg/TrackedObjects.idl" - "msg/TrafficLight.idl" - "msg/TrafficLightRoi.idl" - "msg/TrafficLightRoiArray.idl" - "msg/TrafficSignal.idl" - "msg/TrafficSignalArray.idl" - "msg/TrafficSignalStamped.idl" - "msg/TrafficSignalWithJudge.idl" - DEPENDENCIES - "autoware_auto_geometry_msgs" - "geometry_msgs" - "sensor_msgs" - "std_msgs" - "unique_identifier_msgs" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl deleted file mode 100644 index 0dfd11c6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBox.idl +++ /dev/null @@ -1,71 +0,0 @@ -#include "autoware_auto_geometry_msgs/msg/Quaternion32.idl" -#include "geometry_msgs/msg/Point32.idl" - -module autoware_auto_perception_msgs { - module msg { - typedef geometry_msgs::msg::Point32 geometry_msgs__msg__Point32; - typedef geometry_msgs__msg__Point32 geometry_msgs__msg__Point32__4[4]; - typedef float float__8[8]; - module BoundingBox_Constants { - const uint8 NO_LABEL = 0; - const uint8 CAR = 1; - const uint8 PEDESTRIAN = 2; - const uint8 CYCLIST = 3; - const uint8 MOTORCYCLE = 4; - const uint8 NO_SIGNAL = 0; - const uint8 LEFT_SIGNAL = 1; - const uint8 RIGHT_SIGNAL = 2; - const uint8 BRAKE = 3; - const uint32 POSE_X = 0; - const uint32 POSE_Y = 1; - const uint32 VELOCITY = 2; - const uint32 HEADING = 3; - const uint32 TURN_RATE = 4; - const uint32 SIZE_X = 5; - const uint32 SIZE_Y = 6; - const uint32 ACCELERATION = 7; - }; - @verbatim (language="comment", text= - " Oriented bounding box representation") - struct BoundingBox { - geometry_msgs::msg::Point32 centroid; - - geometry_msgs::msg::Point32 size; - - autoware_auto_geometry_msgs::msg::Quaternion32 orientation; - - @default (value=0.0) - float velocity; - - @default (value=0.0) - float heading; - - @default (value=0.0) - float heading_rate; - - geometry_msgs__msg__Point32__4 corners; - - float__8 variance; - - @verbatim (language="comment", text= - " can hold arbitrary value, e.g. likelihood, area, perimeter") - float value; - - @verbatim (language="comment", text= - " can hold one of the vehicle constants defined below" "\n" - " NO_LABEL as default value") - @default (value=0) - uint8 vehicle_label; - - @verbatim (language="comment", text= - " can hold one of the signal constants defined below" "\n" - " NO_SIGNAL as default value") - @default (value=0) - uint8 signal_label; - - @verbatim (language="comment", text= - " Likelihood of vehicle label") - float class_likelihood; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl deleted file mode 100644 index cedcaf30..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/BoundingBoxArray.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/BoundingBox.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - module BoundingBoxArray_Constants { - const uint32 CAPACITY = 256; - }; - @verbatim (language="comment", text= - " Message for a full set of bounding boxes") - struct BoundingBoxArray { - std_msgs::msg::Header header; - - sequence boxes; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl deleted file mode 100644 index 88ff9d24..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoi.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "geometry_msgs/msg/Polygon.idl" -#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text="A region of interest in an image with class information.") - struct ClassifiedRoi { - @verbatim (language="comment", text="A vector of possible classifications of this object.") - sequence classifications; - - @verbatim (language="comment", text="A 2D polygon describing the outline of an object in image coordinates.") - geometry_msgs::msg::Polygon polygon; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl deleted file mode 100644 index 1b58dd77..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ClassifiedRoiArray.idl +++ /dev/null @@ -1,14 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/ClassifiedRoi.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " Message for a full set of classified ROIs") - struct ClassifiedRoiArray { - std_msgs::msg::Header header; - - sequence rois; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl deleted file mode 100644 index 151e2c65..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObject.idl +++ /dev/null @@ -1,16 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl" -#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" -#include "autoware_auto_perception_msgs/msg/Shape.idl" - -module autoware_auto_perception_msgs { - module msg { - struct DetectedObject { - @range (min=0.0, max=1.0) - float existence_probability; - - sequence classification; - autoware_auto_perception_msgs::msg::DetectedObjectKinematics kinematics; - autoware_auto_perception_msgs::msg::Shape shape; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl deleted file mode 100644 index 216cb667..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjectKinematics.idl +++ /dev/null @@ -1,41 +0,0 @@ -#include "geometry_msgs/msg/Point.idl" -#include "geometry_msgs/msg/Quaternion.idl" -#include "geometry_msgs/msg/TwistWithCovariance.idl" - -module autoware_auto_perception_msgs { - module msg { - module DetectedObjectKinematics_Constants { - /** - * Only position is available, orientation is empty. Note that the shape can be an oriented - * bounding box but the direction the object is facing is unknown, in which case - * orientation should be empty. - */ - const uint8 UNAVAILABLE = 0; - /** - * The orientation is determined only up to a sign flip. For instance, assume that cars are - * longer than they are wide, and the perception pipeline can accurately estimate the - * dimensions of a car. It should set the orientation to coincide with the major axis, with - * the sign chosen arbitrarily, and use this tag to signify that the orientation could - * point to the front or the back. - */ - const uint8 SIGN_UNKNOWN = 1; - /** - * The full orientation is available. Use e.g. for machine-learning models that can - * differentiate between the front and back of a vehicle. - */ - const uint8 AVAILABLE = 2; - }; - - struct DetectedObjectKinematics { - geometry_msgs::msg::PoseWithCovariance pose_with_covariance; - - boolean has_position_covariance; - uint8 orientation_availability; - - geometry_msgs::msg::TwistWithCovariance twist_with_covariance; - - boolean has_twist; - boolean has_twist_covariance; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl deleted file mode 100644 index c257bb4d..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/DetectedObjects.idl +++ /dev/null @@ -1,13 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/DetectedObject.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " This is the output of object detection and the input to tracking.") - struct DetectedObjects { - std_msgs::msg::Header header; - sequence objects; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl deleted file mode 100644 index ed82595a..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/LookingTrafficSignal.idl +++ /dev/null @@ -1,14 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - struct LookingTrafficSignal { - std_msgs::msg::Header header; - boolean is_module_running; - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge perception; - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge external; - autoware_auto_perception_msgs::msg::TrafficSignalWithJudge result; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl deleted file mode 100644 index 905c13e3..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/ObjectClassification.idl +++ /dev/null @@ -1,24 +0,0 @@ -module autoware_auto_perception_msgs { - module msg { - module ObjectClassification_Constants { - const uint8 UNKNOWN = 0; - const uint8 CAR = 1; - const uint8 TRUCK = 2; - const uint8 BUS = 3; - const uint8 TRAILER = 4; - const uint8 MOTORCYCLE = 5; - const uint8 BICYCLE = 6; - const uint8 PEDESTRIAN = 7; - }; - - struct ObjectClassification { - @verbatim (language="comment", text= - " Valid values for the label field are provided in" - " ObjectClassification_Constants.") - uint8 label; - - @range (min=0.0, max=1.0) - float probability; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl deleted file mode 100644 index aad5cce1..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointClusters.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/PointXYZIF.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " Represent the clusters" "\n" - " The cluster 0 is from point 0 to point cluster_boundary[0] - 1." "\n" - " Cluster i would be from point cluster_boundary[i - 1] to cluster_boundary[i] - 1, and so on") - struct PointClusters { - std_msgs::msg::Header header; - - sequence points; - - sequence cluster_boundary; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl deleted file mode 100644 index 14429ec7..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PointXYZIF.idl +++ /dev/null @@ -1,20 +0,0 @@ -module autoware_auto_perception_msgs { - module msg { - module PointXYZIF_Constants { - const uint16 END_OF_SCAN_ID = 65535; - }; - @verbatim (language="comment", text= - " This message is meant to mirror autoware::common::types::PointXYZIF") - struct PointXYZIF { - float x; - - float y; - - float z; - - float intensity; - - uint16 id; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl deleted file mode 100644 index ed611086..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObject.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" -#include "autoware_auto_perception_msgs/msg/Shape.idl" -#include "autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl" -#include "unique_identifier_msgs/msg/UUID.idl" - -module autoware_auto_perception_msgs { - module msg { - struct PredictedObject { - unique_identifier_msgs::msg::UUID object_id; - - @range (min=0.0, max=1.0) - float existence_probability; - - sequence classification; - autoware_auto_perception_msgs::msg::PredictedObjectKinematics kinematics; - - autoware_auto_perception_msgs::msg::Shape shape; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl deleted file mode 100644 index 1cfed5e7..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjectKinematics.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/PredictedPath.idl" -#include "geometry_msgs/msg/AccelWithCovariance.idl" -#include "geometry_msgs/msg/PoseWithCovariance.idl" -#include "geometry_msgs/msg/TwistWithCovariance.idl" - -module autoware_auto_perception_msgs { - module msg { - struct PredictedObjectKinematics { - geometry_msgs::msg::PoseWithCovariance initial_pose_with_covariance; - - geometry_msgs::msg::TwistWithCovariance initial_twist_with_covariance; - - geometry_msgs::msg::AccelWithCovariance initial_acceleration_with_covariance; - - sequence predicted_paths; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl deleted file mode 100644 index f3d2ad66..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedObjects.idl +++ /dev/null @@ -1,13 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/PredictedObject.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " This is the output of object prediction.") - struct PredictedObjects { - std_msgs::msg::Header header; - sequence objects; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl deleted file mode 100644 index 628cf6f0..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/PredictedPath.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "builtin_interfaces/msg/Duration.idl" -#include "geometry_msgs/msg/Pose.idl" - -module autoware_auto_perception_msgs { - module msg { - struct PredictedPath { - sequence path; - - @verbatim (language="comment", text= - " The time_step field defines the interval between consecutive pose predictions in the path array.") - builtin_interfaces::msg::Duration time_step; - float confidence; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl deleted file mode 100644 index 94e88696..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/Shape.idl +++ /dev/null @@ -1,27 +0,0 @@ -#include "geometry_msgs/msg/Polygon.idl" - -module autoware_auto_perception_msgs { - module msg { - module Shape_Constants { - const uint8 BOUNDING_BOX=0; - const uint8 CYLINDER=1; - const uint8 POLYGON=2; - }; - - struct Shape { - @verbatim (language="comment", text= - " Type of the shape") - uint8 type; - - @verbatim (language="comment", text= - " The contour of the shape (POLYGON)") - geometry_msgs::msg::Polygon footprint; - - @verbatim (language="comment", text= - " x: the length of the object (BOUNDING_BOX) or diameter (CYLINDER)" - " y: the width of the object (BOUNDING_BOX)" - " z: the overall height of the object") - geometry_msgs::msg::Vector3 dimensions; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl deleted file mode 100644 index b61cfe1c..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObject.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/ObjectClassification.idl" -#include "autoware_auto_perception_msgs/msg/Shape.idl" -#include "autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl" -#include "unique_identifier_msgs/msg/UUID.idl" - -module autoware_auto_perception_msgs { - module msg { - struct TrackedObject { - unique_identifier_msgs::msg::UUID object_id; - - @range (min=0.0, max=1.0) - float existence_probability; - - sequence classification; - autoware_auto_perception_msgs::msg::TrackedObjectKinematics kinematics; - - autoware_auto_perception_msgs::msg::Shape shape; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl deleted file mode 100644 index 115d7c06..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjectKinematics.idl +++ /dev/null @@ -1,45 +0,0 @@ -#include "geometry_msgs/msg/AccelWithCovariance.idl" -#include "geometry_msgs/msg/Point.idl" -#include "geometry_msgs/msg/Quaternion.idl" -#include "geometry_msgs/msg/TwistWithCovariance.idl" - -module autoware_auto_perception_msgs { - module msg { - module TrackedObjectKinematics_Constants { - /** - * Only position is available, orientation is empty. Note that the shape can be an oriented - * bounding box but the direction the object is facing is unknown, in which case - * orientation should be empty. - */ - const uint8 UNAVAILABLE = 0; - /** - * The orientation is determined only up to a sign flip. For instance, assume that cars are - * longer than they are wide, and the perception pipeline can accurately estimate the - * dimensions of a car. It should set the orientation to coincide with the major axis, with - * the sign chosen arbitrarily, and use this tag to signify that the orientation could - * point to the front or the back. - */ - const uint8 SIGN_UNKNOWN = 1; - /** - * The full orientation is available. Use e.g. for machine-learning models that can - * differentiate between the front and back of a vehicle. - */ - const uint8 AVAILABLE = 2; - }; - - struct TrackedObjectKinematics { - @verbatim (language="comment", text= - " Pose covariance is always provided by tracking.") - geometry_msgs::msg::PoseWithCovariance pose_with_covariance; - - uint8 orientation_availability; - - geometry_msgs::msg::TwistWithCovariance twist_with_covariance; - - geometry_msgs::msg::AccelWithCovariance acceleration_with_covariance; - - @value (default=False) - boolean is_stationary; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl deleted file mode 100644 index 4be6c44b..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrackedObjects.idl +++ /dev/null @@ -1,13 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrackedObject.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " This is the output of object tracking and the input to prediction.") - struct TrackedObjects { - std_msgs::msg::Header header; - sequence objects; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl deleted file mode 100644 index 844e3aef..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLight.idl +++ /dev/null @@ -1,42 +0,0 @@ -module autoware_auto_perception_msgs { - module msg { - module TrafficLight_Constants { - // constants for color - const uint8 RED = 1; - const uint8 AMBER = 2; - const uint8 GREEN = 3; - const uint8 WHITE = 4; - - // constants for shape - const uint8 CIRCLE = 5; - const uint8 LEFT_ARROW = 6; - const uint8 RIGHT_ARROW = 7; - const uint8 UP_ARROW = 8; - const uint8 DOWN_ARROW = 9; - const uint8 DOWN_LEFT_ARROW = 10; - const uint8 DOWN_RIGHT_ARROW = 11; - const uint8 CROSS = 12; - - // constants for status - const uint8 SOLID_OFF = 13; - const uint8 SOLID_ON = 14; - const uint8 FLASHING = 15; - - // constants for common use - const uint8 UNKNOWN = 16; - }; - struct TrafficLight { - @default (value=0) - uint8 color; - - @default (value=0) - uint8 shape; - - @default (value=0) - uint8 status; - - @default (value=0.0) - float confidence; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl deleted file mode 100644 index 10976cb4..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoi.idl +++ /dev/null @@ -1,10 +0,0 @@ -#include "sensor_msgs/msg/RegionOfInterest.idl" - -module autoware_auto_perception_msgs { - module msg { - struct TrafficLightRoi { - sensor_msgs::msg::RegionOfInterest roi; - int32 id; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl deleted file mode 100644 index 094f485d..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficLightRoiArray.idl +++ /dev/null @@ -1,11 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficLightRoi.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - struct TrafficLightRoiArray { - std_msgs::msg::Header header; - sequence rois; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl deleted file mode 100644 index ef17c012..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignal.idl +++ /dev/null @@ -1,19 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficLight.idl" - -module autoware_auto_perception_msgs { - module msg { - @verbatim (language="comment", text= - " A TrafficSignal is defined here as a group of multiple TrafficLights" "\n" - " which each represent a single light, indicator, or bulb.") - struct TrafficSignal { - @verbatim (language="comment", text= - " A value of 0 indicates an invalid map_primitive_id. Signals which are not" - " associated with map primitives should not be used in planning because this" - " indicates that the required signal-to-lane mapping is not available.") - @default (value=0) - int32 map_primitive_id; - - sequence lights; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl deleted file mode 100644 index 22de5791..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalArray.idl +++ /dev/null @@ -1,11 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - struct TrafficSignalArray { - std_msgs::msg::Header header; - sequence signals; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl deleted file mode 100644 index 31a47674..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalStamped.idl +++ /dev/null @@ -1,11 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_perception_msgs { - module msg { - struct TrafficSignalStamped { - std_msgs::msg::Header header; - autoware_auto_perception_msgs::msg::TrafficSignal signal; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl deleted file mode 100644 index 45c61c25..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/msg/TrafficSignalWithJudge.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "autoware_auto_perception_msgs/msg/TrafficSignal.idl" - -module autoware_auto_perception_msgs { - module msg { - module TrafficSignalWithJudge_Constants { - const uint8 JUDGE = 1; - const uint8 NONE = 2; - const uint8 STOP = 3; - const uint8 GO = 4; - }; - struct TrafficSignalWithJudge { - uint8 judge; - boolean has_state; - autoware_auto_perception_msgs::msg::TrafficSignal signal; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml deleted file mode 100644 index 16460341..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_perception_msgs/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - autoware_auto_perception_msgs - 1.0.0 - Interfaces between core Autoware.Auto perception components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - autoware_auto_geometry_msgs - geometry_msgs - sensor_msgs - std_msgs - unique_identifier_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt deleted file mode 100644 index 41006d16..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/CMakeLists.txt +++ /dev/null @@ -1,41 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_planning_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "action/PlannerCostmap.idl" - "action/PlanTrajectory.idl" - "action/RecordTrajectory.idl" - "action/ReplayTrajectory.idl" - "msg/HADMapRoute.idl" - "msg/OrderMovement.idl" - "msg/Route.idl" - "msg/Trajectory.idl" - "msg/TrajectoryPoint.idl" - "msg/Path.idl" - "msg/PathPoint.idl" - "msg/PathWithLaneId.idl" - "msg/PathPointWithLaneId.idl" - "srv/ModifyTrajectory.idl" - DEPENDENCIES - "autoware_auto_geometry_msgs" - "autoware_auto_mapping_msgs" - "builtin_interfaces" - "geometry_msgs" - "nav_msgs" - "std_msgs" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl deleted file mode 100644 index a59c73f2..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlanTrajectory.idl +++ /dev/null @@ -1,27 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/HADMapRoute.idl" -#include "autoware_auto_planning_msgs/msg/Trajectory.idl" - -module autoware_auto_planning_msgs { - module action { - module PlanTrajectory_Result_Constants { - const uint8 SUCCESS = 0; - const uint8 FAIL = 1; - }; - struct PlanTrajectory_Goal { - autoware_auto_planning_msgs::msg::HADMapRoute sub_route; - }; - - struct PlanTrajectory_Result { - @verbatim(language = "comment", text = - "Report of end condition. Value should be one of PlanTrajectory_Constants") - uint8 result; - autoware_auto_planning_msgs::msg::Trajectory trajectory; - }; - - struct PlanTrajectory_Feedback { - @verbatim(language = "comment", text = - "Currently we don't need feedback, but we need some variable to compile") - uint8 unused_variable; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl deleted file mode 100644 index 2fd8dca8..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/PlannerCostmap.idl +++ /dev/null @@ -1,26 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/HADMapRoute.idl" -#include "nav_msgs/msg/OccupancyGrid.idl" -#include "std_msgs/msg/Empty.idl" - -module autoware_auto_planning_msgs { - module action { - struct PlannerCostmap_Goal - { - @verbatim(language = "comment", text = - "Route defined by start and goal point and map primitives" - "between given points.") - autoware_auto_planning_msgs::msg::HADMapRoute route; - }; - struct PlannerCostmap_Result - { - @verbatim(language = "comment", text = - "Costmap with obstacles and lanelets position applied") - nav_msgs::msg::OccupancyGrid costmap; - }; - struct PlannerCostmap_Feedback { - @verbatim(language = "comment", text = - "Currently there is no feedback, but variable is needed to compile") - std_msgs::msg::Empty unused_variable; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl deleted file mode 100644 index 2697cdb6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/RecordTrajectory.idl +++ /dev/null @@ -1,19 +0,0 @@ -module autoware_auto_planning_msgs { - module action { - struct RecordTrajectory_Goal { - @verbatim(language = "comment", text = - "A path to the CSV file which will be created to save the recorded trajectory.") - string record_path; - }; - - struct RecordTrajectory_Result { - @verbatim(language = "comment", text = - "This action has no result but we must provide something here.") - boolean unused_flag; - }; - - struct RecordTrajectory_Feedback { - int32 current_length; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl deleted file mode 100644 index ab74fddf..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/action/ReplayTrajectory.idl +++ /dev/null @@ -1,19 +0,0 @@ -module autoware_auto_planning_msgs { - module action { - struct ReplayTrajectory_Goal { - @verbatim(language = "comment", text = - "A path to the CSV file which contains the recorded trajectory.") - string replay_path; - }; - - struct ReplayTrajectory_Result { - @verbatim(language = "comment", text = - "This action has no result but we must provide something here.") - boolean unused_flag; - }; - - struct ReplayTrajectory_Feedback { - int32 remaining_length; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl deleted file mode 100644 index fb49654c..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/HADMapRoute.idl +++ /dev/null @@ -1,27 +0,0 @@ -#include "autoware_auto_mapping_msgs/msg/HADMapSegment.idl" -#include "geometry_msgs/msg/Pose.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language="comment", text= - " A route within a high-definition map defined by" - " the start and goal points and map primitives" - " describing the route between the two.") - struct HADMapRoute { - std_msgs::msg::Header header; - - @verbatim (language="comment", text= - " The start_pose must exist within the bounds of the primitives in the first" - " segment defined in the route_segments array.") - geometry_msgs::msg::Pose start_pose; - - @verbatim (language="comment", text= - " The goal_pose must exist within the bounds of the primitives in the last" - " segment defined in the route_semgents array.") - geometry_msgs::msg::Pose goal_pose; - - sequence segments; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl deleted file mode 100644 index 6d3ea20b..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/OrderMovement.idl +++ /dev/null @@ -1,21 +0,0 @@ -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - module OrderMovement_Constants { - const uint8 NOTSET = 0; - const uint8 STOP = 1; - const uint8 GO = 2; - const uint8 SLOWDOWN = 3; - }; - - @verbatim (language="comment", text= - "Movement order for planner to follow") - struct OrderMovement { - std_msgs::msg::Header header; - - @default (value=0) - uint8 order_movement; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl deleted file mode 100644 index f4808810..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Path.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/PathPoint.idl" -#include "nav_msgs/msg/OccupancyGrid.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language = "comment", text= - "Contains a PathPoint path and an OccupancyGrid of drivable_area.") - struct Path { - std_msgs::msg::Header header; - sequence points; - nav_msgs::msg::OccupancyGrid drivable_area; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl deleted file mode 100644 index 1ce0ea06..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPoint.idl +++ /dev/null @@ -1,26 +0,0 @@ -#include "geometry_msgs/msg/Pose.idl" -#include "geometry_msgs/msg/Twist.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language = "comment", text= - "Represents a pose from a lanelet map, contains twist information.") - struct PathPoint { - geometry_msgs::msg::Pose pose; - - @default (value=0.0) - float longitudinal_velocity_mps; - - @default (value=0.0) - float lateral_velocity_mps; - - @default (value=0.0) - float heading_rate_rps; - - @verbatim(language = "comment", text = - "Denotes that the point is final, doesn't need further updates.") - @default (value = FALSE) - boolean is_final; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl deleted file mode 100644 index 500c3e73..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/PathPoint.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language = "comment", text= - "Contains a PathPoint and lanelet lane_id information.") - struct PathPointWithLaneId { - autoware_auto_planning_msgs::msg::PathPoint point; - - @verbatim(language = "comment", text = - "Lanelet lane_id information.") - sequence lane_ids; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl deleted file mode 100644 index d87dd61a..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/PathWithLaneId.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/PathPointWithLaneId.idl" -#include "nav_msgs/msg/OccupancyGrid.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language = "comment", text= - "Contains a PathPointWithLaneId path and an OccupancyGrid of drivable_area.") - struct PathWithLaneId { - std_msgs::msg::Header header; - sequence points; - nav_msgs::msg::OccupancyGrid drivable_area; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl deleted file mode 100644 index 76e0fa3f..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Route.idl +++ /dev/null @@ -1,22 +0,0 @@ -#include "autoware_auto_mapping_msgs/msg/MapPrimitive.idl" -#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - module Route_Constants { - const uint32 CAPACITY = 100; - }; - @verbatim (language="comment", text= - "Global route information for the planner") - struct Route { - std_msgs::msg::Header header; - - autoware_auto_planning_msgs::msg::TrajectoryPoint start_point; - - autoware_auto_planning_msgs::msg::TrajectoryPoint goal_point; - - sequence primitives; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl deleted file mode 100644 index 4f9381c4..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/Trajectory.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_planning_msgs { - module msg { - module Trajectory_Constants { - const uint32 CAPACITY = 10000; - }; - @verbatim (language="comment", text= - " A set of trajectory points for the controller") - struct Trajectory { - std_msgs::msg::Header header; - - sequence points; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl deleted file mode 100644 index d07bdf90..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/msg/TrajectoryPoint.idl +++ /dev/null @@ -1,31 +0,0 @@ -#include "builtin_interfaces/msg/Duration.idl" - -module autoware_auto_planning_msgs { - module msg { - @verbatim (language="comment", text= - " Representation of a trajectory point for the controller") - struct TrajectoryPoint { - builtin_interfaces::msg::Duration time_from_start; - - geometry_msgs::msg::Pose pose; - - @default (value=0.0) - float longitudinal_velocity_mps; - - @default (value=0.0) - float lateral_velocity_mps; - - @default (value=0.0) - float acceleration_mps2; - - @default (value=0.0) - float heading_rate_rps; - - @default (value=0.0) - float front_wheel_angle_rad; - - @default (value=0.0) - float rear_wheel_angle_rad; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml deleted file mode 100644 index 3ba50da3..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - autoware_auto_planning_msgs - 1.0.0 - Interfaces between core Autoware.Auto planning components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - autoware_auto_geometry_msgs - autoware_auto_mapping_msgs - builtin_interfaces - geometry_msgs - nav_msgs - std_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl deleted file mode 100644 index 23689287..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_planning_msgs/srv/ModifyTrajectory.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/Trajectory.idl" - -module autoware_auto_planning_msgs { - module srv { - struct ModifyTrajectory_Request - { - @verbatim(language = "comment", text = - "Trajectory to be modified") - autoware_auto_planning_msgs::msg::Trajectory original_trajectory; - }; - struct ModifyTrajectory_Response - { - @verbatim(language = "comment", text = - "Trajectory after modification") - autoware_auto_planning_msgs::msg::Trajectory modified_trajectory; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt deleted file mode 100644 index fb334ae8..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/CMakeLists.txt +++ /dev/null @@ -1,32 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_system_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/AutowareState.idl" - "msg/ControlDiagnostic.idl" - "msg/DiagnosticHeader.idl" - "msg/DrivingCapability.idl" - "msg/EmergencyState.idl" - "msg/Float32MultiArrayDiagnostic.idl" - "msg/HazardStatus.idl" - "msg/HazardStatusStamped.idl" - DEPENDENCIES - "builtin_interfaces" - "diagnostic_msgs" - "std_msgs" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl deleted file mode 100644 index 7e173dd5..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/AutowareState.idl +++ /dev/null @@ -1,25 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_system_msgs { - module msg { - module AutowareState_Constants { - const uint8 INITIALIZING = 1; - const uint8 WAITING_FOR_ROUTE = 2; - const uint8 PLANNING = 3; - const uint8 WAITING_FOR_ENGAGE = 4; - const uint8 DRIVING = 5; - const uint8 ARRIVED_GOAL = 6; - const uint8 FINALIZING = 7; - }; - - @verbatim (language="comment", text= - " A message for reporting the Autoware system status.") - struct AutowareState { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Current state of the Autoware system.") - uint8 state; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl deleted file mode 100644 index 524d9ad0..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/ControlDiagnostic.idl +++ /dev/null @@ -1,33 +0,0 @@ -#include "autoware_auto_system_msgs/msg/DiagnosticHeader.idl" - -module autoware_auto_system_msgs { - module msg { - @verbatim (language="comment", text= - " Diagnostic information for the controller") - struct ControlDiagnostic { - autoware_auto_system_msgs::msg::DiagnosticHeader diag_header; - - @verbatim (language="comment", text= - " Controller specific information") - boolean new_trajectory; - - string<256> trajectory_source; - - string<256> pose_source; - - @verbatim (language="comment", text= - " the error between the current vehicle and the nearest neighbor point") - float lateral_error_m; - - float longitudinal_error_m; - - float velocity_error_mps; - - float acceleration_error_mps2; - - float yaw_error_rad; - - float yaw_rate_error_rps; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl deleted file mode 100644 index 27686038..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DiagnosticHeader.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "builtin_interfaces/msg/Duration.idl" -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_system_msgs { - module msg { - @verbatim (language="comment", text= - " Base information that all diagnostic messages should have") - struct DiagnosticHeader { - string<256> name; - - builtin_interfaces::msg::Time data_stamp; - - builtin_interfaces::msg::Time computation_start; - - builtin_interfaces::msg::Duration runtime; - - uint32 iterations; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl deleted file mode 100644 index 4210cc54..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/DrivingCapability.idl +++ /dev/null @@ -1,21 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" -#include "autoware_auto_system_msgs/msg/HazardStatus.idl" - -module autoware_auto_system_msgs { - module msg { - - @verbatim (language="comment", text= - " A status message for reporting the vehicle driving capabilities.") - struct DrivingCapability { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Status for the autonomous driving mode.") - autoware_auto_system_msgs::msg::HazardStatus autonomous_driving; - - @verbatim (language="comment", text= - " Status for the remote control mode.") - autoware_auto_system_msgs::msg::HazardStatus remote_control; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl deleted file mode 100644 index 33ab1432..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/EmergencyState.idl +++ /dev/null @@ -1,29 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_system_msgs { - module msg { - - module EmergencyState_Constants { - const uint8 NORMAL = 1; - const uint8 OVERRIDE_REQUESTING = 2; - const uint8 MRM_OPERATING = 3; - const uint8 MRM_SUCCEEDED = 4; - const uint8 MRM_FAILED = 5; - }; - - @verbatim (language="comment", text= - " Message for reporting the emergency state.") - struct EmergencyState { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Current emergency state fo the system. Possible states are as follows." - " - NORMAL - the system is not in emergency mode." - " - OVERRIDE_REQUESTING - the override is requesting." - " - MRM_OPERATING - during the minimal risk maneuver (MRM)" - " - MRM_SUCCEEDED - MRM operation succeeded." - " - MRM_FAILED - MRM operation failed.") - uint8 state; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl deleted file mode 100644 index ec9b0b9d..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/Float32MultiArrayDiagnostic.idl +++ /dev/null @@ -1,16 +0,0 @@ -#include "autoware_auto_system_msgs/msg/DiagnosticHeader.idl" -#include "std_msgs/msg/Float32MultiArray.idl" - -module autoware_auto_system_msgs { - module msg { - @verbatim (language="comment", text= - " Generic diagnostic information to be used for debugging purposes") - struct Float32MultiArrayDiagnostic { - autoware_auto_system_msgs::msg::DiagnosticHeader diag_header; - - @verbatim (language="comment", text= - " Debug information as a Float32MultiArray") - std_msgs::msg::Float32MultiArray diag_array; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl deleted file mode 100644 index 5a397979..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatus.idl +++ /dev/null @@ -1,48 +0,0 @@ -#include "diagnostic_msgs/msg/DiagnosticStatus.idl" - -module autoware_auto_system_msgs { - module msg { - module HazardStatus_Constants { - const uint8 NO_FAULT = 0; - const uint8 SAFE_FAULT = 1; - const uint8 LATENT_FAULT = 2; - const uint8 SINGLE_POINT_FAULT = 3; - }; - - @verbatim (language="comment", text= - " A message for reporting the hazard status.") - struct HazardStatus { - - @verbatim (language="comment", text= - " Determines the hazard level.") - @default (value=0) - uint8 level; - - @verbatim (language="comment", text= - " Determines whether the vehicle is in the emergency state.") - @default (value=FALSE) - boolean emergency; - - @verbatim (language="comment", text= - " Determines whether the vehicle emergency state should be held.") - @default (value=FALSE) - boolean emergency_holding; - - @verbatim (language="comment", text= - " Diagnostics categorized as no fault.") - sequence diag_no_fault; - - @verbatim (language="comment", text= - " Diagnostics categorized as safe fault.") - sequence diag_safe_fault; - - @verbatim (language="comment", text= - " Diagnostics categorized as latent fault.") - sequence diag_latent_fault; - - @verbatim (language="comment", text= - " Diagnostics categorized as single point fault.") - sequence diag_single_point_fault; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl deleted file mode 100644 index ce09cb4d..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/msg/HazardStatusStamped.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" -#include "autoware_auto_system_msgs/msg/HazardStatus.idl" - -module autoware_auto_system_msgs { - module msg { - - @verbatim (language="comment", text= - " A message for reporting the hazard status with timestamp.") - struct HazardStatusStamped { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Contains the hazard status with diagnostics information.") - autoware_auto_system_msgs::msg::HazardStatus status; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml deleted file mode 100644 index 4759d007..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_system_msgs/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_auto_system_msgs - 1.0.0 - Interfaces between core Autoware.Auto system components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - builtin_interfaces - diagnostic_msgs - std_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt deleted file mode 100644 index cece6a66..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/CMakeLists.txt +++ /dev/null @@ -1,53 +0,0 @@ -# All rights reserved. -cmake_minimum_required(VERSION 3.5) - -### Export headers -project(autoware_auto_vehicle_msgs) - -# Generate messages -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -rosidl_generate_interfaces(${PROJECT_NAME} - "msg/ControlModeCommand.idl" - "msg/ControlModeReport.idl" - "msg/Engage.idl" - "msg/GearCommand.idl" - "msg/GearReport.idl" - "msg/HandBrakeCommand.idl" - "msg/HandBrakeReport.idl" - "msg/HazardLightsCommand.idl" - "msg/HazardLightsReport.idl" - "msg/HeadlightsCommand.idl" - "msg/HeadlightsReport.idl" - "msg/HornCommand.idl" - "msg/HornReport.idl" - "msg/RawControlCommand.idl" - "msg/StationaryLockingCommand.idl" - "msg/SteeringReport.idl" - "msg/TurnIndicatorsCommand.idl" - "msg/TurnIndicatorsReport.idl" - "msg/VehicleControlCommand.idl" - "msg/VehicleKinematicState.idl" - "msg/VehicleOdometry.idl" - "msg/VehicleStateCommand.idl" - "msg/VehicleStateReport.idl" - "msg/VelocityReport.idl" - "msg/WheelEncoder.idl" - "msg/WipersCommand.idl" - "msg/WipersReport.idl" - "srv/AutonomyModeChange.idl" - "srv/ControlModeCommand.srv" - DEPENDENCIES - "autoware_auto_planning_msgs" - "builtin_interfaces" - "std_msgs" - ADD_LINTER_TESTS -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package() diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl deleted file mode 100644 index c365de69..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeCommand.idl +++ /dev/null @@ -1,19 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module ControlModeCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 AUTONOMOUS = 1; - const uint8 MANUAL = 2; - }; - @verbatim (language="comment", text= - " ControlModeCommand.msg") - struct ControlModeCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 mode; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl deleted file mode 100644 index 3ab71f50..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/ControlModeReport.idl +++ /dev/null @@ -1,24 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module ControlModeReport_Constants { - const uint8 NO_COMMAND = 0; - const uint8 AUTONOMOUS = 1; - const uint8 AUTONOMOUS_STEER_ONLY = 2; - const uint8 AUTONOMOUS_VELOCITY_ONLY = 3; - const uint8 MANUAL = 4; - const uint8 DISENGAGED = 5; - const uint8 NOT_READY = 6; - }; - @verbatim (language="comment", text= - " ControlModeReport.msg") - - struct ControlModeReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 mode; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl deleted file mode 100644 index 57ee2109..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/Engage.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - - @verbatim (language="comment", text= - " Command for controlling the engagement state of the vehicle.") - struct Engage { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Determines if a vehicle should be engaged.") - @default (value=FALSE) - boolean engage; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl deleted file mode 100644 index a0f606a4..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearCommand.idl +++ /dev/null @@ -1,40 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module GearCommand_Constants { - const uint8 NONE = 0; - const uint8 NEUTRAL = 1; - const uint8 DRIVE = 2; - const uint8 DRIVE_2 = 3; - const uint8 DRIVE_3 = 4; - const uint8 DRIVE_4 = 5; - const uint8 DRIVE_5 = 6; - const uint8 DRIVE_6 = 7; - const uint8 DRIVE_7 = 8; - const uint8 DRIVE_8 = 9; - const uint8 DRIVE_9 = 10; - const uint8 DRIVE_10 = 11; - const uint8 DRIVE_11 = 12; - const uint8 DRIVE_12 = 13; - const uint8 DRIVE_13 = 14; - const uint8 DRIVE_14 = 15; - const uint8 DRIVE_15 = 16; - const uint8 DRIVE_16 = 17; - const uint8 DRIVE_17 = 18; - const uint8 DRIVE_18 = 19; - const uint8 REVERSE = 20; - const uint8 REVERSE_2 = 21; - const uint8 PARK = 22; - const uint8 LOW = 23; - const uint8 LOW_2 = 24; - }; - - struct GearCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 command; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl deleted file mode 100644 index d82c4de4..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/GearReport.idl +++ /dev/null @@ -1,40 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module GearReport_Constants { - const uint8 NONE = 0; - const uint8 NEUTRAL = 1; - const uint8 DRIVE = 2; - const uint8 DRIVE_2 = 3; - const uint8 DRIVE_3 = 4; - const uint8 DRIVE_4 = 5; - const uint8 DRIVE_5 = 6; - const uint8 DRIVE_6 = 7; - const uint8 DRIVE_7 = 8; - const uint8 DRIVE_8 = 9; - const uint8 DRIVE_9 = 10; - const uint8 DRIVE_10 = 11; - const uint8 DRIVE_11 = 12; - const uint8 DRIVE_12 = 13; - const uint8 DRIVE_13 = 14; - const uint8 DRIVE_14 = 15; - const uint8 DRIVE_15 = 16; - const uint8 DRIVE_16 = 17; - const uint8 DRIVE_17 = 18; - const uint8 DRIVE_18 = 19; - const uint8 REVERSE = 20; - const uint8 REVERSE_2 = 21; - const uint8 PARK = 22; - const uint8 LOW = 23; - const uint8 LOW_2 = 24; - }; - - struct GearReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl deleted file mode 100644 index 42323cde..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeCommand.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " Command for controlling an electronic hand brake.") - - struct HandBrakeCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=FALSE) - boolean active; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl deleted file mode 100644 index da1bf9e6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HandBrakeReport.idl +++ /dev/null @@ -1,12 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - struct HandBrakeReport { - builtin_interfaces::msg::Time stamp; - - @default (value=FALSE) - boolean report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl deleted file mode 100644 index d5a4405c..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsCommand.idl +++ /dev/null @@ -1,21 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module HazardLightsCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 DISABLE = 1; - const uint8 ENABLE = 2; - }; - - @verbatim (language="comment", text= - " Command for controlling a vehicle's hazard lights.") - - struct HazardLightsCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 command; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl deleted file mode 100644 index 264bcded..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HazardLightsReport.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module HazardLightsReport_Constants { - const uint8 DISABLE = 1; - const uint8 ENABLE = 2; - }; - - struct HazardLightsReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl deleted file mode 100644 index 9512bb41..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsCommand.idl +++ /dev/null @@ -1,22 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module HeadlightsCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 DISABLE = 1; - const uint8 ENABLE_LOW = 2; - const uint8 ENABLE_HIGH = 3; - }; - - @verbatim (language="comment", text= - " Command for controlling headlights.") - - struct HeadlightsCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 command; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl deleted file mode 100644 index 498e58c7..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HeadlightsReport.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module HeadlightsReport_Constants { - const uint8 DISABLE = 1; - const uint8 ENABLE_LOW = 2; - const uint8 ENABLE_HIGH = 3; - }; - - struct HeadlightsReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl deleted file mode 100644 index 234ba028..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornCommand.idl +++ /dev/null @@ -1,15 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " Command for controlling a horn.") - - struct HornCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=FALSE) - boolean active; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl deleted file mode 100644 index 89b2c9ef..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/HornReport.idl +++ /dev/null @@ -1,12 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - struct HornReport { - builtin_interfaces::msg::Time stamp; - - @default (value=FALSE) - boolean report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl deleted file mode 100644 index 1bebaeba..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/RawControlCommand.idl +++ /dev/null @@ -1,21 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - struct RawControlCommand { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Units for all below fields are not defined. The exact semantics for each field is defined by each" "\n" - " vehicle interface implementation. It is the system integrator's responsibility to ensure these are" "\n" - " consistent when using this interface") - uint32 throttle; - - uint32 brake; - - int32 front_steer; - - int32 rear_steer; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl deleted file mode 100644 index 6ff96a7e..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/StationaryLockingCommand.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " Command to avoid stationary locking \(Park gear, Parking Brake, etc.\)." - " This command assumes that the VehicleInterface or lower levels are controlling" - " the mechanism by which a vehicle is prevented from moving \(\"stationary locking\"\)" - " and will automatically attempt to apply stationary locking if a 0-velocity command" - " is applied for an arbitrary period of time. Setting the avoid_stationary_locking" - " field to TRUE tells the VehicleInterface or lower levels to avoid making this transition.") - - struct StationaryLockingCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=FALSE) - boolean avoid_stationary_locking; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl deleted file mode 100644 index ed6a0545..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/SteeringReport.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " SteeringReport.msg") - struct SteeringReport { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " Desired angle of the steering tire in radians left (positive)" - " or right (negative) of center (0.0)") - @default (value=0.0) - float steering_tire_angle; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl deleted file mode 100644 index 342c22b7..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsCommand.idl +++ /dev/null @@ -1,22 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module TurnIndicatorsCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 DISABLE = 1; - const uint8 ENABLE_LEFT = 2; - const uint8 ENABLE_RIGHT = 3; - }; - - @verbatim (language="comment", text= - " Command for controlling turn indicators.") - - struct TurnIndicatorsCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 command; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl deleted file mode 100644 index 2a7f975a..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/TurnIndicatorsReport.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module TurnIndicatorsReport_Constants { - const uint8 DISABLE = 1; - const uint8 ENABLE_LEFT = 2; - const uint8 ENABLE_RIGHT = 3; - }; - - struct TurnIndicatorsReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl deleted file mode 100644 index ebcc6a02..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleControlCommand.idl +++ /dev/null @@ -1,27 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " Information that is sent to Vehicle interface") - struct VehicleControlCommand { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " should be negative when reversed") - @default (value=0.0) - float long_accel_mps2; - - @verbatim (language="comment", text= - " should be negative when reversed") - @default (value=0.0) - float velocity_mps; - - @default (value=0.0) - float front_wheel_angle_rad; - - @default (value=0.0) - float rear_wheel_angle_rad; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl deleted file mode 100644 index f1589281..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleKinematicState.idl +++ /dev/null @@ -1,18 +0,0 @@ -#include "autoware_auto_planning_msgs/msg/TrajectoryPoint.idl" -#include "geometry_msgs/msg/Transform.idl" -#include "std_msgs/msg/Header.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " VehicleKinematicState.msg" "\n" - " Representation of a trajectory point with timestamp for the controller") - struct VehicleKinematicState { - std_msgs::msg::Header header; - - autoware_auto_planning_msgs::msg::TrajectoryPoint state; - - geometry_msgs::msg::Transform delta; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl deleted file mode 100644 index 50c699d2..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleOdometry.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " VehicleOdometry.msg") - struct VehicleOdometry { - builtin_interfaces::msg::Time stamp; - - @default (value=0.0) - float velocity_mps; - - @default (value=0.0) - float front_wheel_angle_rad; - - @default (value=0.0) - float rear_wheel_angle_rad; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl deleted file mode 100644 index 3cba7ca4..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateCommand.idl +++ /dev/null @@ -1,57 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module VehicleStateCommand_Constants { - const uint8 BLINKER_NO_COMMAND = 0; - const uint8 BLINKER_OFF = 1; - const uint8 BLINKER_LEFT = 2; - const uint8 BLINKER_RIGHT = 3; - const uint8 BLINKER_HAZARD = 4; - const uint8 HEADLIGHT_NO_COMMAND = 0; - const uint8 HEADLIGHT_OFF = 1; - const uint8 HEADLIGHT_ON = 2; - const uint8 HEADLIGHT_HIGH = 3; - const uint8 WIPER_NO_COMMAND = 0; - const uint8 WIPER_OFF = 1; - const uint8 WIPER_LOW = 2; - const uint8 WIPER_HIGH = 3; - const uint8 WIPER_CLEAN = 14; // Match WipersCommand::ENABLE_CLEAN - const uint8 GEAR_NO_COMMAND = 0; - const uint8 GEAR_DRIVE = 1; - const uint8 GEAR_REVERSE = 2; - const uint8 GEAR_PARK = 3; - const uint8 GEAR_LOW = 4; - const uint8 GEAR_NEUTRAL = 5; - const uint8 MODE_NO_COMMAND = 0; - const uint8 MODE_AUTONOMOUS = 1; - const uint8 MODE_MANUAL = 2; - }; - @verbatim (language="comment", text= - " VehicleStateCommand.msg") - struct VehicleStateCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 blinker; - - @default (value=0) - uint8 headlight; - - @default (value=0) - uint8 wiper; - - @default (value=0) - uint8 gear; - - @default (value=0) - uint8 mode; - - @default (value=FALSE) - boolean hand_brake; - - @default (value=FALSE) - boolean horn; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl deleted file mode 100644 index 1a85fa48..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VehicleStateReport.idl +++ /dev/null @@ -1,50 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module VehicleStateReport_Constants { - const uint8 BLINKER_OFF = 1; - const uint8 BLINKER_LEFT = 2; - const uint8 BLINKER_RIGHT = 3; - const uint8 BLINKER_HAZARD = 4; - const uint8 HEADLIGHT_OFF = 1; - const uint8 HEADLIGHT_ON = 2; - const uint8 HEADLIGHT_HIGH = 3; - const uint8 WIPER_OFF = 1; - const uint8 WIPER_LOW = 2; - const uint8 WIPER_HIGH = 3; - const uint8 WIPER_CLEAN = 14; // Match WipersCommand::ENABLE_CLEAN - const uint8 GEAR_DRIVE = 1; - const uint8 GEAR_REVERSE = 2; - const uint8 GEAR_PARK = 3; - const uint8 GEAR_LOW = 4; - const uint8 GEAR_NEUTRAL = 5; - const uint8 MODE_AUTONOMOUS = 1; - const uint8 MODE_MANUAL = 2; - const uint8 MODE_DISENGAGED = 3; - const uint8 MODE_NOT_READY = 4; - }; - - struct VehicleStateReport { - builtin_interfaces::msg::Time stamp; - - @verbatim (language="comment", text= - " 0 to 100") - uint8 fuel; - - uint8 blinker; - - uint8 headlight; - - uint8 wiper; - - uint8 gear; - - uint8 mode; - - boolean hand_brake; - - boolean horn; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl deleted file mode 100644 index 04f32028..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/VelocityReport.idl +++ /dev/null @@ -1,20 +0,0 @@ -#include "std_msgs/msg/Header.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - " VelocityReport.msg") - struct VelocityReport { - std_msgs::msg::Header header; - - @default (value=0.0) - float longitudinal_velocity; - - @default (value=0.0) - float lateral_velocity; - - @default (value=0.0) - float heading_rate; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl deleted file mode 100644 index 17f00f1f..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WheelEncoder.idl +++ /dev/null @@ -1,17 +0,0 @@ -#include "std_msgs/msg/Header.idl" - -module autoware_auto_vehicle_msgs { - module msg { - @verbatim (language="comment", text= - "Representation of a wheel-encoder measurement") - struct WheelEncoder { - std_msgs::msg::Header header; - - @verbatim (language="comment", text= - " Negative speed values indicate rotation in the opposite " "\n" - " direction of the normal direction of travel of the vehicle.") - @default (value=0.0) - float speed_mps; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl deleted file mode 100644 index 1747f2d6..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersCommand.idl +++ /dev/null @@ -1,37 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module WipersCommand_Constants { - const uint8 NO_COMMAND = 0; - const uint8 DISABLE = 1; - const uint8 ENABLE_LOW = 2; - const uint8 ENABLE_HIGH = 3; - const uint8 ENABLE_INT_1 = 4; - const uint8 ENABLE_INT_2 = 5; - const uint8 ENABLE_INT_3 = 6; - const uint8 ENABLE_INT_4 = 7; - const uint8 ENABLE_INT_5 = 8; - const uint8 ENABLE_INT_6 = 9; - const uint8 ENABLE_INT_7 = 10; - const uint8 ENABLE_INT_8 = 11; - const uint8 ENABLE_INT_9 = 12; - const uint8 ENABLE_INT_10 = 13; - const uint8 ENABLE_CLEAN = 14; - }; - - @verbatim (language="comment", text= - " Command for controlling a wiper or group of wipers.") - - @verbatim (language="comment", text= - " Each wiper or group of simultaneously-controlled wipers" - " should have their own topic which receives this message.") - - struct WipersCommand { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 command; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl deleted file mode 100644 index fe422583..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/msg/WipersReport.idl +++ /dev/null @@ -1,33 +0,0 @@ -#include "builtin_interfaces/msg/Time.idl" - -module autoware_auto_vehicle_msgs { - module msg { - module WipersReport_Constants { - const uint8 DISABLE = 1; - const uint8 ENABLE_LOW = 2; - const uint8 ENABLE_HIGH = 3; - const uint8 ENABLE_INT_1 = 4; - const uint8 ENABLE_INT_2 = 5; - const uint8 ENABLE_INT_3 = 6; - const uint8 ENABLE_INT_4 = 7; - const uint8 ENABLE_INT_5 = 8; - const uint8 ENABLE_INT_6 = 9; - const uint8 ENABLE_INT_7 = 10; - const uint8 ENABLE_INT_8 = 11; - const uint8 ENABLE_INT_9 = 12; - const uint8 ENABLE_INT_10 = 13; - const uint8 ENABLE_CLEAN = 14; - }; - - @verbatim (language="comment", text= - " Each wiper or group of simultaneously-controlled wipers" - " should have their own topic which receives this message.") - - struct WipersReport { - builtin_interfaces::msg::Time stamp; - - @default (value=0) - uint8 report; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml deleted file mode 100644 index 19f65639..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/package.xml +++ /dev/null @@ -1,28 +0,0 @@ - - - - autoware_auto_vehicle_msgs - 1.0.0 - Interfaces between core Autoware.Auto vehicle components - Apex.AI, Inc. - Apache 2 - - ament_cmake_auto - - rosidl_default_generators - - autoware_auto_planning_msgs - builtin_interfaces - std_msgs - - rosidl_default_runtime - - ament_lint_auto - ament_lint_common - - rosidl_interface_packages - - - ament_cmake - - diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl deleted file mode 100644 index 7be179d8..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/AutonomyModeChange.idl +++ /dev/null @@ -1,22 +0,0 @@ -#include "std_msgs/msg/Empty.idl" - -module autoware_auto_vehicle_msgs { - module srv { - module AutonomyModeChange_Request_Constants { - const uint8 MODE_MANUAL = 0; - const uint8 MODE_AUTONOMOUS = 1; - }; - struct AutonomyModeChange_Request - { - @verbatim(language = "comment", text = - "The desired autonomy mode") - uint8 mode; - }; - struct AutonomyModeChange_Response - { - @verbatim(language = "comment", text = - "No response is used because changing the autonomy mode requires non-trivial time") - std_msgs::msg::Empty empty; - }; - }; -}; diff --git a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv b/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv deleted file mode 100644 index 988a7a66..00000000 --- a/src/autoware_msg/autoware_auto_msgs/autoware_auto_vehicle_msgs/srv/ControlModeCommand.srv +++ /dev/null @@ -1,12 +0,0 @@ -uint8 NO_COMMAND = 0 -uint8 AUTONOMOUS = 1 -uint8 AUTONOMOUS_STEER_ONLY = 2 -uint8 AUTONOMOUS_VELOCITY_ONLY = 3 -uint8 MANUAL = 4 - -builtin_interfaces/Time stamp -uint8 mode - ---- - -bool success diff --git a/src/autoware_msg/autoware_auto_msgs/build_depends.repos b/src/autoware_msg/autoware_auto_msgs/build_depends.repos deleted file mode 100644 index 56f46b6f..00000000 --- a/src/autoware_msg/autoware_auto_msgs/build_depends.repos +++ /dev/null @@ -1 +0,0 @@ -repositories: diff --git a/src/tilde/CMakeLists.txt b/src/tilde/CMakeLists.txt deleted file mode 100644 index 2516a7f8..00000000 --- a/src/tilde/CMakeLists.txt +++ /dev/null @@ -1,158 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(tilde_cmake REQUIRED) -find_package(tilde_msg REQUIRED) -find_package(rmw REQUIRED) -find_package(LTTngUST REQUIRED) - -tilde_package() - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - find_package(sensor_msgs REQUIRED) - find_package(std_msgs REQUIRED) - ament_lint_auto_find_test_dependencies() - - ament_add_gtest(test_tilde_publisher - test/test_tilde_publisher.cpp) - target_include_directories(test_tilde_publisher - PUBLIC - $ - $) - target_link_libraries(test_tilde_publisher - ${PROJECT_NAME}) - ament_target_dependencies(test_tilde_publisher - "rclcpp" - "tilde_msg") - - ament_add_gtest(test_tilde_node - test/test_tilde_node.cpp) - target_include_directories(test_tilde_node - PUBLIC - $ - $) - target_link_libraries(test_tilde_node - ${PROJECT_NAME}) - ament_target_dependencies(test_tilde_node - "rclcpp" - "tilde_msg" - "sensor_msgs" - "std_msgs") - - ament_add_gtest(test_stamp_processor - test/test_stamp_processor.cpp) - target_include_directories(test_stamp_processor - PUBLIC - $ - $) - target_link_libraries(test_stamp_processor - ${PROJECT_NAME}) - ament_target_dependencies(test_stamp_processor - "rclcpp" - "sensor_msgs" - "std_msgs") - - ament_add_gtest(test_stee_node - test/test_stee_node.cpp) - target_include_directories(test_stee_node - PUBLIC - $ - $) - target_link_libraries(test_stee_node - ${PROJECT_NAME}) - ament_target_dependencies(test_stee_node - "rclcpp" - "tilde_msg" - "sensor_msgs") - - ament_add_gtest(test_stee_source_table - test/test_stee_source_table.cpp) - target_include_directories(test_stee_source_table - PUBLIC - $ - $) - target_link_libraries(test_stee_source_table - ${PROJECT_NAME}) - ament_target_dependencies(test_stee_source_table - "rclcpp" - "tilde_msg" - "sensor_msgs") - -endif() - -include_directories(include) -ament_export_include_directories(include) -install( - DIRECTORY include/ - DESTINATION include) - -add_library(${PROJECT_NAME} SHARED - "src/tilde_node.cpp" - "src/tilde_publisher.cpp" - "src/stee_sources_table.cpp" - "src/stee_node.cpp" - "src/tp.c" -) -target_link_libraries(${PROJECT_NAME} ${LTTNGUST_LIBRARIES}) -ament_target_dependencies(${PROJECT_NAME} - "rclcpp" - "tilde_msg" - LTTngUST -) -target_compile_definitions(${PROJECT_NAME} - PRIVATE "TILDE_BUILDING_LIBRARY") -ament_export_targets(${PROJECT_NAME}) -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(rclcpp tilde_msg) - -add_library(stee_republisher_node SHARED - src/stee_republisher_node_pointcloud2.cpp - src/stee_republisher_node_imu.cpp - src/stee_republisher_node_map.cpp) -ament_target_dependencies(stee_republisher_node - "rclcpp_components") -target_link_libraries(stee_republisher_node - ${PROJECT_NAME}) -rclcpp_components_register_node(stee_republisher_node - PLUGIN "tilde::SteeRepublisherNode" - EXECUTABLE stee_republisher_node_pointcloud2_exe) -rclcpp_components_register_node(stee_republisher_node - PLUGIN "tilde::SteeRepublisherNodeImu" - EXECUTABLE stee_republisher_node_imu_exe) -rclcpp_components_register_node(stee_republisher_node - PLUGIN "tilde::SteeRepublisherNodeMap" - EXECUTABLE stee_republisher_node_map_exe) - -install( - TARGETS ${PROJECT_NAME} - EXPORT ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -install( - TARGETS stee_republisher_node - stee_republisher_node_pointcloud2_exe - stee_republisher_node_imu_exe - stee_republisher_node_map_exe - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) - -ament_package() diff --git a/src/tilde/README.md b/src/tilde/README.md deleted file mode 100644 index 5bc9d981..00000000 --- a/src/tilde/README.md +++ /dev/null @@ -1,3 +0,0 @@ -# Tilde - -see [doc](../../doc/README.md) diff --git a/src/tilde/include/tilde/message_conversion.hpp b/src/tilde/include/tilde/message_conversion.hpp deleted file mode 100644 index 75b4dabe..00000000 --- a/src/tilde/include/tilde/message_conversion.hpp +++ /dev/null @@ -1,106 +0,0 @@ -// Copyright 2021 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__MESSAGE_CONVERSION_HPP_ -#define TILDE__MESSAGE_CONVERSION_HPP_ - -#include "tilde/message_conversion_detail.hpp" - -#include - -// sensing_msgs -#include "tilde_msg/msg/stee_imu.hpp" -#include "tilde_msg/msg/stee_point_cloud2.hpp" - -// geometry_msgs -#include "tilde_msg/msg/stee_polygon_stamped.hpp" -#include "tilde_msg/msg/stee_pose_stamped.hpp" -#include "tilde_msg/msg/stee_pose_with_covariance_stamped.hpp" -#include "tilde_msg/msg/stee_twist_stamped.hpp" -#include "tilde_msg/msg/stee_twist_with_covariance_stamped.hpp" - -// nav_msgs -#include "tilde_msg/msg/stee_occupancy_grid.hpp" -#include "tilde_msg/msg/stee_odometry.hpp" - -// autoware_auto_perception_msgs -#include "tilde_msg/msg/stee_detected_objects.hpp" -#include "tilde_msg/msg/stee_predicted_objects.hpp" -#include "tilde_msg/msg/stee_tracked_objects.hpp" -#include "tilde_msg/msg/stee_traffic_light_roi_array.hpp" -#include "tilde_msg/msg/stee_traffic_signal_array.hpp" - -// autoware_auto_planning_msgs -#include "tilde_msg/msg/stee_path.hpp" -#include "tilde_msg/msg/stee_path_with_lane_id.hpp" -#include "tilde_msg/msg/stee_trajectory.hpp" - -// autoware_auto_control_msgs -#include "tilde_msg/msg/stee_ackermann_control_command.hpp" -#include "tilde_msg/msg/stee_ackermann_lateral_command.hpp" -#include "tilde_msg/msg/stee_longitudinal_command.hpp" - -namespace tilde -{ -// define your type -using TypeTable = std::tuple< - Pair, - Pair, - - Pair, - Pair, - Pair< - geometry_msgs::msg::PoseWithCovarianceStamped, tilde_msg::msg::SteePoseWithCovarianceStamped>, - Pair, - Pair< - geometry_msgs::msg::TwistWithCovarianceStamped, tilde_msg::msg::SteeTwistWithCovarianceStamped>, - - Pair, - Pair, - - Pair, - Pair, - Pair, - Pair< - autoware_auto_perception_msgs::msg::TrafficLightRoiArray, - tilde_msg::msg::SteeTrafficLightRoiArray>, - Pair< - autoware_auto_perception_msgs::msg::TrafficSignalArray, tilde_msg::msg::SteeTrafficSignalArray>, - - Pair, - Pair, - Pair, - - Pair< - autoware_auto_control_msgs::msg::AckermannControlCommand, - tilde_msg::msg::SteeAckermannControlCommand>, - Pair< - autoware_auto_control_msgs::msg::AckermannLateralCommand, - tilde_msg::msg::SteeAckermannLateralCommand>, - Pair< - autoware_auto_control_msgs::msg::LongitudinalCommand, - tilde_msg::msg::SteeLongitudinalCommand> >; - -template -struct _ConvertedType -{ - using type = typename Get::type; -}; - -template -using ConvertedMessageType = typename _ConvertedType::type; - -} // namespace tilde - -#endif // TILDE__MESSAGE_CONVERSION_HPP_ diff --git a/src/tilde/include/tilde/message_conversion_detail.hpp b/src/tilde/include/tilde/message_conversion_detail.hpp deleted file mode 100644 index 171c6df4..00000000 --- a/src/tilde/include/tilde/message_conversion_detail.hpp +++ /dev/null @@ -1,50 +0,0 @@ -// Copyright 2021 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__MESSAGE_CONVERSION_DETAIL_HPP_ -#define TILDE__MESSAGE_CONVERSION_DETAIL_HPP_ - -#include - -namespace tilde -{ - -template -struct Pair -{ - using Key = K; - using Value = V; -}; - -template -struct Get; - -template -struct Get> -{ - using type = typename std::conditional< - std::is_same::value, typename Head::Value, std::nullptr_t>::type; -}; - -template -struct Get> -{ - using type = typename std::conditional< - std::is_same::value, typename Head::Value, - typename Get>::type>::type; -}; - -} // namespace tilde - -#endif // TILDE__MESSAGE_CONVERSION_DETAIL_HPP_ diff --git a/src/tilde/include/tilde/stee_node.hpp b/src/tilde/include/tilde/stee_node.hpp deleted file mode 100644 index 28c6b9d1..00000000 --- a/src/tilde/include/tilde/stee_node.hpp +++ /dev/null @@ -1,256 +0,0 @@ -// Copyright 2021 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__STEE_NODE_HPP_ -#define TILDE__STEE_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "tilde/message_conversion.hpp" -#include "tilde/stee_publisher.hpp" -#include "tilde/stee_sources_table.hpp" -#include "tilde/stee_subscription.hpp" -#include "tilde_msg/msg/stee_source.hpp" - -#include -#include -#include -#include -#include - -namespace tilde -{ -template -inline constexpr bool always_false_v = false; - -class SteeNode : public rclcpp::Node -{ -public: - RCLCPP_SMART_PTR_DEFINITIONS(SteeNode) - - /// see corresponding rclcpp::Node constructor - RCLCPP_PUBLIC - explicit SteeNode( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - /// see corresponding rclcpp::Node constructor - RCLCPP_PUBLIC - explicit SteeNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - RCLCPP_PUBLIC - virtual ~SteeNode(); - - template < - typename MessageT, typename ConvertedMessageT = ConvertedMessageType, - typename CallbackT, typename AllocatorT = std::allocator, - typename MessageDeleter = std::default_delete, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename CallbackArgT = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename SubscriptionT = rclcpp::Subscription, - typename ConvertedSubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy, - typename ConvertedMessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy, -#if ROS_DISTRO_GALACTIC - typename SteeSubscriptionT = SteeSubscription< - MessageT, ConvertedMessageT, AllocatorT, MessageMemoryStrategyT, - ConvertedMessageMemoryStrategyT> -#else - typename SteeSubscriptionT = SteeSubscription< - MessageT, ConvertedMessageT, AllocatorT, typename rclcpp::TypeAdapter::custom_type, - typename rclcpp::TypeAdapter::ros_message_type, MessageMemoryStrategyT, - ConvertedMessageMemoryStrategyT> -#endif - > - std::shared_ptr create_stee_subscription( - const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, - const rclcpp::SubscriptionOptionsWithAllocator & options = - rclcpp::SubscriptionOptionsWithAllocator(), - typename MessageMemoryStrategyT::SharedPtr msg_mem_strategy = - (MessageMemoryStrategyT::create_default())) - { - std::shared_ptr sub{nullptr}; - std::shared_ptr converted_sub{nullptr}; - auto stee_sub = std::make_shared(); - - if (enable_stee_) { - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(this); - auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); - auto converted_topic_name = resolved_topic_name + "/stee"; - - auto new_callback = [this, resolved_topic_name, - callback](std::unique_ptr converted_msg) -> void { - using ConstRef = const MessageT &; - using UniquePtr = std::unique_ptr; - using SharedConstPtr = std::shared_ptr; - using ConstRefSharedConstPtr = const std::shared_ptr &; - using SharedPtr = std::shared_ptr; - - auto not_stop_topic = stop_topics_.find(resolved_topic_name) == stop_topics_.end(); - - // We added NOLINT because - // google/cpplint cannot handle `if constexpr` well. - // https://github.com/cpplint/cpplint/pull/136 is not applied. - if constexpr (std::is_same_v) { // NOLINT - if (not_stop_topic) { - set_source_table(resolved_topic_name, &converted_msg); - } - callback(converted_msg->body); - } else if constexpr (std::is_same_v) { // NOLINT - if (not_stop_topic) { - set_source_table(resolved_topic_name, converted_msg.get()); - } - auto msg = std::make_unique(std::move(converted_msg->body)); - callback(std::move(msg)); - } else if constexpr (std::is_same_v) { // NOLINT - if (not_stop_topic) { - set_source_table(resolved_topic_name, converted_msg.get()); - } - auto msg = std::make_shared(std::move(converted_msg->body)); - callback(msg); - } else if constexpr (std::is_same_v) { // NOLINT - if (not_stop_topic) { - set_source_table(resolved_topic_name, converted_msg.get()); - } - const auto msg = std::make_shared(std::move(converted_msg->body)); - callback(msg); - } else if constexpr (std::is_same_v) { // NOLINT - if (not_stop_topic) { - set_source_table(resolved_topic_name, converted_msg.get()); - } - auto msg = std::make_shared(std::move(converted_msg->body)); - callback(msg); - } else { - static_assert(always_false_v, "non-exhaustive visitor"); - } - }; - - // TODO(y-okumura-isp): how to prepare converted_msg_mem_strategy - converted_sub = - create_subscription(converted_topic_name, qos, new_callback, options); - - if (converted_sub->get_publisher_count() == 0) { - RCLCPP_WARN(this->get_logger(), "no SteePublisher: '%s'", converted_topic_name.c_str()); - } - - stee_sub->set_converted_sub(converted_sub); - } else { - sub = create_subscription(topic_name, qos, callback, options, msg_mem_strategy); - stee_sub->set_sub(sub); - } - - return stee_sub; - } - - template < - typename MessageT, typename ConvertedMessageT = ConvertedMessageType, - typename AllocatorT = std::allocator, - typename PublisherT = rclcpp::Publisher, - typename ConvertedPublisherT = rclcpp::Publisher, - typename SteePublisherT = SteePublisher> - std::shared_ptr create_stee_publisher( - const std::string & topic_name, const rclcpp::QoS & qos, - const rclcpp::PublisherOptionsWithAllocator & options = - rclcpp::PublisherOptionsWithAllocator()) - { - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(this); - auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); - - auto pub = - create_publisher(resolved_topic_name, qos, options); - auto converted_pub = create_publisher( - resolved_topic_name + "/stee", qos, options); - auto stee_pub = std::make_shared( - source_table_, pub, converted_pub, get_fully_qualified_name(), this->get_clock(), - steady_clock_); - return stee_pub; - } - - template < - typename MessageT, typename ConvertedMessageT = ConvertedMessageType, - typename AllocatorT = std::allocator, - typename PublisherT = rclcpp::Publisher, - typename ConvertedPublisherT = rclcpp::Publisher, - typename SteePublisherT = SteePublisher> - std::shared_ptr create_stee_republisher( - const std::string & topic_name, const rclcpp::QoS & qos, - const rclcpp::PublisherOptionsWithAllocator & options = - rclcpp::PublisherOptionsWithAllocator()) - { - auto converted_pub = create_publisher( - topic_name + "/stee", qos, options); - auto stee_pub = std::make_shared( - source_table_, nullptr, converted_pub, get_fully_qualified_name(), this->get_clock(), - steady_clock_); - return stee_pub; - } - -private: - void init(); - std::shared_ptr steady_clock_; - - std::shared_ptr source_table_; - - /// stop topics for preventing loop topic structure - /** - * set this by ROS2 parameter. - * key: stee_stop_topics - * value: string[] - * - * TODO(y-okumura-isp): read this parameter dynamically. - * We can set it only by startup option for now. - */ - std::set stop_topics_; - - /// Enable STEE or not - /** - * We can set this parameter by "enable_stee" only at initialization. - * This is because if we change "enable/disable" dynamically, - * we need to dynamically change the message type (original type or STEE type). - * We cannot do this without re-create the publisher/subscription pair. - */ - bool enable_stee_{}; - - template > - void set_source_table(const std::string & topic, const ConvertedMessageT * msg) - { - auto stamp = Process::get_timestamp_from_const(&(msg->body)); - - if (!stamp) { - return; - } - - if (msg->sources.size() > 0) { - source_table_->set(topic, *stamp, msg->sources); - } else { - std::vector sources; - tilde_msg::msg::SteeSource source_msg; - source_msg.topic = topic; - source_msg.stamp = *stamp; - source_msg.first_subscription_steady_time = steady_clock_->now(); - sources.emplace_back(std::move(source_msg)); - source_table_->set(topic, *stamp, sources); - } - } -}; - -} // namespace tilde - -#endif // TILDE__STEE_NODE_HPP_ diff --git a/src/tilde/include/tilde/stee_publisher.hpp b/src/tilde/include/tilde/stee_publisher.hpp deleted file mode 100644 index cee533a3..00000000 --- a/src/tilde/include/tilde/stee_publisher.hpp +++ /dev/null @@ -1,218 +0,0 @@ -// Copyright 2021 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__STEE_PUBLISHER_HPP_ -#define TILDE__STEE_PUBLISHER_HPP_ - -#include "rclcpp/clock.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/publisher.hpp" -#include "tilde/message_conversion.hpp" -#include "tilde/stee_sources_table.hpp" -#include "tilde/tilde_publisher.hpp" -#include "tilde_msg/msg/stee_source.hpp" - -#include -#include -#include -#include -#include - -namespace tilde -{ -template < - typename MessageT, typename ConvertedMessageT = ConvertedMessageType, - typename AllocatorT = std::allocator> -class SteePublisher -{ -private: - using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; - using MessageAllocator = typename MessageAllocatorTraits::allocator_type; - using MessageDeleter = rclcpp::allocator::Deleter; - using PublisherT = rclcpp::Publisher; - using ConvertedPublisherT = rclcpp::Publisher; - -public: - RCLCPP_SMART_PTR_DEFINITIONS(SteePublisher) - - /// Default constructor - SteePublisher( - std::shared_ptr source_table, std::shared_ptr pub, - std::shared_ptr converted_pub, const std::string & node_fqn, - std::shared_ptr clock, std::shared_ptr steady_clock) - : source_table_(source_table), - pub_(pub), - converted_pub_(converted_pub), - node_fqn_(node_fqn), - clock_(clock), - steady_clock_(steady_clock) - { - } - - void publish(std::unique_ptr msg) - { - auto converted_msg = std::make_unique(); - // TODO(y-okumura-isp): Can we avoid copy? - converted_msg->body = *msg; - set_sources(converted_msg.get()); - - converted_pub_->publish(std::move(converted_msg)); - - if (pub_) { - pub_->publish(std::move(msg)); - } - } - - void publish(const MessageT & msg) - { - ConvertedMessageT converted_msg; - // TODO(y-okumura-isp): Can we avoid copy? - converted_msg.body = msg; - set_sources(&converted_msg); - - converted_pub_->publish(converted_msg); - - if (pub_) { - pub_->publish(msg); - } - } - - /** - * publish() variant - * We can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(const rcl_serialized_message_t & serialized_msg) - { - std::cout << "publish serialized message (not supported)" << std::endl; - // publish_info(get_timestamp(clock_->now(), msg.get())); - if (pub_) { - pub_->publish(serialized_msg); - } - } - - /** - * publish() variant - * We can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(const rclcpp::SerializedMessage & serialized_msg) - { - std::cout << "publish SerializedMessage (not supported)" << std::endl; - if (pub_) { - pub_->publish(serialized_msg); - } - } - - /** - * publish() variant - * We can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(rclcpp::LoanedMessage && loaned_msg) - { - std::cout << "publish LoanedMessage (not supported)" << std::endl; - if (pub_) { - pub_->publish(loaned_msg); - } - } - - size_t get_subscription_count() const - { - size_t ret = 0; - if (pub_) { - ret += pub_->get_subscription_count(); - } - - return ret + converted_pub_->get_subscription_count(); - } - - size_t get_intra_process_subscription_count() const - { - size_t ret = 0; - if (pub_) { - ret = pub_->get_intra_process_subscription_count(); - } - return ret + converted_pub_->get_intra_process_subscription_count(); - } - - RCLCPP_PUBLIC - const char * get_topic_name() const - { - if (pub_) { - return pub_->get_topic_name(); - } - - return converted_pub_->get_topic_name(); - } - - /// Explicit API - /** - * \param[in] sub_topic topic FQN - * \param[in] stamp header.stamp of the used message - */ - RCLCPP_PUBLIC - void add_explicit_input_info(const std::string & sub_topic, const rclcpp::Time & stamp) - { - assert(stamp.get_clock_type() == RCL_ROS_TIME); - is_explicit_ = true; - explicit_info_[sub_topic].insert(stamp); - } - -private: - std::shared_ptr source_table_; - std::shared_ptr pub_; - std::shared_ptr converted_pub_; - const std::string node_fqn_; - std::shared_ptr clock_; - std::shared_ptr steady_clock_; - - bool is_explicit_{false}; - // explicit input data - std::map> explicit_info_; - - void set_sources(ConvertedMessageT * converted_msg) - { - std::set found; - - if (!is_explicit_) { - auto topic_sources = source_table_->get_latest_sources(); - for (auto & topic_source : topic_sources) { - for (auto & source : topic_source.second) { - if (found.find(source) != found.end()) { - continue; - } - found.insert(source); - converted_msg->sources.emplace_back(std::move(source)); - } - } - } else { - for (const auto & topic_stamps : explicit_info_) { - const auto & topic = topic_stamps.first; - for (const auto & stamp : topic_stamps.second) { - auto sources = source_table_->get_sources(topic, stamp); - for (auto & source : sources) { - if (found.find(source) != found.end()) { - continue; - } - found.insert(source); - converted_msg->sources.emplace_back(std::move(source)); - } - } - } - explicit_info_.clear(); - } - } -}; - -} // namespace tilde - -#endif // TILDE__STEE_PUBLISHER_HPP_ diff --git a/src/tilde/include/tilde/stee_sources_table.hpp b/src/tilde/include/tilde/stee_sources_table.hpp deleted file mode 100644 index cb806485..00000000 --- a/src/tilde/include/tilde/stee_sources_table.hpp +++ /dev/null @@ -1,90 +0,0 @@ -// Copyright 2021 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__STEE_SOURCES_TABLE_HPP_ -#define TILDE__STEE_SOURCES_TABLE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "tilde_msg/msg/stee_source.hpp" - -#include -#include -#include -#include - -namespace tilde -{ - -struct SteeSourceCmp -{ - bool operator()( - const tilde_msg::msg::SteeSource & lhs, const tilde_msg::msg::SteeSource & rhs) const; -}; - -class SteeSourcesTable -{ -public: - // resolved topic name - using TopicName = std::string; - // header stamp - using Stamp = rclcpp::Time; - using SourcesMsg = std::vector; - using TopicSources = std::map; - // main data structure - using Sources = std::map>; - // implicit relation - using Latest = std::map; - - /// Constructor - SteeSourcesTable( - size_t default_max_stamps_per_topic, - std::map max_stamps_per_topic = std::map()); - - /// Set input sources. - /** - * \param[in] topic resolved topic name - * \param[in] stamp message stamp - * \@aram[in] sources_msg stee sources in the message - */ - void set(const TopicName & topic, const Stamp & stamp, const SourcesMsg & sources_msg); - - /// Get the latest sources. - /** - * \param[in] topic resolved topic name - * \return empty if no topic - */ - TopicSources get_latest_sources() const; - - /// Get sources of the specific message - /** - * \param[in] topic resolved topic name - * \param[in] stamp message stamp - * \return empty if not found - */ - SourcesMsg get_sources(const TopicName & topic, const Stamp & stamp) const; - -private: - /// default maximum number of stamps to Sources - size_t default_max_stamps_per_topic_; - /// maximum number of stamps to Sources - std::map max_stamps_per_topic_; - /// Sources - Sources sources_; - /// Implicit relation - Latest latest_; -}; - -} // namespace tilde - -#endif // TILDE__STEE_SOURCES_TABLE_HPP_ diff --git a/src/tilde/include/tilde/stee_subscription.hpp b/src/tilde/include/tilde/stee_subscription.hpp deleted file mode 100644 index fbf47a6e..00000000 --- a/src/tilde/include/tilde/stee_subscription.hpp +++ /dev/null @@ -1,100 +0,0 @@ -// Copyright 2021 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__STEE_SUBSCRIPTION_HPP_ -#define TILDE__STEE_SUBSCRIPTION_HPP_ - -#include "rclcpp/subscription.hpp" -#include "tilde/message_conversion.hpp" - -#include -#include -#include - -namespace tilde -{ - -#if ROS_DISTRO_GALACTIC -template < - typename CallbackMessageT, - typename ConvertedCallbackMessageT = ConvertedMessageType, - typename AllocatorT = std::allocator, - typename MessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy, - typename ConvertedMessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy > -#else -template < - typename MessageT, typename ConvertedMessageT = ConvertedMessageType, - typename AllocatorT = std::allocator, - typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, - typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, - typename MessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy, - typename ConvertedMessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy > -#endif -class SteeSubscription -{ -private: -#if ROS_DISTRO_GALACTIC - using SubscriptionT = rclcpp::Subscription; - using ConvertedSubscriptionT = - rclcpp::Subscription; -#else - using SubscriptionT = - rclcpp::Subscription; - using ConvertedSubscriptionT = rclcpp::Subscription< - ConvertedMessageT, AllocatorT, typename rclcpp::TypeAdapter::custom_type, - typename rclcpp::TypeAdapter::ros_message_type, - ConvertedMessageMemoryStrategyT>; -#endif - -public: - RCLCPP_SMART_PTR_DEFINITIONS(SteeSubscription) - - /// Constructor - /** - * Hold only one of sub or converted_sub. - */ - SteeSubscription() {} - - void set_sub(std::shared_ptr sub) - { - assert(converted_sub_ == nullptr); - sub_ = sub; - topic_fqn_ = sub->get_topic_name(); - } - - void set_converted_sub(std::shared_ptr converted_sub) - { - assert(sub_ == nullptr); - converted_sub_ = converted_sub; - std::string extended_name = converted_sub_->get_topic_name(); - std::string affix = "/stee"; - assert(affix.length() < extended_name.length()); - topic_fqn_ = extended_name.substr(0, extended_name.length() - affix.length()); - } - - [[nodiscard]] RCLCPP_PUBLIC const char * get_topic_name() const { return topic_fqn_.c_str(); } - -private: - std::shared_ptr sub_; - std::shared_ptr converted_sub_; - std::string topic_fqn_; -}; - -} // namespace tilde - -#endif // TILDE__STEE_SUBSCRIPTION_HPP_ diff --git a/src/tilde/include/tilde/tilde_node.hpp b/src/tilde/include/tilde/tilde_node.hpp deleted file mode 100644 index 87d72330..00000000 --- a/src/tilde/include/tilde/tilde_node.hpp +++ /dev/null @@ -1,261 +0,0 @@ -// Copyright 2021 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__TILDE_NODE_HPP_ -#define TILDE__TILDE_NODE_HPP_ - -#include "rcl_interfaces/msg/set_parameters_result.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/message_info.hpp" -#include "rclcpp/node.hpp" -#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" -#include "rclcpp/visibility_control.hpp" -#include "rmw/types.h" -#include "tilde/tp.h" -#include "tilde_msg/msg/message_tracking_tag.hpp" -#include "tilde_publisher.hpp" - -#include -#include -#include -#include -#include - -#define TILDE_NODE_GET_PTR(MessageT, msg, out) \ - using S = std::decay_t; \ - using ConstRef = const MessageT &; \ - using UniquePtr = std::unique_ptr; \ - using SharedConstPtr = std::shared_ptr; \ - using ConstRefSharedConstPtr = const std::shared_ptr &; \ - using SharedPtr = std::shared_ptr; \ - \ - if constexpr (std::is_same_v) { \ - (out) = &(msg); \ - } else if constexpr (std::is_same_v) { \ - (out) = (msg).get(); \ - } else if constexpr (std::is_same_v) { \ - (out) = (msg).get(); \ - } else if constexpr (std::is_same_v) { \ - (out) = (msg).get(); \ - } else if constexpr (std::is_same_v) { \ - (out) = (msg).get(); \ - } else { \ - static_assert(always_false_v, "non-exhaustive visitor!"); \ - } - -namespace tilde -{ -template -inline constexpr bool always_false_v = false; - -/// Use TildeNode instead of rclcpp::Node and its create_* methods -class TildeNode : public rclcpp::Node -{ - typedef std::map> PublisherMap; - -public: - RCLCPP_SMART_PTR_DEFINITIONS(TildeNode) - - /// see corresponding rclcpp::Node constructor - RCLCPP_PUBLIC - explicit TildeNode( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - /// see corresponding rclcpp::Node constructor - RCLCPP_PUBLIC - explicit TildeNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - RCLCPP_PUBLIC - virtual ~TildeNode() = default; - - /// create custom subscription - template < - typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, - typename MessageDeleter = std::default_delete, - typename CallbackMessageT = - typename rclcpp::subscription_traits::has_message_type::type, - typename CallbackArgT = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename SubscriptionT = rclcpp::Subscription, - typename MessageMemoryStrategyT = - rclcpp::message_memory_strategy::MessageMemoryStrategy> - std::shared_ptr create_tilde_subscription( - const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, - const rclcpp::SubscriptionOptionsWithAllocator & options = - rclcpp::SubscriptionOptionsWithAllocator(), - typename MessageMemoryStrategyT::SharedPtr msg_mem_strategy = - (MessageMemoryStrategyT::create_default())) - { - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(this); - auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); - - auto callback_addr = &callback; - - tracepoint( - TRACEPOINT_PROVIDER, tilde_subscription_init, callback_addr, get_fully_qualified_name(), - resolved_topic_name.c_str()); - - auto main_topic_callback = [this, resolved_topic_name, callback, - callback_addr](CallbackArgT msg) -> void { - if (this->enable_tilde_) { - auto subscription_time = this->now(); - auto subscription_time_steady = this->steady_clock_->now(); - - tracepoint( - TRACEPOINT_PROVIDER, tilde_subscribe, callback_addr, - subscription_time_steady.nanoseconds()); - - const MessageT * p_msg; - TILDE_NODE_GET_PTR(MessageT, msg, p_msg); - register_message_as_input( - p_msg, resolved_topic_name, subscription_time, subscription_time_steady); - } - // finally, call original function - callback(std::forward(msg)); - }; - - return create_subscription( - topic_name, qos, main_topic_callback, options, msg_mem_strategy); - } - - template < - typename MessageT, typename AllocatorT = std::allocator, - typename PublisherT = rclcpp::Publisher, - typename TildePublisherT = TildePublisher> - std::shared_ptr create_tilde_publisher( - const std::string & topic_name, const rclcpp::QoS & qos, - const rclcpp::PublisherOptionsWithAllocator & options = - rclcpp::PublisherOptionsWithAllocator()) - { - auto pub = create_publisher(topic_name, qos, options); - auto info_topic = std::string(pub->get_topic_name()) + "/message_tracking_tag"; - auto info_pub = - create_publisher(info_topic, rclcpp::QoS(1), options); - - auto tilde_pub = std::make_shared( - info_pub, pub, get_fully_qualified_name(), this->get_clock(), steady_clock_, - this->enable_tilde_); - tilde_pubs_[info_topic] = tilde_pub; - - tracepoint( - TRACEPOINT_PROVIDER, tilde_publisher_init, tilde_pub.get(), get_fully_qualified_name(), - pub->get_topic_name()); - - return tilde_pub; - } - - /// register message as input - /** - * Register message to the both implicit and explicit data store. - * Canonically, this is called when subscription gets a message. - * - * \param p_msg[in] message raw pointer, can be freed after the function call - * \param resolved_topic_name[in] topic FQN - * \param subscription_time subscription time on ROS_TIME - * \param subscription_time subscription time on steady clock - */ - template > - void register_message_as_input( - const MessageT * p_msg, const std::string & resolved_topic_name, - const rclcpp::Time & subscription_time, const rclcpp::Time & subscription_time_steady) - { - // prepare InputInfo - - auto header_stamp = Process::get_timestamp_from_const(p_msg); - - auto input_info = std::make_shared(); - - input_info->sub_time = subscription_time; - input_info->sub_time_steady = subscription_time_steady; - if (header_stamp) { - input_info->has_header_stamp = true; - input_info->header_stamp = *header_stamp; - } - - // TODO(y-okumura-isp): consider race condition in multi threaded executor. - // i.e. subA comes when subB callback which uses topicA is running - for (auto & [topic, tp] : tilde_pubs_) { - tp->set_implicit_input_info(resolved_topic_name, input_info); - if (input_info->has_header_stamp) { - tp->set_explicit_subscription_time(resolved_topic_name, input_info); - } - } - } - - /// automatically find subscription_time and subscription_time_steady - /** - * Fill subscription_time and subscription_time_steady from internal data. - * This if for TILDE framework internal use, - * so users are expected to use explicit API. - * - * \param p_msg[in] - * \param resolved_topic_name[in] - * \param subscription_time[out] - * \param subscription_time_steady[out] - * \return true if subscription_time found else false - * \sa register_message_as_input - */ - template > - bool find_subscription_time( - const MessageT * p_msg, const std::string & resolved_topic_name, - rclcpp::Time & subscription_time, rclcpp::Time & subscription_time_steady) - { - // get header stamp - auto header_stamp = Process::get_timestamp_from_const(p_msg); - - InputInfo input_info; - - if (header_stamp) { - input_info.has_header_stamp = true; - input_info.header_stamp = *header_stamp; - } - - bool found = false; - if (header_stamp && tilde_pubs_.size() > 0) { - auto pub = tilde_pubs_.begin()->second; - found = pub->get_input_info(resolved_topic_name, *header_stamp, input_info); - } - - if (found) { - subscription_time = input_info.sub_time; - subscription_time_steady = input_info.sub_time_steady; - } - - return found; - } - - rclcpp::Time get_steady_time() { return steady_clock_->now(); } - -private: - /// topic vs publishers - PublisherMap tilde_pubs_; - /// node clock may be simulation time - std::shared_ptr steady_clock_; - - /// whether to enable tilde - bool enable_tilde_; - - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; - - void init(); -}; - -} // namespace tilde - -#undef TILDE_NODE_GET_PTR - -#endif // TILDE__TILDE_NODE_HPP_ diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp deleted file mode 100644 index 1da633e3..00000000 --- a/src/tilde/include/tilde/tilde_publisher.hpp +++ /dev/null @@ -1,418 +0,0 @@ -// Copyright 2021 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__TILDE_PUBLISHER_HPP_ -#define TILDE__TILDE_PUBLISHER_HPP_ - -#include "rclcpp/clock.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/publisher.hpp" -#include "tilde/tp.h" -#include "tilde_msg/msg/message_tracking_tag.hpp" - -#include -#include -#include -#include -#include -#include -#include - -#define TILDE_S_TO_NS(seconds) ((seconds) * (1000LL * 1000LL * 1000LL)) - -namespace tilde -{ - -/// Internal class to hold input message information -class InputInfo -{ -public: - rclcpp::Time sub_time; - rclcpp::Time sub_time_steady; - bool has_header_stamp; - rclcpp::Time header_stamp; - - InputInfo() : has_header_stamp(false) {} - - bool operator==(const InputInfo & rhs) const; -}; - -/// detect header field, not found case -template -struct HasHeader : public std::false_type -{ -}; - -/// detect header field, found case -template -struct HasHeader : std::true_type -{ -}; - -/// has top level stamp -template -struct HasStamp : public std::false_type -{ -}; - -template -struct HasStamp : public std::true_type -{ -}; - -/// has top level stamp and no header -template -struct HasStampWithoutHeader : public std::false_type -{ -}; - -template -struct HasStampWithoutHeader< - M, typename std::enable_if>, HasStamp>>::type> -: public std::true_type -{ -}; - -/// SFINAE to get header.stamp, not found case -template -struct Process -{ - /// stamp getter for non-const pointer without header field - /** - * \param[in] m message - */ - static std::optional get_timestamp(M * m) - { - (void)m; - return {}; - } - - /// stamp getter for const pointer without header field - /** - * Return dummy stamp as M has no header field. - * - * \param[in] m message - */ - static std::optional get_timestamp_from_const(const M * m) - { - (void)m; - return {}; - } -}; - -/// SFINAEEs to get header.stamp, found case -template -struct Process::value>::type> -{ - /// stamp getter for non-const pointer with header field - /** - * Return header.stamp - * - * \param[in] m message - */ - static std::optional get_timestamp(M * m) { return m->header.stamp; } - - /// stamp getter for const pointer with header field - /** - * Return header.stamp - * - * \param[in] m message - */ - static std::optional get_timestamp_from_const(const M * m) - { - return m->header.stamp; - } -}; - -/// SFINAE to get top level stamp, found case -template -struct Process::value>::type> -{ - /// stamp getter for non-const pointer with header field - /** - * Return header.stamp - * - * \param[in] m message - */ - static std::optional get_timestamp(M * m) { return m->stamp; } - - /// stamp getter for const pointer with header field - /** - * Return header.stamp - * - * \param[in] m message - */ - static std::optional get_timestamp_from_const(const M * m) { return m->stamp; } -}; - -template -auto get_timestamp(rclcpp::Time t, T * a) -> decltype(rclcpp::Time(a->header.stamp), t) -{ - // std::cout << "get header timestamp" << std::endl; - rclcpp::Time ret(a->header.stamp); - return ret; -} - -rclcpp::Time get_timestamp(rclcpp::Time t, ...); - -class TildePublisherBase -{ -public: - using InfoMsg = tilde_msg::msg::MessageTrackingTag; - - /// Constructor - /** - * \param[in] clock for RCL_ROS_TIME - * \param[in] clock for RCL_STEADY_TIME - * \param[in] node_fqn a node name - * \param[in] enable enable TILDE or not - */ - explicit TildePublisherBase( - std::shared_ptr clock, std::shared_ptr steady_clock, - std::string node_fqn, bool enable = true); - - /// Set implicit input info - /** - * This is a TILDE internal API for connecting input and output. - * - * \param[in] sub_topic Subscribed topic name - * \param[in] p InputInfo to set - */ - void set_implicit_input_info( - const std::string & sub_topic, const std::shared_ptr p); - - /// Explicit API helper - /** - * It's a TILDE internal API for connecting input and output by holding - * "sub_topic + stamp" vs InputInfo. - * - * \param[in] sub_topic Subscribed topic name - * \param[in] p InputInfo - */ - void set_explicit_subscription_time( - const std::string & sub_topic, const std::shared_ptr p); - - /// Explicit API - /** - * Declare input messages explicitly. - * Specify all messages you use before publishing the message. - * - * TildePublisher gets corresponding InputInfo from topic name + stamp. - * If not found, input_info.header_stamp in MessageTrackingTag is filled - * but sub_time and sub_time_steady are zero cleared. - * - * \param[in] sub_topic used topic - * \param[in] stamp header.stamp of the used message - */ - void add_explicit_input_info(const std::string & sub_topic, const rclcpp::Time & stamp); - - /// Fill input info field of the argument message - /** - * It's a TILDE internal API. - * Fill intput info field according to implicit and explicit info. - * Explicit info is cleared after calling this. - * - * \param[out] info_msg Target message - */ - void fill_input_info(tilde_msg::msg::MessageTrackingTag & info_msg); - - /// Set how long to hold InputInfo - /** - * TildePublisher holds InputInfo of every message just a seconds to - * fill in sub_time and sub_time_steady fields of MessageTrackingTag - * for explicit API. - * You can adjust how many seconds to hold InputInfo. - * The default is 2 seconds. - * - * \param[in] sec how many seconds to hold InputInfo - */ - void set_max_sub_callback_infos_sec(size_t sec); - - /// Get input info - /** - * \param[in] topic fully qualified topic name - * \param[in] header_stamp target header.stamp - * \param[out] info output data - * - * \return true if found else false - */ - bool get_input_info( - const std::string & topic, const rclcpp::Time & header_stamp, InputInfo & info); - - /// Enable or disable TILDE - /** - * \param[in] enable boolean - */ - void set_enable(bool enable); - - void print_input_infos(); - -protected: - std::shared_ptr clock_; - std::shared_ptr steady_clock_; - - const std::string node_fqn_; - int64_t seq_; - - std::map sub_topics_; - - bool enable_; - -private: - // parent node subscription topic vs InputInfo - std::map> input_infos_; - - // explicit InputInfo - // If this is set, FW creates MessageTrackingTag only by this info - bool is_explicit_; - std::map> explicit_input_infos_; - // topic, header stamp vs sub callback time - std::map>> - explicit_sub_time_infos_; - - // how many seconds to preserve explicit_sub_callback_infos per topic - size_t MAX_SUB_CALLBACK_INFOS_SEC_; -}; - -/// rclcpp::Publisher TILDE version -template > -class TildePublisher : public TildePublisherBase -{ -private: - using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; - using MessageAllocator = typename MessageAllocatorTraits::allocator_type; - using MessageDeleter = rclcpp::allocator::Deleter; - using PublisherT = rclcpp::Publisher; - using MessageTrackingTagPublisher = rclcpp::Publisher; - -public: - RCLCPP_SMART_PTR_DEFINITIONS(TildePublisher) - - /// Default constructor - TildePublisher( - std::shared_ptr info_pub, std::shared_ptr pub, - const std::string & node_fqn, const std::shared_ptr & clock, - const std::shared_ptr & steady_clock, bool enable) - : TildePublisherBase(clock, steady_clock, node_fqn, enable), info_pub_(info_pub), pub_(pub) - { - } - - void publish(std::unique_ptr msg) - { - if (enable_) { - auto stamp = Process::get_timestamp(msg.get()); - publish_info(stamp); - } - pub_->publish(std::move(msg)); - } - - void publish(const MessageT & msg) - { - if (enable_) { - auto stamp = Process::get_timestamp_from_const(&msg); - publish_info(stamp); - } - pub_->publish(msg); - } - - /** - * publish() variant - * can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(const rcl_serialized_message_t & serialized_msg) - { - std::cout << "publish serialized message (not supported)" << std::endl; - // publish_info(get_timestamp(clock_->now(), msg.get())); - pub_->publish(serialized_msg); - } - - /** - * publish() variant - * can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(const rclcpp::SerializedMessage & serialized_msg) - { - std::cout << "publish SerializedMessage (not supported)" << std::endl; - pub_->publish(serialized_msg); - } - - /** - * publish() variant - * can send a main message but cannot send the corresponding MessageTrackingTag - */ - void publish(rclcpp::LoanedMessage && loaned_msg) - { - std::cout << "publish LoanedMessage (not supported)" << std::endl; - pub_->publish(loaned_msg); - } - - // TODO(y-okumura-isp) get_allocator - - size_t get_subscription_count() const { return pub_->get_subscription_count(); } - - size_t get_intra_process_subscription_count() const - { - return pub_->get_intra_process_subscription_count(); - } - - RCLCPP_PUBLIC - const char * get_topic_name() const { return pub_->get_topic_name(); } - -private: - std::shared_ptr info_pub_; - std::shared_ptr pub_; - const std::string node_fqn_; - - /// Publish MessageTrackingTag - /** - * \param has_header_stamp whether main message has header.stamp - * \param t header stamp - */ - void publish_info(const std::optional & t) - { - bool has_header_stamp = t ? true : false; - - auto msg = std::make_unique(); - msg->header.stamp = clock_->now(); - // msg->header.frame_id // Nothing todo - - msg->output_info.topic_name = pub_->get_topic_name(); - msg->output_info.node_fqn = node_fqn_; - msg->output_info.seq = seq_; - seq_++; - msg->output_info.pub_time = clock_->now(); - msg->output_info.pub_time_steady = steady_clock_->now(); - msg->output_info.has_header_stamp = has_header_stamp; - if (has_header_stamp) { - msg->output_info.header_stamp = *t; - } - - fill_input_info(*msg); - - for (auto & input_info : msg->input_infos) { - auto pub_time = - TILDE_S_TO_NS(msg->output_info.pub_time.sec) + msg->output_info.pub_time.nanosec; - auto sub_time_steady = - TILDE_S_TO_NS(input_info.sub_time_steady.sec) + input_info.sub_time_steady.nanosec; - auto sub = &sub_topics_[input_info.topic_name]; - tracepoint(TRACEPOINT_PROVIDER, tilde_publish, this, pub_time, sub, sub_time_steady); - } - - info_pub_->publish(std::move(msg)); - } -}; - -} // namespace tilde - -#endif // TILDE__TILDE_PUBLISHER_HPP_ diff --git a/src/tilde/include/tilde/tp.h b/src/tilde/include/tilde/tp.h deleted file mode 100644 index 5f0295cf..00000000 --- a/src/tilde/include/tilde/tp.h +++ /dev/null @@ -1,70 +0,0 @@ -// Copyright 2021 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. - -// Provide fake header guard for cpplint -#undef TILDE__TP_H_ -#ifndef TILDE__TP_H_ -#define TILDE__TP_H_ - -#undef TRACEPOINT_PROVIDER -#define TRACEPOINT_PROVIDER ros2_caret - -#undef TRACEPOINT_INCLUDE -#define TRACEPOINT_INCLUDE "tilde/tp.h" - -#if !defined(_TP_H) || defined(TRACEPOINT_HEADER_MULTI_READ) -#define _TP_H - -#include - -TRACEPOINT_EVENT( - TRACEPOINT_PROVIDER, tilde_subscription_init, - TP_ARGS( - const void *, subscription_arg, const char *, node_name_arg, const char *, topic_name_arg), - TP_FIELDS(ctf_integer_hex(const void *, subscription, subscription_arg) - ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) - -TRACEPOINT_EVENT( - TRACEPOINT_PROVIDER, tilde_subscribe, - TP_ARGS(const void *, subscription_arg, const uint64_t, tilde_message_id_arg), - TP_FIELDS(ctf_integer_hex(const void *, subscription, subscription_arg) - ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg))) - -TRACEPOINT_EVENT( - TRACEPOINT_PROVIDER, tilde_publisher_init, - TP_ARGS(const void *, publisher_arg, const char *, node_name_arg, const char *, topic_name_arg), - TP_FIELDS(ctf_integer_hex(const void *, publisher, publisher_arg) - ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) - -TRACEPOINT_EVENT( - TRACEPOINT_PROVIDER, tilde_subscribe_added, - TP_ARGS( - const void *, subscription_id_arg, const char *, node_name_arg, const char *, topic_name_arg), - TP_FIELDS(ctf_integer_hex(const void *, subscription_id, subscription_id_arg) - ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) - -TRACEPOINT_EVENT( - TRACEPOINT_PROVIDER, tilde_publish, - TP_ARGS( - const void *, publisher_arg, const uint64_t, tilde_publish_timestamp_arg, const void *, - subscription_id_arg, const uint64_t, tilde_message_id_arg), - TP_FIELDS(ctf_integer_hex(const void *, publisher, publisher_arg) - ctf_integer(const uint64_t, tilde_publish_timestamp, tilde_publish_timestamp_arg) - ctf_integer_hex(const void *, subscription_id, subscription_id_arg) - ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg))) -#endif /* _TP_H */ - -#include - -#endif // TILDE__TP_H_ diff --git a/src/tilde/package.xml b/src/tilde/package.xml deleted file mode 100644 index c82d02fd..00000000 --- a/src/tilde/package.xml +++ /dev/null @@ -1,26 +0,0 @@ - - - - tilde - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - ament_cmake - - liblttng-ust-dev - rclcpp - rclcpp_components - rmw - tilde_cmake - tilde_msg - - ament_lint_auto - caret_lint_common - sensor_msgs - - - ament_cmake - - diff --git a/src/tilde/src/stee_node.cpp b/src/tilde/src/stee_node.cpp deleted file mode 100644 index 138901eb..00000000 --- a/src/tilde/src/stee_node.cpp +++ /dev/null @@ -1,54 +0,0 @@ -// Copyright 2021 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/stee_node.hpp" - -#include - -using tilde::SteeNode; - -SteeNode::SteeNode(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) -{ - init(); -} - -SteeNode::SteeNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options) -: Node(node_name, namespace_, options) -{ - init(); -} - -SteeNode::~SteeNode() {} - -void SteeNode::init() -{ - steady_clock_.reset(new rclcpp::Clock(RCL_STEADY_TIME)); - // TODO(y-okumura-isp): set appropriate max stamps - source_table_.reset(new SteeSourcesTable(100)); - - declare_parameter("enable_stee", true); - get_parameter("enable_stee", enable_stee_); - - auto stop_topics_vec = declare_parameter>( - "stee_stop_topics", - std::vector{ - "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias", - }); - for (const auto & t : stop_topics_vec) { - stop_topics_.insert(t); - } -} diff --git a/src/tilde/src/stee_republisher_node_imu.cpp b/src/tilde/src/stee_republisher_node_imu.cpp deleted file mode 100644 index 77a68064..00000000 --- a/src/tilde/src/stee_republisher_node_imu.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// Copyright 2021 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/stee_node.hpp" - -using sensor_msgs::msg::Imu; -using tilde_msg::msg::SteeImu; - -namespace tilde -{ - -/// Stee Republisher -// TODO(y-okumura-isp): support other message types than Imu -class SteeRepublisherNodeImu : public SteeNode -{ -public: - explicit SteeRepublisherNodeImu(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode("stee_republisher_node", options) - { - init(); - } - - explicit SteeRepublisherNodeImu( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, options) - { - init(); - } - - explicit SteeRepublisherNodeImu( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, namespace_, options) - { - init(); - } - - virtual ~SteeRepublisherNodeImu() {} - -private: - std::vector original_input_topics_; - std::string original_output_topic_; - - // subscriptions for original input topics - std::vector::SharedPtr> input_stee_subscriptions_; - - // subscription + publisher for republish - rclcpp::Subscription::SharedPtr output_subscription_; - SteePublisher::SharedPtr output_republisher_; - - void init() - { - original_input_topics_ = - declare_parameter>("input_topics", std::vector{}); - original_output_topic_ = declare_parameter("output_topic", ""); - - for (const auto & input_topic : original_input_topics_) { - auto sub = create_stee_subscription( - input_topic, - rclcpp::QoS(1).best_effort(), // TODO(y-okumura-isp): parameterize QoS - [](Imu::UniquePtr msg) { (void)msg; }); - input_stee_subscriptions_.push_back(sub); - } - - output_republisher_ = create_stee_republisher(original_output_topic_, rclcpp::QoS(1)); - output_subscription_ = create_subscription( - original_output_topic_, rclcpp::QoS(1).best_effort(), [this](Imu::UniquePtr msg) { - output_republisher_->publish(std::move(msg)); - }); // TODO(y-okumura-isp): parameterize QoS - } -}; - -} // namespace tilde - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNodeImu) diff --git a/src/tilde/src/stee_republisher_node_map.cpp b/src/tilde/src/stee_republisher_node_map.cpp deleted file mode 100644 index 469493d3..00000000 --- a/src/tilde/src/stee_republisher_node_map.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// Copyright 2021 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/stee_node.hpp" - -using sensor_msgs::msg::PointCloud2; -using tilde_msg::msg::SteePointCloud2; - -namespace tilde -{ - -/// Stee Republisher -// TODO(y-okumura-isp): support other message types than PointCloud2 -class SteeRepublisherNodeMap : public SteeNode -{ -public: - explicit SteeRepublisherNodeMap(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode("stee_republisher_node", options) - { - init(); - } - - explicit SteeRepublisherNodeMap( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, options) - { - init(); - } - - explicit SteeRepublisherNodeMap( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, namespace_, options) - { - init(); - } - - virtual ~SteeRepublisherNodeMap() {} - -private: - std::vector original_input_topics_; - std::string original_output_topic_; - - // subscriptions for original input topics - std::vector::SharedPtr> input_stee_subscriptions_; - - // subscription + publisher for republish - rclcpp::Subscription::SharedPtr output_subscription_; - SteePublisher::SharedPtr output_republisher_; - - void init() - { - original_input_topics_ = - declare_parameter>("input_topics", std::vector{}); - original_output_topic_ = declare_parameter("output_topic", ""); - - for (const auto & input_topic : original_input_topics_) { - auto sub = create_stee_subscription( - input_topic, rclcpp::QoS(1), [](PointCloud2::UniquePtr msg) { (void)msg; }); - input_stee_subscriptions_.push_back(sub); - } - - output_republisher_ = create_stee_republisher( - original_output_topic_, rclcpp::QoS(1).transient_local()); - output_subscription_ = create_subscription( - original_output_topic_, rclcpp::QoS(1).transient_local(), [this](PointCloud2::UniquePtr msg) { - output_republisher_->publish(std::move(msg)); - }); // TODO(y-okumura-isp): parameterize QoS - } -}; - -} // namespace tilde - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNodeMap) diff --git a/src/tilde/src/stee_republisher_node_pointcloud2.cpp b/src/tilde/src/stee_republisher_node_pointcloud2.cpp deleted file mode 100644 index 466b3c5f..00000000 --- a/src/tilde/src/stee_republisher_node_pointcloud2.cpp +++ /dev/null @@ -1,88 +0,0 @@ -// Copyright 2021 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/stee_node.hpp" - -using sensor_msgs::msg::PointCloud2; -using tilde_msg::msg::SteePointCloud2; - -namespace tilde -{ - -/// Stee Republisher -// TODO(y-okumura-isp): support other message types than PointCloud2 -class SteeRepublisherNode : public SteeNode -{ -public: - explicit SteeRepublisherNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode("stee_republisher_node", options) - { - init(); - } - - explicit SteeRepublisherNode( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, options) - { - init(); - } - - explicit SteeRepublisherNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) - : SteeNode(node_name, namespace_, options) - { - init(); - } - - virtual ~SteeRepublisherNode() {} - -private: - std::vector original_input_topics_; - std::string original_output_topic_; - - // subscriptions for original input topics - std::vector::SharedPtr> point_cloud2_stee_subscriptions_; - - // subscription + publisher for republish - rclcpp::Subscription::SharedPtr point_cloud2_subscription_; - SteePublisher::SharedPtr point_cloud2_republisher_; - - void init() - { - original_input_topics_ = - declare_parameter>("input_topics", std::vector{}); - original_output_topic_ = declare_parameter("output_topic", ""); - - for (const auto & input_topic : original_input_topics_) { - auto sub = create_stee_subscription( - input_topic, - rclcpp::QoS(1).best_effort(), // TODO(y-okumura-isp): parameterize QoS - [](PointCloud2::UniquePtr msg) { (void)msg; }); - point_cloud2_stee_subscriptions_.push_back(sub); - } - - point_cloud2_republisher_ = - create_stee_republisher(original_output_topic_, rclcpp::QoS(1).best_effort()); - point_cloud2_subscription_ = create_subscription( - original_output_topic_, rclcpp::QoS(1).best_effort(), [this](PointCloud2::UniquePtr msg) { - point_cloud2_republisher_->publish(std::move(msg)); - }); // TODO(y-okumura-isp): parameterize QoS - } -}; - -} // namespace tilde - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNode) diff --git a/src/tilde/src/stee_sources_table.cpp b/src/tilde/src/stee_sources_table.cpp deleted file mode 100644 index e3863b69..00000000 --- a/src/tilde/src/stee_sources_table.cpp +++ /dev/null @@ -1,111 +0,0 @@ -// Copyright 2021 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/stee_sources_table.hpp" - -#include -#include -#include - -using tilde::SteeSourceCmp; -using tilde::SteeSourcesTable; - -SteeSourcesTable::SteeSourcesTable( - size_t default_max_stamps_per_topic, - std::map max_stamps_per_topic) -: default_max_stamps_per_topic_(default_max_stamps_per_topic), - max_stamps_per_topic_(std::move(max_stamps_per_topic)) -{ -} - -bool SteeSourceCmp::operator()( - const tilde_msg::msg::SteeSource & lhs, const tilde_msg::msg::SteeSource & rhs) const -{ - return ( - lhs.topic < rhs.topic || lhs.stamp.sec < rhs.stamp.sec || - lhs.stamp.nanosec < rhs.stamp.nanosec || - lhs.first_subscription_steady_time.sec < rhs.first_subscription_steady_time.sec || - lhs.first_subscription_steady_time.nanosec < rhs.first_subscription_steady_time.nanosec); -} - -void SteeSourcesTable::set( - const SteeSourcesTable::TopicName & topic, const SteeSourcesTable::Stamp & stamp, - const SteeSourcesTable::SourcesMsg & sources_msg) -{ - assert(stamp.get_clock_type() == RCL_ROS_TIME); - - std::set found; - for (const auto & source : sources_msg) { - if (found.find(source) != found.end()) { - continue; - } - found.insert(source); - sources_[topic][stamp].push_back(source); - } - latest_[topic] = stamp; - - auto max_stamps = default_max_stamps_per_topic_; - auto max_it = max_stamps_per_topic_.find(topic); - if (max_it != max_stamps_per_topic_.end()) { - max_stamps = max_it->second; - } - - auto & stamp_vs_sources = sources_[topic]; - while (stamp_vs_sources.size() > max_stamps) { - stamp_vs_sources.erase(stamp_vs_sources.begin()); - } -} - -SteeSourcesTable::TopicSources SteeSourcesTable::get_latest_sources() const -{ - SteeSourcesTable::TopicSources ret; - - for (const auto & latest_it : latest_) { - const auto & topic = latest_it.first; - const auto & stamp = latest_it.second; - - auto sources_it = sources_.find(topic); - if (sources_it == sources_.end()) { - continue; - } - - const auto & stamp_sources = sources_it->second; - auto stamp_sources_it = stamp_sources.find(stamp); - if (stamp_sources_it != stamp_sources.end()) { - const auto & sources = stamp_sources_it->second; - ret[topic] = sources; - } - } - - return ret; -} - -SteeSourcesTable::SourcesMsg SteeSourcesTable::get_sources( - const SteeSourcesTable::TopicName & topic, const SteeSourcesTable::Stamp & stamp) const -{ - assert(stamp.get_clock_type() == RCL_ROS_TIME); - SteeSourcesTable::SourcesMsg empty; - - auto it = sources_.find(topic); - if (it == sources_.end()) { - return empty; - } - - auto it_stamp_vs_sources = it->second.find(stamp); - if (it_stamp_vs_sources == it->second.end()) { - return empty; - } - - return it_stamp_vs_sources->second; -} diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp deleted file mode 100644 index d63f19a6..00000000 --- a/src/tilde/src/tilde_node.cpp +++ /dev/null @@ -1,61 +0,0 @@ -// Copyright 2021 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/tilde_node.hpp" - -#include -#include - -using tilde::TildeNode; - -TildeNode::TildeNode(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) -{ - init(); -} - -TildeNode::TildeNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options) -: Node(node_name, namespace_, options) -{ - init(); -} - -void TildeNode::init() -{ - steady_clock_.reset(new rclcpp::Clock(RCL_STEADY_TIME)); - 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) { - auto result = rcl_interfaces::msg::SetParametersResult(); - - result.successful = true; - for (const auto & parameter : parameters) { - if (parameter.get_name() == "enable_tilde") { - enable_tilde_ = parameter.as_bool(); - for (auto & [topic, pub] : tilde_pubs_) { - (void)topic; - pub->set_enable(enable_tilde_); - } - } - } - - return result; - }); -} diff --git a/src/tilde/src/tilde_publisher.cpp b/src/tilde/src/tilde_publisher.cpp deleted file mode 100644 index 9c7fcb47..00000000 --- a/src/tilde/src/tilde_publisher.cpp +++ /dev/null @@ -1,179 +0,0 @@ -// Copyright 2021 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/tilde_publisher.hpp" - -#include "tilde_msg/msg/sub_topic_time_info.hpp" - -#include -#include - -using tilde::InputInfo; -using tilde::TildePublisherBase; - -bool InputInfo::operator==(const InputInfo & rhs) const -{ - return sub_time == rhs.sub_time && sub_time_steady == rhs.sub_time_steady && - has_header_stamp == rhs.has_header_stamp && header_stamp == rhs.header_stamp; -} - -rclcpp::Time tilde::get_timestamp(rclcpp::Time t, ...) -{ - std::cout << "get rclcpp::Time t" << std::endl; - return t; -} - -TildePublisherBase::TildePublisherBase( - std::shared_ptr clock, std::shared_ptr steady_clock, - std::string node_fqn, bool enable) -: clock_(std::move(clock)), - steady_clock_(std::move(steady_clock)), - node_fqn_(std::move(node_fqn)), - seq_(0), - enable_(enable), - is_explicit_(false), - MAX_SUB_CALLBACK_INFOS_SEC_(2) -{ -} - -void TildePublisherBase::set_implicit_input_info( - const std::string & sub_topic, const std::shared_ptr p) -{ - input_infos_[sub_topic] = p; - if (sub_topics_.count(sub_topic) == 0) { - sub_topics_[sub_topic] = sub_topic; - tracepoint( - TRACEPOINT_PROVIDER, tilde_subscribe_added, &sub_topics_[sub_topic], node_fqn_.c_str(), - sub_topic.c_str()); - } -} - -void TildePublisherBase::add_explicit_input_info( - const std::string & sub_topic, const rclcpp::Time & stamp) -{ - InputInfo info; - - if (!is_explicit_) { - is_explicit_ = true; - } - - // find sub callback time info and fill info - auto it = explicit_sub_time_infos_.find(sub_topic); - if (it != explicit_sub_time_infos_.end() && it->second.find(stamp) != it->second.end()) { - info.sub_time = it->second[stamp]->sub_time; - info.sub_time_steady = it->second[stamp]->sub_time_steady; - } - - info.has_header_stamp = true; - info.header_stamp = stamp; - explicit_input_infos_[sub_topic].push_back(info); - - if (sub_topics_.count(sub_topic) == 0) { - sub_topics_[sub_topic] = sub_topic; - tracepoint( - TRACEPOINT_PROVIDER, tilde_subscribe_added, &sub_topics_[sub_topic], node_fqn_.c_str(), - sub_topic.c_str()); - } -} - -void TildePublisherBase::fill_input_info(tilde_msg::msg::MessageTrackingTag & info_msg) -{ - info_msg.input_infos.clear(); - - if (!is_explicit_) { - info_msg.input_infos.resize(input_infos_.size()); - - size_t i = 0; - for (const auto & [topic, input_info] : input_infos_) { - info_msg.input_infos[i].topic_name = topic; - info_msg.input_infos[i].sub_time = input_info->sub_time; - info_msg.input_infos[i].sub_time_steady = input_info->sub_time_steady; - info_msg.input_infos[i].has_header_stamp = input_info->has_header_stamp; - info_msg.input_infos[i].header_stamp = input_info->header_stamp; - i++; - } - } else { - for (const auto & [topic, input_infos] : explicit_input_infos_) { - for (const auto & input_info : input_infos) { - tilde_msg::msg::SubTopicTimeInfo info; - info.topic_name = topic; - info.sub_time = input_info.sub_time; - info.sub_time_steady = input_info.sub_time_steady; - info.has_header_stamp = input_info.has_header_stamp; - info.header_stamp = input_info.header_stamp; - info_msg.input_infos.push_back(info); - } - } - explicit_input_infos_.clear(); - } -} - -void TildePublisherBase::set_explicit_subscription_time( - const std::string & sub_topic, const std::shared_ptr p) -{ - auto & header_stamp2sub_time = explicit_sub_time_infos_[sub_topic]; - header_stamp2sub_time[p->header_stamp] = p; - - // cleanup old infos - rclcpp::Duration dur(MAX_SUB_CALLBACK_INFOS_SEC_, 0); - auto thres = clock_->now() - dur; - - for (auto it = header_stamp2sub_time.begin(); it != header_stamp2sub_time.end();) { - if (it->first < thres) { - it = header_stamp2sub_time.erase(it); - } else { - break; - } - } -} - -void TildePublisherBase::set_max_sub_callback_infos_sec(size_t sec) -{ - MAX_SUB_CALLBACK_INFOS_SEC_ = sec; -} - -bool TildePublisherBase::get_input_info( - const std::string & topic, const rclcpp::Time & header_stamp, InputInfo & info) -{ - auto hs2ii_it = explicit_sub_time_infos_.find(topic); - if (hs2ii_it == explicit_sub_time_infos_.end()) { - return false; - } - - auto header_stamp2input_info = hs2ii_it->second; - auto it = header_stamp2input_info.find(header_stamp); - if (it == header_stamp2input_info.end()) { - return false; - } - - info = *(it->second); - return true; -} - -void TildePublisherBase::set_enable(bool enable) { enable_ = enable; } - -void TildePublisherBase::print_input_infos() -{ - auto time2str = [](const rclcpp::Time & rt) -> std::string { - builtin_interfaces::msg::Time t = rt; - return std::string("sec: ") + std::to_string(t.sec) + " nsec: " + std::to_string(t.nanosec); - }; - - std::cout << "print_input_infos\n"; - for (auto & [topic, pinfo] : input_infos_) { - std::cout << " " << topic << "\n"; - std::cout << " sub_time: " << time2str(pinfo->sub_time) - << "\n stamp: " << time2str(pinfo->header_stamp) << std::endl; - } -} diff --git a/src/tilde/src/tp.c b/src/tilde/src/tp.c deleted file mode 100644 index d7d3e7b1..00000000 --- a/src/tilde/src/tp.c +++ /dev/null @@ -1,19 +0,0 @@ -// Copyright 2021 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. - -#define TRACEPOINT_CREATE_PROBES - -#define TRACEPOINT_DEFINE - -#include "tilde/tp.h" diff --git a/src/tilde/test/test_stamp_processor.cpp b/src/tilde/test/test_stamp_processor.cpp deleted file mode 100644 index a70c15e8..00000000 --- a/src/tilde/test/test_stamp_processor.cpp +++ /dev/null @@ -1,153 +0,0 @@ -// Copyright 2021 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 "builtin_interfaces/msg/time.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include - -#include -#include -#include - -using sensor_msgs::msg::PointCloud2; -using std_msgs::msg::String; -using tilde::Process; - -class TestStampProcessor : public ::testing::Test -{ -}; - -struct MsgWithTopLevelStamp -{ - builtin_interfaces::msg::Time stamp; -}; - -struct MsgWithHeaderAndTopLevelStamp -{ - std_msgs::msg::Header header; - builtin_interfaces::msg::Time stamp; -}; - -TEST_F(TestStampProcessor, pointer_with_header_stamp) -{ - PointCloud2 msg; - - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - msg.header.stamp = expect; - auto stamp = Process::get_timestamp(&msg); - - EXPECT_TRUE(stamp ? true : false); - EXPECT_EQ(*stamp, expect); -} - -TEST_F(TestStampProcessor, pointer_without_header_stamp) -{ - String msg; - auto stamp = Process::get_timestamp(&msg); - - EXPECT_FALSE((stamp ? true : false)); -} - -TEST_F(TestStampProcessor, const_pointer_with_header_stamp) -{ - PointCloud2 _msg; - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - _msg.header.stamp = expect; - - const PointCloud2 msg(_msg); - auto stamp = Process::get_timestamp_from_const(&msg); - - EXPECT_TRUE((stamp ? true : false)); - EXPECT_EQ(*stamp, expect); -} - -TEST_F(TestStampProcessor, const_pointer_without_header_stamp) -{ - const String msg; - auto stamp = Process::get_timestamp_from_const(&msg); - - EXPECT_FALSE((stamp ? true : false)); -} - -TEST_F(TestStampProcessor, pointer_with_top_level_stamp) -{ - MsgWithTopLevelStamp msg; - - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - msg.stamp = expect; - auto stamp = Process::get_timestamp(&msg); - - EXPECT_FALSE(tilde::HasHeader::value); - EXPECT_TRUE(tilde::HasStamp::value); - EXPECT_TRUE(tilde::HasStampWithoutHeader::value); - EXPECT_TRUE((stamp ? true : false)); - EXPECT_EQ(*stamp, expect); -} - -TEST_F(TestStampProcessor, const_pointer_with_top_level_stamp) -{ - MsgWithTopLevelStamp _msg; - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - _msg.stamp = expect; - - const MsgWithTopLevelStamp msg(_msg); - auto stamp = Process::get_timestamp_from_const(&msg); - - EXPECT_FALSE(tilde::HasHeader::value); - EXPECT_TRUE(tilde::HasStamp::value); - EXPECT_TRUE(tilde::HasStampWithoutHeader::value); - EXPECT_TRUE((stamp ? true : false)); - EXPECT_EQ(*stamp, expect); -} - -TEST_F(TestStampProcessor, pointer_with_header_and_top_level_stamp) -{ - MsgWithHeaderAndTopLevelStamp msg; - - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - rclcpp::Time unexpected(3, 4, RCL_ROS_TIME); - msg.header.stamp = expect; - msg.stamp = unexpected; - auto stamp = Process::get_timestamp(&msg); - - EXPECT_TRUE(tilde::HasHeader::value); - EXPECT_TRUE(tilde::HasStamp::value); - EXPECT_FALSE(tilde::HasStampWithoutHeader::value); - EXPECT_TRUE((stamp ? true : false)); - EXPECT_EQ(*stamp, expect); -} - -TEST_F(TestStampProcessor, const_pointer_with_header_and_top_level_stamp) -{ - MsgWithHeaderAndTopLevelStamp _msg; - - rclcpp::Time expect(1, 2, RCL_ROS_TIME); - rclcpp::Time unexpected(3, 4, RCL_ROS_TIME); - _msg.header.stamp = expect; - _msg.stamp = unexpected; - - const MsgWithHeaderAndTopLevelStamp msg(_msg); - auto stamp = Process::get_timestamp_from_const(&msg); - - EXPECT_TRUE(tilde::HasHeader::value); - EXPECT_TRUE(tilde::HasStamp::value); - EXPECT_FALSE(tilde::HasStampWithoutHeader::value); - EXPECT_TRUE((stamp ? true : false)); - EXPECT_EQ(*stamp, expect); -} diff --git a/src/tilde/test/test_stee_node.cpp b/src/tilde/test/test_stee_node.cpp deleted file mode 100644 index 6edaeb71..00000000 --- a/src/tilde/test/test_stee_node.cpp +++ /dev/null @@ -1,655 +0,0 @@ -// Copyright 2021 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/stee_node.hpp" -#include "tilde_msg/msg/stee_point_cloud2.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include - -#include -#include -#include - -using sensor_msgs::msg::PointCloud2; -using tilde::SteeNode; -using tilde_msg::msg::SteePointCloud2; - -class TestSteeNode : public ::testing::Test -{ -public: - void SetUp() override { rclcpp::init(0, nullptr); } - - void TearDown() override { rclcpp::shutdown(); } -}; - -rclcpp::Time get_time(int32_t seconds, uint32_t nanoseconds) -{ - // Use RCL_ROS_TIME because rclcpp::Time(builtin_interfaces::msg::Time) uses - // RCL_ROS_TIME. - return rclcpp::Time(seconds, nanoseconds, RCL_ROS_TIME); -} - -TEST_F(TestSteeNode, stee_publisher_unique_ptr) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto main_node = std::make_shared("stee_node", options); - auto stee_pub = main_node->create_stee_publisher("topic", 1); - - auto checker_node = std::make_shared("checker_node"); - bool received_main = false; - auto main_sub = checker_node->create_subscription( - "topic", 1, [&received_main](PointCloud2::UniquePtr msg) -> void { - EXPECT_EQ(msg->header.frame_id, "unique"); - received_main = true; - }); - bool received_converted = false; - auto converted_sub = checker_node->create_subscription( - "topic/stee", 1, [&received_converted](SteePointCloud2::UniquePtr msg) -> void { - EXPECT_EQ(msg->body.header.frame_id, "unique"); - received_converted = true; - }); - - auto unique_ptr_msg = std::make_unique(); - unique_ptr_msg->header.frame_id = "unique"; - - stee_pub->publish(std::move(unique_ptr_msg)); - - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(received_main); - EXPECT_TRUE(received_converted); -} - -TEST_F(TestSteeNode, stee_publisher_const_reference) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto main_node = std::make_shared("stee_node", options); - auto stee_pub = main_node->create_stee_publisher("topic", 1); - - auto checker_node = std::make_shared("checker_node"); - bool received_main = false; - auto main_sub = checker_node->create_subscription( - "topic", 1, [&received_main](const PointCloud2 & msg) -> void { - EXPECT_EQ(msg.header.frame_id, "unique"); - received_main = true; - }); - bool received_converted = false; - auto converted_sub = checker_node->create_subscription( - "topic/stee", 1, [&received_converted](const SteePointCloud2 & msg) -> void { - EXPECT_EQ(msg.body.header.frame_id, "unique"); - received_converted = true; - }); - - PointCloud2 msg; - msg.header.frame_id = "unique"; - - stee_pub->publish(msg); - - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(received_main); - EXPECT_TRUE(received_converted); -} - -TEST_F(TestSteeNode, stee_subscription_unique_ptr) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto sensor_node = std::make_shared("sensor_node", options); - auto checker_node = std::make_shared("checker_node", options); - auto checker_node2 = std::make_shared("checker_node2", options); - const std::string test_string = "recv_unique_ptr"; - - auto pub = sensor_node->create_stee_publisher("topic", 1); - - bool received = false; - // simulate use defined callback - auto sub = checker_node->create_stee_subscription( - "topic", 1, [test_string, &received](PointCloud2::UniquePtr msg) -> void { - received = true; - EXPECT_EQ(msg->header.frame_id, test_string); - }); - - bool received2 = false; - auto sub2 = checker_node2->create_subscription( - "topic/stee", 1, [test_string, &received2](SteePointCloud2::UniquePtr msg) -> void { - received2 = true; - EXPECT_EQ(msg->body.header.frame_id, test_string); - }); - - PointCloud2 msg; - msg.header.frame_id = test_string; - - pub->publish(msg); - rclcpp::spin_some(checker_node); - rclcpp::spin_some(checker_node2); - - EXPECT_TRUE(received); - EXPECT_TRUE(received2); -} - -TEST_F(TestSteeNode, stee_subscription_shared_const) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto sensor_node = std::make_shared("sensor_node", options); - auto checker_node = std::make_shared("checker_node", options); - auto checker_node2 = std::make_shared("checker_node2", options); - const std::string test_string = "recv_shared_const"; - - auto pub = sensor_node->create_stee_publisher("topic", 1); - - bool received = false; - // simulate use defined callback - auto sub = checker_node->create_stee_subscription( - "topic", 1, [test_string, &received](std::shared_ptr msg) -> void { - received = true; - EXPECT_EQ(msg->header.frame_id, test_string); - }); - - bool received2 = false; - auto sub2 = checker_node2->create_subscription( - "topic/stee", 1, [test_string, &received2](SteePointCloud2::UniquePtr msg) -> void { - received2 = true; - EXPECT_EQ(msg->body.header.frame_id, test_string); - }); - - PointCloud2 msg; - msg.header.frame_id = test_string; - - pub->publish(msg); - rclcpp::spin_some(checker_node); - rclcpp::spin_some(checker_node2); - - EXPECT_TRUE(received); - EXPECT_TRUE(received2); -} - -TEST_F(TestSteeNode, one_sub_msg_one_pub) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - auto checker_node = std::make_shared("checker_node", options); - - auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); - - auto main_pub = main_node->create_stee_publisher("main", 1); - bool main_received = false; - // simulate use defined callback - auto main_sub = main_node->create_stee_subscription( - "sensor", 1, [&main_received](std::shared_ptr msg) -> void { - main_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor"); - }); - - bool checker_received = false; - auto checker_sub = checker_node->create_subscription( - "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 1u); - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 100lu); - }); - - // publish sensor - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(main_received); - EXPECT_FALSE(checker_received); - - // publish main - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_received); -} - -TEST_F(TestSteeNode, two_sub_msg_select_latest) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - auto checker_node = std::make_shared("checker_node", options); - - auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); - - auto main_pub = main_node->create_stee_publisher("main", 1); - bool main_received = false; - // simulate use defined callback - auto main_sub = main_node->create_stee_subscription( - "sensor", 1, [&main_received](std::shared_ptr msg) -> void { - main_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor"); - }); - - bool checker_received = false; - auto checker_sub = checker_node->create_subscription( - "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 1u); - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 200lu); - }); - - // publish sensor (old value) - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(main_received); - EXPECT_FALSE(checker_received); - - // publish sensor (new value) - main_received = false; - - msg.header.stamp = get_time(1, 200); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(main_received); - EXPECT_FALSE(checker_received); - - // publish main - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_received); -} - -TEST_F(TestSteeNode, two_sub_msg_explicit) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - auto checker_node = std::make_shared("checker_node", options); - - auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); - - auto main_pub = main_node->create_stee_publisher("main", 1); - bool main_received = false; - // simulate use defined callback - auto main_sub = main_node->create_stee_subscription( - "sensor", 1, [&main_received](std::shared_ptr msg) -> void { - main_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor"); - }); - - bool checker_received = false; - auto checker_sub = checker_node->create_subscription( - "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 1u); - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 100lu); - }); - - // publish sensor (old value) - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(main_received); - EXPECT_FALSE(checker_received); - - // publish sensor (new value) - main_received = false; - - msg.header.stamp = get_time(1, 200); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(main_received); - EXPECT_FALSE(checker_received); - - // publish main with explicit API - main_pub->add_explicit_input_info("/sensor", get_time(1, 100)); - - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_received); -} - -// https://github.com/tier4/TILDE/issues/61 -TEST_F(TestSteeNode, test_non_fqn_topic_explicit_after_publish) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - auto checker_node = std::make_shared("checker_node", options); - - auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); - - auto main_pub = main_node->create_stee_publisher("main", 1); - bool main_received = false; - // simulate use defined callback - auto main_sub = main_node->create_stee_subscription( - "sensor", 1, [&main_received](std::shared_ptr msg) -> void { - main_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor"); - }); - - size_t num_sub = 0; - bool checker_received = false; - auto checker_sub = checker_node->create_subscription( - "main/stee", 1, - [&num_sub, &checker_received](std::shared_ptr msg) -> void { - if (num_sub == 0) { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 1u); - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 100lu); - ++num_sub; - } else if (num_sub == 1) { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 1u); - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor"); - EXPECT_EQ(source.stamp.sec, 2); - EXPECT_EQ(source.stamp.nanosec, 200lu); - ++num_sub; - } - }); - - // publish sensor1 - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - main_pub->add_explicit_input_info(main_sub->get_topic_name(), get_time(1, 100)); - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(main_received); - EXPECT_TRUE(checker_received); - EXPECT_EQ(num_sub, 1u); - - // publish sensor2 - main_received = false; - checker_received = false; - - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_sensor"; - - sensor_pub->publish(msg); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - main_pub->add_explicit_input_info(main_sub->get_topic_name(), get_time(2, 200)); - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(main_received); - EXPECT_TRUE(checker_received); - EXPECT_EQ(num_sub, 2u); -} - -TEST_F(TestSteeNode, two_topics_one_pub) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - auto checker_node = std::make_shared("checker_node2", options); - const std::string test_string = "recv_shared_const"; - - auto sensor1_pub = sensor_node->create_stee_publisher("sensor1", 1); - auto sensor2_pub = sensor_node->create_stee_publisher("sensor2", 1); - - auto main_pub = main_node->create_stee_publisher("main", 1); - - // sensor subscription - bool sensor1_received = false; - auto sensor1_sub = main_node->create_stee_subscription( - "sensor1", 1, [test_string, &sensor1_received](std::shared_ptr msg) -> void { - sensor1_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor1"); - }); - bool sensor2_received = false; - auto sensor2_sub = main_node->create_stee_subscription( - "sensor2", 1, [test_string, &sensor2_received](std::shared_ptr msg) -> void { - sensor2_received = true; - EXPECT_EQ(msg->header.frame_id, "by_sensor2"); - }); - - // checker subscription - bool checker_received = false; - auto checker_sub = checker_node->create_subscription( - "main/stee", 1, - [test_string, &checker_received](std::shared_ptr msg) -> void { - checker_received = true; - EXPECT_EQ(msg->body.header.frame_id, "by_main"); - EXPECT_EQ(msg->sources.size(), 2u); - { - const auto & source = msg->sources[0]; - EXPECT_EQ(source.topic, "/sensor1"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 100lu); - } - { - const auto & source = msg->sources[1]; - EXPECT_EQ(source.topic, "/sensor2"); - EXPECT_EQ(source.stamp.sec, 1); - EXPECT_EQ(source.stamp.nanosec, 200lu); - } - }); - - // publish sensor1 - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor1"; - - sensor1_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(sensor1_received); - EXPECT_FALSE(sensor2_received); - EXPECT_FALSE(checker_received); - - // publish sensor2p - msg.header.stamp = get_time(1, 200); - msg.header.frame_id = "by_sensor2"; - - sensor2_pub->publish(msg); - rclcpp::spin_some(main_node); - - EXPECT_TRUE(sensor1_received); - EXPECT_TRUE(sensor2_received); - EXPECT_FALSE(checker_received); - - // publish main - msg.header.stamp = get_time(2, 200); - msg.header.frame_id = "by_main"; - main_pub->publish(msg); - - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_received); -} - -TEST_F(TestSteeNode, param_enable_stee) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - options.append_parameter_override("enable_stee", false); - - // if not enable_stee, we can communicate non-stee publisher and - // stee subscription - auto sensor_node = std::make_shared("sensor_node", options); - auto main_node = std::make_shared("main_node", options); - - auto non_stee_pub = sensor_node->create_publisher("topic", 1); - - bool got_message = false; - auto main_stee_sub = main_node->create_stee_subscription( - "topic", 1, [&got_message](std::shared_ptr msg) -> void { - (void)msg; - got_message = true; - }); - - // publish sensor - PointCloud2 msg; - msg.header.stamp = get_time(1, 100); - msg.header.frame_id = "by_sensor1"; - non_stee_pub->publish(msg); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - EXPECT_TRUE(got_message); -} - -TEST_F(TestSteeNode, remove_duplicated_sources) -{ - /* DAG is - * A --> B --> C --> - * | ^ - * +-----------+ - * - * Check C publishes unique A entry although the same message of A is used by B, and C. - * Originally, A was listed twice. - */ - - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto node_a = std::make_shared("nodeA", options); - auto node_b = std::make_shared("nodeB", options); - auto node_c = std::make_shared("nodeC", options); - auto checker_node = std::make_shared("checker_node", options); - - // nodeA directly publish SteePointCloud2 to fill sources field manually - auto pub_a = node_a->create_publisher("a/stee", 1); - - // subscriptions of A - bool get_b_a{false}; - auto sub_b_a = node_b->create_stee_subscription( - "a", 1, [&get_b_a](std::shared_ptr msg) { - (void)msg; - get_b_a = true; - }); - bool get_c_a{false}; - auto sub_c_a = node_c->create_stee_subscription( - "a", 1, [&get_c_a](std::shared_ptr msg) { - (void)msg; - get_c_a = true; - }); - - auto pub_b = node_b->create_stee_publisher("b", 1); - - auto pub_c = node_c->create_stee_publisher("c", 1); - bool get_c_b{false}; - auto sub_c_b = node_c->create_stee_subscription( - "b", 1, [&pub_c, &get_c_b](std::shared_ptr msg) { - get_c_b = true; - pub_c->publish(*msg); - }); - - bool checker_node_called{false}; - auto sub_checker = checker_node->create_subscription( - "c/stee", 1, [&checker_node_called](std::shared_ptr msg) { - checker_node_called = true; - EXPECT_EQ(msg->sources.size(), 1u); - EXPECT_EQ(msg->sources[0].topic, "in"); - EXPECT_EQ(msg->sources[0].stamp.sec, 8); - EXPECT_EQ(msg->sources[0].stamp.nanosec, 18u); - EXPECT_EQ(msg->sources[0].first_subscription_steady_time.sec, 5); - EXPECT_EQ(msg->sources[0].first_subscription_steady_time.nanosec, 15u); - }); - - SteePointCloud2 msg_a; - msg_a.body.header.stamp = get_time(10, 110); - tilde_msg::msg::SteeSource source; - source.topic = "in"; - source.stamp = get_time(8, 18); - source.first_subscription_steady_time = get_time(5, 15); - msg_a.sources.push_back(source); - - pub_a->publish(msg_a); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node_b); - rclcpp::spin_some(node_c); - - PointCloud2 msg_b; - msg_b.header.stamp = msg_a.body.header.stamp; - pub_b->publish(msg_b); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node_c); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(get_b_a); - EXPECT_TRUE(get_c_a); - EXPECT_TRUE(get_c_b); - EXPECT_TRUE(checker_node_called); -} diff --git a/src/tilde/test/test_stee_source_table.cpp b/src/tilde/test/test_stee_source_table.cpp deleted file mode 100644 index 985cb7b0..00000000 --- a/src/tilde/test/test_stee_source_table.cpp +++ /dev/null @@ -1,370 +0,0 @@ -// Copyright 2021 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/stee_sources_table.hpp" - -#include - -#include -#include -#include - -using tilde::SteeSourceCmp; -using tilde::SteeSourcesTable; -using tilde_msg::msg::SteeSource; - -TEST(TestSteeSourceCmp, simple_test) -{ - SteeSourceCmp cmp; - tilde_msg::msg::SteeSource lhs; - tilde_msg::msg::SteeSource rhs; - - // topic order differs - lhs.topic = "in1"; - rhs.topic = "in2"; - - EXPECT_TRUE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); - - // same topic order - rhs.topic = lhs.topic; - EXPECT_FALSE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); - - // stamp.sec differs - rhs.stamp.sec = 1; - EXPECT_TRUE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); - - lhs.stamp.sec = rhs.stamp.sec; - - // stamp.nanosec differs - rhs.stamp.nanosec = 1; - EXPECT_TRUE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); - - lhs.stamp.nanosec = rhs.stamp.nanosec; - - // first_subscription_steady_time.sec differs - rhs.first_subscription_steady_time.sec = 1; - EXPECT_TRUE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); - - lhs.first_subscription_steady_time.sec = rhs.first_subscription_steady_time.sec; - - // first_subscription_steady_time.nanosec differs - rhs.first_subscription_steady_time.nanosec = 1; - EXPECT_TRUE(cmp(lhs, rhs)); - EXPECT_FALSE(cmp(rhs, lhs)); -} - -class TestSteeSourcesTable : public ::testing::Test -{ -public: - void SetUp() override {} - - void TearDown() override {} -}; - -rclcpp::Time get_time(int32_t sec, uint32_t nsec) { return rclcpp::Time(sec, nsec, RCL_ROS_TIME); } - -void add_sources( - const std::string & in_topic, const rclcpp::Time & stamp, const rclcpp::Time & steady, - std::vector & out) -{ - SteeSource s; - s.topic = in_topic; - s.stamp = stamp; - s.first_subscription_steady_time = steady; - out.emplace_back(std::move(s)); -} - -TEST_F(TestSteeSourcesTable, set_one_entry) -{ - SteeSourcesTable table(100); - - /** - * node topology: - * "in" -> "topic1" - */ - - // in - auto in_stamp = get_time(100, 0); - auto in_steady = get_time(0, 10); - SteeSourcesTable::SourcesMsg sources_msg; - add_sources("in", in_stamp, in_steady, sources_msg); - - // topic1 - auto topic11_stamp = get_time(101, 0); - table.set("topic1", topic11_stamp, sources_msg); - - auto check = [&in_stamp, &in_steady](const SteeSourcesTable::SourcesMsg & msg) -> void { - EXPECT_EQ(msg.size(), 1u); - - auto in_source = msg[0]; - EXPECT_EQ(in_source.topic, "in"); - EXPECT_EQ(in_source.stamp, in_stamp); - EXPECT_EQ(in_source.first_subscription_steady_time, in_steady); - }; - - // test for get_latest_sources - { - auto latest_sources = table.get_latest_sources(); - - EXPECT_EQ(latest_sources.size(), 1u); - EXPECT_EQ(latest_sources.begin()->first, "topic1"); - - auto in_sources = latest_sources.begin()->second; - check(in_sources); - } - - // test for get_sources - { - auto in_sources = table.get_sources("topic1", topic11_stamp); - EXPECT_EQ(in_sources.size(), 1u); - - check(in_sources); - } - - // test for get_sources, not found cases - { - auto not_found1 = table.get_sources("topic2", topic11_stamp); - EXPECT_EQ(not_found1.size(), 0u); - - auto stamp2 = get_time(0, 1); - auto not_found2 = table.get_sources("topic1", stamp2); - EXPECT_EQ(not_found2.size(), 0u); - } -} - -TEST_F(TestSteeSourcesTable, set_one_entry_multiple_source_topics) -{ - SteeSourcesTable table(100); - - /** - * node topology: - * in1 --> topic1 - * in2 / - */ - - SteeSourcesTable::SourcesMsg sources; - auto in11_stamp = get_time(100, 0); - auto in11_steady = get_time(0, 10); - add_sources("in1", in11_stamp, in11_steady, sources); - - auto in21_stamp = get_time(100, 10); - auto in21_steady = get_time(0, 20); - add_sources("in2", in21_stamp, in21_steady, sources); - - // topic1 input - auto topic11_stamp = get_time(110, 0); - table.set("topic1", topic11_stamp, sources); - - auto check = [&in11_stamp, &in11_steady, &in21_stamp, - &in21_steady](const SteeSourcesTable::SourcesMsg & msg) -> void { - EXPECT_EQ(msg.size(), 2u); - for (const auto & in_source : msg) { - if (in_source.topic == "in1") { - EXPECT_EQ(in_source.stamp, in11_stamp); - EXPECT_EQ(in_source.first_subscription_steady_time, in11_steady); - } else if (in_source.topic == "in2") { - EXPECT_EQ(in_source.stamp, in21_stamp); - EXPECT_EQ(in_source.first_subscription_steady_time, in21_steady); - } else { - FAIL() << "unknown source topic"; - } - } - }; - - // test for get_latest_sources - { - auto latest_sources = table.get_latest_sources(); - - EXPECT_EQ(latest_sources.size(), 1u); - EXPECT_EQ(latest_sources.begin()->first, "topic1"); - - auto in_sources = latest_sources.begin()->second; - check(in_sources); - } - - // test for get_sources - { - auto in_sources = table.get_sources("topic1", topic11_stamp); - check(in_sources); - } -} - -TEST_F(TestSteeSourcesTable, source_topic_has_multiple_stamp) -{ - SteeSourcesTable table(100); - - /** - * node topology: - * in1 --> topic1 - * - * topic1 has multiple stamps. - */ - - // topic1 msg1 input - SteeSourcesTable::SourcesMsg sources11; - auto in11_stamp = get_time(110, 0); - auto in11_steady = get_time(0, 11); - add_sources("in1", in11_stamp, in11_steady, sources11); - - auto topic11_stamp = get_time(110, 11); - table.set("topic1", topic11_stamp, sources11); - - // topic1 msg2 input - SteeSourcesTable::SourcesMsg sources12; - auto in12_stamp = get_time(120, 00); - auto in12_steady = get_time(0, 12); - add_sources("in1", in12_stamp, in12_steady, sources12); - - auto topic12_stamp = get_time(120, 12); - table.set("topic1", topic12_stamp, sources12); - - // test for get_latest sources - auto latest_sources = table.get_latest_sources(); - EXPECT_EQ(latest_sources.size(), 1u); - EXPECT_EQ(latest_sources.begin()->first, "topic1"); - - const auto & in_sources = latest_sources.begin()->second; - EXPECT_EQ(in_sources.size(), 1u); - const auto & in_source = *in_sources.begin(); - EXPECT_EQ(in_source.topic, "in1"); - EXPECT_EQ(in_source.stamp, in12_stamp); - EXPECT_EQ(in_source.first_subscription_steady_time, in12_steady); -} - -TEST_F(TestSteeSourcesTable, source_topic_has_multiple_stamp_skew) -{ - SteeSourcesTable table(100); - - /** - * node topology: - * in1 --> topic1 - * - * topic1 has two stamps, t11 and t12 where t11 < t12. - * But message arrival order is t12 -> t11. - * Thus t11 is the latest message (i.e. skew) - */ - - // topic1 msg2 input - SteeSourcesTable::SourcesMsg sources12; - auto in12_stamp = get_time(120, 00); - auto in12_steady = get_time(0, 12); - add_sources("in1", in12_stamp, in12_steady, sources12); - - auto topic12_stamp = get_time(120, 12); - table.set("topic1", topic12_stamp, sources12); - - // topic1 msg1 input - SteeSourcesTable::SourcesMsg sources11; - auto in11_stamp = get_time(110, 0); - auto in11_steady = get_time(0, 11); - add_sources("in1", in11_stamp, in11_steady, sources11); - - auto topic11_stamp = get_time(110, 11); - table.set("topic1", topic11_stamp, sources11); - - // test for get_latest sources - auto latest_sources = table.get_latest_sources(); - EXPECT_EQ(latest_sources.size(), 1u); - EXPECT_EQ(latest_sources.begin()->first, "topic1"); - - const auto & in_sources = latest_sources.begin()->second; - EXPECT_EQ(in_sources.size(), 1u); - const auto & in_source = *in_sources.begin(); - EXPECT_EQ(in_source.topic, "in1"); - EXPECT_EQ(in_source.stamp, in11_stamp); - EXPECT_EQ(in_source.first_subscription_steady_time, in11_steady); -} - -TEST_F(TestSteeSourcesTable, stamp_deletion) -{ - SteeSourcesTable table(1); - - /** - * node topology: - * in1 --> topic1 - * - * topic1 has multiple stamps. - */ - - // topic1 msg1 input - SteeSourcesTable::SourcesMsg sources11; - auto in11_stamp = get_time(110, 0); - auto in11_steady = get_time(0, 11); - add_sources("in1", in11_stamp, in11_steady, sources11); - - auto topic11_stamp = get_time(110, 11); - table.set("topic1", topic11_stamp, sources11); - - EXPECT_EQ(table.get_sources("topic1", topic11_stamp).size(), 1u); - - // topic1 msg2 input - SteeSourcesTable::SourcesMsg sources12; - auto in12_stamp = get_time(120, 00); - auto in12_steady = get_time(0, 12); - add_sources("in1", in12_stamp, in12_steady, sources12); - - auto topic12_stamp = get_time(120, 12); - table.set("topic1", topic12_stamp, sources12); - - // check topic1 msg1 is purged - EXPECT_EQ(table.get_sources("topic1", topic11_stamp).size(), 0u); -} - -TEST_F(TestSteeSourcesTable, duplicated_stamp) -{ - SteeSourcesTable table(100); - - { - SteeSourcesTable::SourcesMsg sources_msg; - auto stamp = get_time(110, 0); - auto steady = get_time(0, 11); - tilde_msg::msg::SteeSource source; - source.topic = "in"; - source.stamp = stamp; - source.first_subscription_steady_time = steady; - - // set same stamp multiple times - sources_msg.push_back(source); - sources_msg.push_back(source); - sources_msg.push_back(source); - - // set different source - source.topic = "in1"; - sources_msg.push_back(source); - - table.set("topic", stamp, sources_msg); - } - - // expect only two sources - auto latest_sources = table.get_latest_sources()["topic"]; - EXPECT_EQ(latest_sources.size(), 2u); - for (const auto & source : latest_sources) { - if (source.topic == "in") { - EXPECT_EQ(source.stamp.sec, 110); - EXPECT_EQ(source.stamp.nanosec, 0u); - EXPECT_EQ(source.first_subscription_steady_time.sec, 0); - EXPECT_EQ(source.first_subscription_steady_time.nanosec, 11u); - } else if (source.topic == "in1") { - EXPECT_EQ(source.stamp.sec, 110); - EXPECT_EQ(source.stamp.nanosec, 0u); - EXPECT_EQ(source.first_subscription_steady_time.sec, 0); - EXPECT_EQ(source.first_subscription_steady_time.nanosec, 11u); - } - } -} diff --git a/src/tilde/test/test_tilde_node.cpp b/src/tilde/test/test_tilde_node.cpp deleted file mode 100644 index af3c634c..00000000 --- a/src/tilde/test/test_tilde_node.cpp +++ /dev/null @@ -1,476 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" -#include "tilde_msg/msg/message_tracking_tag.hpp" -#include "tilde_msg/msg/test_top_level_stamp.hpp" - -#include "rosgraph_msgs/msg/clock.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include - -#include -#include -#include - -using tilde::InputInfo; -using tilde::TildeNode; -using tilde_msg::msg::MessageTrackingTag; - -class TestTildeNode : public ::testing::Test -{ -public: - void SetUp() override { rclcpp::init(0, nullptr); } - - void TearDown() override { rclcpp::shutdown(); } -}; - -std::string str(const builtin_interfaces::msg::Time & time) -{ - std::string s = ""; - s += std::to_string(time.sec); - s += "."; - s += std::to_string(time.nanosec); - return s; -} - -/** - * consider the following system: - * sensor_node -> main_node -> checker_node - * - * Check main_node MessageTrackingTag stamps by checker_node. - * The type of message is PointCloud2. - */ -TEST_F(TestTildeNode, simple_case) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto sensor_node = std::make_shared("sensorNode", options); - auto main_node = std::make_shared("pubNode", options); - auto checker_node = std::make_shared("checkerNode", options); - - auto sensor_pub = sensor_node->create_publisher("in_topic", 1); - auto clock_pub = sensor_node->create_publisher("/clock", 1); - - // apply "/clock" - rosgraph_msgs::msg::Clock clock_msg; - clock_msg.clock.sec = 123; - clock_msg.clock.nanosec = 456; - - clock_pub->publish(clock_msg); - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - // prepare pub/sub - auto main_pub = main_node->create_tilde_publisher("out_topic", 1); - auto main_sub = main_node->create_tilde_subscription( - "in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - std::cout << "main_sub_callback" << std::endl; - (void)msg; - main_pub->publish(std::move(msg)); - }); - - bool checker_sub_called = false; - auto checker_sub = checker_node->create_subscription( - "out_topic/message_tracking_tag", 1, - [&checker_sub_called, - clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { - checker_sub_called = true; - std::cout << "checker_sub_callback" << std::endl; - std::cout << "message_tracking_tag_msg: \n" - << "pub_time: " << str(message_tracking_tag_msg->output_info.pub_time) << "\n" - << "pub_time_steady: " << str(message_tracking_tag_msg->output_info.pub_time_steady) - << "\n" - << std::endl; - EXPECT_EQ(message_tracking_tag_msg->output_info.pub_time, clock_msg.clock); - EXPECT_EQ(message_tracking_tag_msg->output_info.has_header_stamp, true); - }); - - // do scenario - auto sensor_msg = std::make_unique(); - sensor_msg->header.stamp = sensor_node->now(); - sensor_pub->publish(std::move(sensor_msg)); - - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_sub_called); -} - -/** - * consider the following system: - * sensor_node -> main_node -> checker_node - * - * Check main_node MessageTrackingTag stamps by checker_node. - * The type of message is std_msgs::msg::String. - */ -TEST_F(TestTildeNode, no_header_case) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - auto sensor_node = std::make_shared("sensorNode", options); - auto main_node = std::make_shared("pubNode", options); - auto checker_node = std::make_shared("checkerNode", options); - - auto sensor_pub = sensor_node->create_publisher("in_topic", 1); - auto clock_pub = sensor_node->create_publisher("/clock", 1); - - // apply "/clock" - rosgraph_msgs::msg::Clock clock_msg; - clock_msg.clock.sec = 123; - clock_msg.clock.nanosec = 456; - - clock_pub->publish(clock_msg); - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - // prepare pub/sub - auto main_pub = main_node->create_tilde_publisher("out_topic", 1); - auto main_sub = main_node->create_tilde_subscription( - "in_topic", 1, [&main_pub](std_msgs::msg::String::UniquePtr msg) -> void { - std::cout << "main_sub_callback" << std::endl; - (void)msg; - main_pub->publish(std::move(msg)); - }); - - bool checker_sub_called = false; - auto checker_sub = checker_node->create_subscription( - "out_topic/message_tracking_tag", 1, - [&checker_sub_called, - clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { - checker_sub_called = true; - std::cout << "checker_sub_callback" << std::endl; - std::cout << "message_tracking_tag_msg: \n" - << "pub_time: " << str(message_tracking_tag_msg->output_info.pub_time) << "\n" - << "pub_time_steady: " << str(message_tracking_tag_msg->output_info.pub_time_steady) - << "\n" - << std::endl; - EXPECT_EQ(message_tracking_tag_msg->output_info.pub_time, clock_msg.clock); - EXPECT_EQ(message_tracking_tag_msg->output_info.has_header_stamp, false); - }); - - // do scenario - auto msg = std::make_unique(); - sensor_pub->publish(std::move(msg)); - - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - EXPECT_TRUE(checker_sub_called); -} - -TEST_F(TestTildeNode, enable_tilde) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - options.append_parameter_override("enable_tilde", false); - - auto sensor_node = std::make_shared("sensorNode", options); - auto main_node = std::make_shared("pubNode", options); - auto checker_node = std::make_shared("checkerNode", options); - - bool enable_tilde = true; - main_node->get_parameter("enable_tilde", enable_tilde); - EXPECT_EQ(enable_tilde, false); - - auto sensor_pub = sensor_node->create_publisher("in_topic", 1); - auto clock_pub = sensor_node->create_publisher("/clock", 1); - - // apply "/clock" - rosgraph_msgs::msg::Clock clock_msg; - clock_msg.clock.sec = 123; - clock_msg.clock.nanosec = 456; - - clock_pub->publish(clock_msg); - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - // prepare pub/sub - auto main_pub = main_node->create_tilde_publisher("out_topic", 1); - auto main_sub = main_node->create_tilde_subscription( - "in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - std::cout << "main_sub_callback" << std::endl; - (void)msg; - main_pub->publish(std::move(msg)); - }); - - bool checker_sub_called = false; - auto checker_sub = checker_node->create_subscription( - "out_topic/message_tracking_tag", 1, - [&checker_sub_called, - clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { - (void)message_tracking_tag_msg; - checker_sub_called = true; - EXPECT_TRUE(false); // expect never comes here - }); - - // do scenario - auto sensor_msg = std::make_unique(); - sensor_msg->header.stamp = sensor_node->now(); - sensor_pub->publish(std::move(sensor_msg)); - - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - EXPECT_EQ(checker_sub_called, false); -} - -TEST_F(TestTildeNode, register_message_as_input_find_subscription_time) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto sensor_node = std::make_shared("sensorNode", options); - auto main_node = std::make_shared("mainNode", options); - - auto sensor_pub = sensor_node->create_publisher("/in_topic", 1); - auto clock_pub = sensor_node->create_publisher("/clock", 1); - - // prepare pub/sub - auto main_pub = main_node->create_tilde_publisher("/out_topic", 1); - auto main_sub = main_node->create_tilde_subscription( - "/in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - std::cout << "main_sub_callback" << std::endl; - (void)msg; - main_pub->publish(std::move(msg)); - }); - - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(main_node); - auto in_topic_resolved_name = node_topics_interface->resolve_topic_name("/in_topic"); - - // publish @123.456 - rosgraph_msgs::msg::Clock clock_msg1; - clock_msg1.clock.sec = 123; - clock_msg1.clock.nanosec = 456; - - clock_pub->publish(clock_msg1); - for (int i = 0; - i < 10 || !(main_node->now() == clock_msg1.clock && sensor_node->now() == clock_msg1.clock); - i++) { - rclcpp::spin_some(sensor_node); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - rclcpp::spin_some(main_node); - } - - EXPECT_EQ(sensor_node->now(), clock_msg1.clock); - EXPECT_EQ(main_node->now(), clock_msg1.clock); - - sensor_msgs::msg::PointCloud2 sensor_msg1; - sensor_msg1.header.stamp = sensor_node->now(); - sensor_pub->publish(sensor_msg1); - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - // publish @124.321 - rosgraph_msgs::msg::Clock clock_msg2; - clock_msg2.clock.sec = 124; - clock_msg2.clock.nanosec = 321; - - clock_pub->publish(clock_msg2); - for (int i = 0; - i < 10 || !(main_node->now() == clock_msg2.clock && sensor_node->now() == clock_msg2.clock); - i++) { - rclcpp::spin_some(sensor_node); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - rclcpp::spin_some(main_node); - } - - EXPECT_EQ(sensor_node->now(), clock_msg2.clock); - EXPECT_EQ(main_node->now(), clock_msg2.clock); - - sensor_msgs::msg::PointCloud2 sensor_msg2; - sensor_msg2.header.stamp = sensor_node->now(); - sensor_pub->publish(sensor_msg2); - rclcpp::spin_some(sensor_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(main_node); - - // check - rclcpp::Time subscription_time, subscription_time_steady; - auto found = main_node->find_subscription_time( - &sensor_msg1, in_topic_resolved_name, subscription_time, subscription_time_steady); - EXPECT_TRUE(found); - builtin_interfaces::msg::Time subscription_time_msg = subscription_time; - EXPECT_EQ(subscription_time_msg.sec, clock_msg1.clock.sec); - EXPECT_EQ(subscription_time_msg.nanosec, clock_msg1.clock.nanosec); -} - -TEST_F(TestTildeNode, publish_top_level_stamp) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto main_node = std::make_shared("mainNode", options); - auto checker_node = std::make_shared("checkerNode", options); - - // prepare publishers - auto main_pub = - main_node->create_tilde_publisher("out_topic", 1); - auto clock_pub = main_node->create_publisher("/clock", 1); - - // apply clock - rosgraph_msgs::msg::Clock clock_msg; - clock_msg.clock.sec = 123; - clock_msg.clock.nanosec = 456; - - clock_pub->publish(clock_msg); - rclcpp::spin_some(main_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - // prepare checker subscription - bool checker_sub_called = false; - auto checker_sub = checker_node->create_subscription( - "out_topic/message_tracking_tag", 1, - [&checker_sub_called, - &clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { - (void)message_tracking_tag_msg; - checker_sub_called = true; - EXPECT_TRUE(message_tracking_tag_msg->output_info.has_header_stamp); - }); - - // publish - tilde_msg::msg::TestTopLevelStamp msg; - msg.stamp.sec = 1234; - msg.stamp.nanosec = 5678; - - main_pub->publish(msg); - - rclcpp::spin_some(main_node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(checker_sub_called); -} - -TEST_F(TestTildeNode, param_enable_tilde) -{ - rclcpp::NodeOptions options; - options.append_parameter_override("use_sim_time", true); - - auto main_node = std::make_shared("mainNode", options); - auto checker_node = std::make_shared("checkerNode", options); - - auto main_pub = - main_node->create_tilde_publisher("out_topic", 1); - - bool checker_sub_called = false; - auto checker_sub = checker_node->create_subscription( - "out_topic/message_tracking_tag", 1, - [&checker_sub_called]( - tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { - (void)message_tracking_tag_msg; - checker_sub_called = true; - EXPECT_TRUE(message_tracking_tag_msg->output_info.has_header_stamp); - }); - - // under enable_tilde = true - tilde_msg::msg::TestTopLevelStamp msg; - msg.stamp.sec = 1234; - msg.stamp.nanosec = 5678; - - main_pub->publish(msg); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_TRUE(checker_sub_called); - - // under enable_tilde = false - rclcpp::Parameter param("enable_tilde", false); - main_node->set_parameter(param); - rclcpp::spin_some(main_node); - checker_sub_called = false; - - msg.stamp.sec = 2234; - msg.stamp.nanosec = 5679; - - main_pub->publish(msg); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(checker_node); - - EXPECT_FALSE(checker_sub_called); -} - -class ChildParamCallback : public TildeNode -{ -public: - explicit ChildParamCallback(const rclcpp::NodeOptions & options) - : TildeNode("child_param_callback", options) - { - this->declare_parameter("child_param", "original"); - this->get_parameter("child_param", child_param_); - - param_callback_handle_ = this->add_on_set_parameters_callback( - [this](const std::vector & parameters) { - auto result = rcl_interfaces::msg::SetParametersResult(); - - result.successful = true; - for (const auto & parameter : parameters) { - if (parameter.get_name() == "child_param") { - child_param_ = parameter.as_string(); - } - } - return result; - }); - } - - std::string child_param_; - -private: - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; -}; - -TEST_F(TestTildeNode, child_param_callback) -{ - rclcpp::NodeOptions options; - - auto node = std::make_shared(options); - - // check default - bool enable_tilde = false; - node->get_parameter("enable_tilde", enable_tilde); - EXPECT_TRUE(enable_tilde); - - std::string child_param; - node->get_parameter("child_param", child_param); - EXPECT_EQ(child_param, "original"); - - // update - rclcpp::Parameter enable_tilde_update("enable_tilde", false); - node->set_parameter(enable_tilde_update); - rclcpp::Parameter child_param_update("child_param", "updated"); - node->set_parameter(child_param_update); - - // check - node->get_parameter("enable_tilde", enable_tilde); - EXPECT_FALSE(enable_tilde); - - node->get_parameter("child_param", child_param); - EXPECT_EQ(child_param, "updated"); -} diff --git a/src/tilde/test/test_tilde_publisher.cpp b/src/tilde/test/test_tilde_publisher.cpp deleted file mode 100644 index 42765e09..00000000 --- a/src/tilde/test/test_tilde_publisher.cpp +++ /dev/null @@ -1,369 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "tilde/tilde_publisher.hpp" -#include "tilde_msg/msg/message_tracking_tag.hpp" - -#include - -#include -#include - -using tilde::InputInfo; -using tilde::TildePublisherBase; -using tilde_msg::msg::MessageTrackingTag; - -class TestTildePublisher : public ::testing::Test -{ -public: - void SetUp() {} -}; - -void expect_near(const rclcpp::Time && lhs, const rclcpp::Time && rhs, uint64_t thres_ms = 1) -{ - uint64_t thres_ns = thres_ms * 1000 * 1000; - EXPECT_NEAR(lhs.nanoseconds(), rhs.nanoseconds(), thres_ns); -} - -TEST_F(TestTildePublisher, set_implicit_and_fill_input_info) -{ - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - - TildePublisherBase pub(clock, steady_clock, "node_name"); - auto info = std::make_shared(); - info->sub_time = rclcpp::Time(1, 0); - info->sub_time_steady = steady_clock->now(); - info->has_header_stamp = true; - info->header_stamp = rclcpp::Time(0, 1); - const std::string TOPIC = "sample_topic"; - - pub.set_implicit_input_info(TOPIC, info); - - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(1, 0)); - expect_near( - rclcpp::Time(msg.input_infos[0].sub_time_steady, RCL_STEADY_TIME), steady_clock->now()); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, rclcpp::Time(0, 1)); -} - -TEST_F(TestTildePublisher, add_explicit_input_info_sub_time_not_found) -{ - // set input_info & explicit_input_info - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - - TildePublisherBase pub(clock, steady_clock, "node_name"); - auto now = clock->now(); - auto info_stamp = now - rclcpp::Duration(1, 0); - auto search_stamp = now - rclcpp::Duration(2, 0); - - auto info = std::make_shared(); - info->sub_time = now; - info->sub_time_steady = steady_clock->now(); - info->has_header_stamp = true; - info->header_stamp = info_stamp; - const std::string TOPIC = "sample_topic"; - - // they should be ignored - pub.set_implicit_input_info(TOPIC, info); - pub.set_implicit_input_info(TOPIC + "2", info); - - // use TOPIC + search_stamp explicit info - // although corresponding explicit sub_time doesn't exist - pub.add_explicit_input_info(TOPIC, search_stamp); - - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(0, 0)); - EXPECT_EQ(msg.input_infos[0].sub_time_steady, rclcpp::Time(0, 0)); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, search_stamp); -} - -TEST_F(TestTildePublisher, set_explicit_subscription_time_success_then_purged) -{ - // set input_info & explicit_input_info - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TildePublisherBase pub(clock, steady_clock, "node_name"); - - const std::string TOPIC = "sample_topic"; - auto now = clock->now(); - auto recv_base = now - rclcpp::Duration(10, 0); - auto recv_base_steady = steady_clock->now(); - auto stamp_base = now - rclcpp::Duration(20, 0); - pub.set_max_sub_callback_infos_sec(21); // pseudo-boundary condition - - for (int i = 0; i < 10; i++) { - auto input_info = std::make_shared(); - input_info->sub_time = recv_base + rclcpp::Duration(i, 0); - input_info->sub_time_steady = recv_base_steady + rclcpp::Duration(i, 0); - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base + rclcpp::Duration(i, 0); - - pub.set_explicit_subscription_time(TOPIC, input_info); - } - - // normal case, middle of data - pub.add_explicit_input_info(TOPIC, stamp_base + rclcpp::Duration(5, 0)); - - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, recv_base + rclcpp::Duration(5, 0)); - EXPECT_EQ(msg.input_infos[0].sub_time_steady, recv_base_steady + rclcpp::Duration(5, 0)); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base + rclcpp::Duration(5, 0)); - - // boundary condition: the 1st element exists - pub.add_explicit_input_info(TOPIC, stamp_base); - - pub.fill_input_info(msg); - - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, recv_base); - EXPECT_EQ(msg.input_infos[0].sub_time_steady, recv_base_steady); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); - - // 1st element should be deleted - pub.set_max_sub_callback_infos_sec(20); // pseudo-boundary condition - auto input_info = std::make_shared(); - input_info->sub_time = recv_base + rclcpp::Duration(10, 0); - input_info->sub_time_steady = recv_base_steady + rclcpp::Duration(10, 0); - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base + rclcpp::Duration(10, 0); - pub.set_explicit_subscription_time(TOPIC, input_info); - - pub.add_explicit_input_info(TOPIC, stamp_base); - - pub.fill_input_info(msg); - - // check explicit info is deleted - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(0, 0)); - EXPECT_EQ(msg.input_infos[0].sub_time_steady, rclcpp::Time(0, 0)); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); -} - -TEST_F(TestTildePublisher, set_multiple_topic) -{ - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TildePublisherBase pub(clock, steady_clock, "node_name"); - const std::string TOPIC1 = "sample_topic1"; - const std::string TOPIC2 = "sample_topic2"; - - auto now = clock->now(); - auto recv_base = now - rclcpp::Duration(10, 0); - auto recv_base_steady = steady_clock->now(); - auto stamp_base = now - rclcpp::Duration(20, 0); - pub.set_max_sub_callback_infos_sec(100); // large value to prevent cleanup - - auto input_info = std::make_shared(); - input_info->sub_time = recv_base; - input_info->sub_time_steady = recv_base_steady; - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base; - - pub.set_explicit_subscription_time(TOPIC1, input_info); - pub.add_explicit_input_info(TOPIC1, stamp_base); - - auto recv_base2 = recv_base + rclcpp::Duration(2, 1); - auto recv_base2_steady = recv_base_steady + rclcpp::Duration(2, 1); - auto stamp_base2 = stamp_base + rclcpp::Duration(1, 1); - - auto input_info2 = std::make_shared(); - input_info2->sub_time = recv_base2; - input_info2->sub_time_steady = recv_base2_steady; - input_info2->has_header_stamp = true; - input_info2->header_stamp = stamp_base2; - - pub.set_explicit_subscription_time(TOPIC2, input_info2); - pub.add_explicit_input_info(TOPIC2, stamp_base2); - - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - - EXPECT_EQ(msg.input_infos.size(), 2ul); - int idx1 = 0; - int idx2 = 1; - if (msg.input_infos[0].topic_name == TOPIC2) { - idx1 = 1; - idx2 = 0; - } - - EXPECT_EQ(msg.input_infos[idx1].topic_name, TOPIC1); - EXPECT_EQ(msg.input_infos[idx1].sub_time, recv_base); - EXPECT_EQ(msg.input_infos[idx1].sub_time_steady, recv_base_steady); - EXPECT_EQ(msg.input_infos[idx1].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[idx1].header_stamp, stamp_base); - - EXPECT_EQ(msg.input_infos[idx2].topic_name, TOPIC2); - EXPECT_EQ(msg.input_infos[idx2].sub_time, recv_base2); - EXPECT_EQ(msg.input_infos[idx2].sub_time_steady, recv_base2_steady); - EXPECT_EQ(msg.input_infos[idx2].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[idx2].header_stamp, stamp_base2); -} - -TEST_F(TestTildePublisher, no_explicit_after_add_explicit) -{ - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TildePublisherBase pub(clock, steady_clock, "node_name"); - pub.set_max_sub_callback_infos_sec(100); // large value to prevent cleanup - - const std::string TOPIC = "sample_topic"; - - auto now = clock->now(); - auto now_steady = steady_clock->now(); - auto stamp_base = now; - - // (1) use explicit API - /// TOPIC subscription - { - auto input_info = std::make_shared(); - input_info->sub_time = now; - input_info->sub_time_steady = now_steady; - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base; - - pub.set_implicit_input_info(TOPIC, input_info); - pub.set_explicit_subscription_time(TOPIC, input_info); - } - - /// use explicit API - pub.add_explicit_input_info(TOPIC, stamp_base); - - /// publish - { - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - - /// check - EXPECT_EQ(msg.input_infos.size(), 1ul); - EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); - EXPECT_EQ(msg.input_infos[0].sub_time, now); - EXPECT_EQ(msg.input_infos[0].sub_time_steady, now_steady); - EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); - EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); - } - - // (test case1) publish without subscription - /// publish - { - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - /// check - EXPECT_EQ(msg.input_infos.size(), 0ul); - } - - // (test case2) publish after subscription but no explicit API - auto now2 = now + rclcpp::Duration(5, 0); - auto now_steady2 = now_steady + rclcpp::Duration(5, 0); - auto stamp_base2 = now2; - - /// sub - { - auto input_info = std::make_shared(); - input_info->sub_time = now2; - input_info->sub_time_steady = now_steady2; - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base2; - - pub.set_explicit_subscription_time(TOPIC, input_info); - - pub.set_implicit_input_info(TOPIC, input_info); - pub.set_explicit_subscription_time(TOPIC, input_info); - } - - /// publish - { - tilde_msg::msg::MessageTrackingTag msg; - pub.fill_input_info(msg); - /// check - EXPECT_EQ(msg.input_infos.size(), 0ul); - } -} - -TEST_F(TestTildePublisher, get_input_info) -{ - auto clock = std::make_shared(); - auto steady_clock = std::make_shared(RCL_STEADY_TIME); - TildePublisherBase pub(clock, steady_clock, "node_name"); - pub.set_max_sub_callback_infos_sec(1000); // large value to prevent cleanup - - const std::string TOPIC = "sample_topic"; - - // register [TOPIC][stamp_base] - auto now = clock->now(); - auto now_steady = steady_clock->now(); - auto stamp_base = now; - - auto input_info = std::make_shared(); - input_info->sub_time = now; - input_info->sub_time_steady = now_steady; - input_info->has_header_stamp = true; - input_info->header_stamp = stamp_base; - - pub.set_implicit_input_info(TOPIC, input_info); - pub.set_explicit_subscription_time(TOPIC, input_info); - - // register [TOPIC][stamp_base2] - rclcpp::Duration dur(1, 2); - auto now2 = now + dur; - auto now_steady2 = now_steady + dur; - auto stamp_base2 = now + dur; - - auto input_info2 = std::make_shared(); - input_info2->sub_time = now2; - input_info2->sub_time_steady = now_steady2; - input_info2->has_header_stamp = true; - input_info2->header_stamp = stamp_base2; - - pub.set_implicit_input_info(TOPIC, input_info2); - pub.set_explicit_subscription_time(TOPIC, input_info2); - - // topic not found - InputInfo out; - EXPECT_FALSE(pub.get_input_info("topic", stamp_base, out)); - EXPECT_FALSE(out.has_header_stamp); - - // topic found but stamp not found - EXPECT_FALSE(pub.get_input_info(TOPIC, clock->now(), out)); - EXPECT_FALSE(out.has_header_stamp); - - // found [TOPIC][stamp_base] - EXPECT_TRUE(pub.get_input_info(TOPIC, stamp_base, out)); - EXPECT_EQ(out, *input_info); - - // found [TOPIC][stamp_base2] - EXPECT_TRUE(pub.get_input_info(TOPIC, stamp_base2, out)); - EXPECT_EQ(out, *input_info2); -} diff --git a/src/tilde_cmake/CMakeLists.txt b/src/tilde_cmake/CMakeLists.txt deleted file mode 100644 index 5205d42d..00000000 --- a/src/tilde_cmake/CMakeLists.txt +++ /dev/null @@ -1,20 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_cmake) - -# find dependencies -find_package(ament_cmake_auto REQUIRED) -ament_auto_find_build_dependencies() - -list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS - "tilde_cmake-extras.cmake" -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -ament_auto_package( - INSTALL_TO_SHARE - cmake -) diff --git a/src/tilde_cmake/README.md b/src/tilde_cmake/README.md deleted file mode 100644 index f005b0e5..00000000 --- a/src/tilde_cmake/README.md +++ /dev/null @@ -1,18 +0,0 @@ -# tilde_cmake - -This package provides CMake scripts for Tilde. - -## Usage - -### tilde_package.cmake - -Call `tilde_package()` before defining build targets, which will set common options for Tilde. - -```cmake -cmake_minimum_required(VERSION 3.5) -project(package_name) - -find_package(tilde_cmake REQUIRED) -tilde_package() -find_package(tilde) -``` diff --git a/src/tilde_cmake/cmake/tilde_package.cmake b/src/tilde_cmake/cmake/tilde_package.cmake deleted file mode 100644 index aabac4ac..00000000 --- a/src/tilde_cmake/cmake/tilde_package.cmake +++ /dev/null @@ -1,25 +0,0 @@ -# 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. - -macro(tilde_package) - # Set ROS_DISTRO macros - set(ROS_DISTRO $ENV{ROS_DISTRO}) - if(${ROS_DISTRO} STREQUAL "rolling") - add_compile_definitions(ROS_DISTRO_ROLLING) - elseif(${ROS_DISTRO} STREQUAL "galactic") - add_compile_definitions(ROS_DISTRO_GALACTIC) - elseif(${ROS_DISTRO} STREQUAL "humble") - add_compile_definitions(ROS_DISTRO_HUMBLE) - endif() -endmacro() diff --git a/src/tilde_cmake/package.xml b/src/tilde_cmake/package.xml deleted file mode 100644 index 72afce88..00000000 --- a/src/tilde_cmake/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - tilde_cmake - 0.0.0 - TODO: Package description - y-okumura - Apache License 2.0 - - ament_cmake - - ament_lint_auto - caret_lint_common - - - ament_cmake - - diff --git a/src/tilde_cmake/tilde_cmake-extras.cmake b/src/tilde_cmake/tilde_cmake-extras.cmake deleted file mode 100644 index 24c4a358..00000000 --- a/src/tilde_cmake/tilde_cmake-extras.cmake +++ /dev/null @@ -1,15 +0,0 @@ -# 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_cmake_DIR}/tilde_package.cmake") diff --git a/src/tilde_deadline_detector/CMakeLists.txt b/src/tilde_deadline_detector/CMakeLists.txt deleted file mode 100644 index 9b2a930e..00000000 --- a/src/tilde_deadline_detector/CMakeLists.txt +++ /dev/null @@ -1,73 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_deadline_detector) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -include_directories(include) - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(rclcpp REQUIRED) -find_package(tilde_msg REQUIRED) -find_package(rclcpp_components REQUIRED) - -add_library(tilde_deadline_detector_node SHARED - src/forward_estimator.cpp - src/tilde_deadline_detector_node.cpp) -ament_target_dependencies(tilde_deadline_detector_node - rclcpp - rclcpp_components - tilde_msg) - -rclcpp_components_register_node(tilde_deadline_detector_node - PLUGIN "tilde_deadline_detector::TildeDeadlineDetectorNode" - EXECUTABLE tilde_deadline_detector_node_exe) - - -if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(tilde REQUIRED) - find_package(sensor_msgs REQUIRED) - find_package(builtin_interfaces REQUIRED) - - ament_add_gtest(test_forward_estimator - src/forward_estimator.cpp - test/test_forward_estimator.cpp) - ament_target_dependencies(test_forward_estimator - rclcpp - tilde_msg) - - ament_add_gtest(test_tilde_deadline_detector_node - test/test_tilde_deadline_detector_node.cpp) - ament_target_dependencies(test_tilde_deadline_detector_node - rclcpp - builtin_interfaces - sensor_msgs - tilde - tilde_msg) - target_link_libraries(test_tilde_deadline_detector_node - tilde_deadline_detector_node) - -endif() - -install(TARGETS - tilde_deadline_detector_node - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin) - -ament_package() diff --git a/src/tilde_deadline_detector/README.md b/src/tilde_deadline_detector/README.md deleted file mode 100644 index 5164c096..00000000 --- a/src/tilde_deadline_detector/README.md +++ /dev/null @@ -1,59 +0,0 @@ -# tilde_deadline_detector - -## Description - -Deadline detector by TILDE MessageTrackingTag. -You can detect deadlines in some scenarios: - -- from the sensor to some node -- a specific path - -## Requirement - -- ROS 2 galactic -- TILDE enabled application -- The Messages should have the `header.stamp` field. - -## Build - -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 -$ ros2 run tilde_deadline_detector tilde_deadline_detector_node_exe \ - --ros-args --params-file src/tilde_deadline_detector/autoware_sensors.yaml -``` - -By default, it prints subscribed topics, then runs silently. -The deadline notification is not implemented yet. - -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` | topics of which you want to detect deadline | -| | `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_deadline_detector/autoware_sensors.yaml b/src/tilde_deadline_detector/autoware_sensors.yaml deleted file mode 100644 index 07b14e70..00000000 --- a/src/tilde_deadline_detector/autoware_sensors.yaml +++ /dev/null @@ -1,50 +0,0 @@ -tilde_deadline_detector_node: - ros__parameters: - sensor_topics: [ - # gsm8 - "/sensing/lidar/front_lower/self_cropped/pointcloud_ex", - "/sensing/lidar/front_upper/self_cropped/pointcloud_ex", - "/sensing/lidar/left_lower/self_cropped/pointcloud_ex", - "/sensing/lidar/left_upper/self_cropped/pointcloud_ex", - "/sensing/lidar/rear_lower/self_cropped/pointcloud_ex", - "/sensing/lidar/rear_upper/self_cropped/pointcloud_ex", - "/sensing/lidar/right_lower/self_cropped/pointcloud_ex", - "/sensing/lidar/right_upper/self_cropped/pointcloud_ex", - # office map - "/sensing/lidar/top/self_cropped/pointcloud_ex", - "/sensing/lidar/left/self_cropped/pointcloud_ex", - "/sensing/lidar/right/self_cropped/pointcloud_ex", - # loop - "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias", - ] - target_topics: [ - # "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias" # loop of EKF/NDT - # "/localization/kinematic_state", # output of localization - # "/localization/pose_twist_fusion_filter/kinematic_state" - "/control/trajectory_follower/longitudinal/control_cmd", - ] - # specify deadline ms for topics in target_topics order. - # 0 means no deadline, and negative values are replaced by 0 - deadline_ms: [0] - skips_main_out: [ - # office map - "/sensing/lidar/top/rectified/pointcloud_ex", - "/sensing/lidar/left/rectified/pointcloud_ex", - "/sensing/lidar/right/rectified/pointcloud_ex", - "/sensing/lidar/top/rectified/pointcloud", - "/sensing/lidar/left/rectified/pointcloud", - "/sensing/lidar/right/rectified/pointcloud", - "/sensing/lidar/no_ground/pointcloud", - ] - skips_main_in: - [ - "/sensing/lidar/top/mirror_cropped/pointcloud_ex", - "/sensing/lidar/left/mirror_cropped/pointcloud_ex", - "/sensing/lidar/right/mirror_cropped/pointcloud_ex", - "/sensing/lidar/top/mirror_cropped/pointcloud_ex", - "/sensing/lidar/left/mirror_cropped/pointcloud_ex", - "/sensing/lidar/right/mirror_cropped/pointcloud_ex", - "/sensing/lidar/concatenated/pointcloud", - ] - print_report: true - print_pending_messages: false diff --git a/src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp b/src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp deleted file mode 100644 index c6227dca..00000000 --- a/src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp +++ /dev/null @@ -1,140 +0,0 @@ -// 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_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ -#define TILDE_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_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_deadline_detector - -#endif // TILDE_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ diff --git a/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp b/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp deleted file mode 100644 index 15c8a1aa..00000000 --- a/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp +++ /dev/null @@ -1,93 +0,0 @@ -// 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_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ -#define TILDE_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ - -#include "rclcpp/rclcpp.hpp" -#include "tilde_deadline_detector/forward_estimator.hpp" -#include "tilde_msg/msg/deadline_notification.hpp" -#include "tilde_msg/msg/message_tracking_tag.hpp" - -#include - -#include -#include -#include -#include - -namespace tilde_deadline_detector -{ -struct PerformanceCounter -{ - void add(float v); - - float avg{0.0}; - float max{0.0}; - uint64_t cnt{0}; -}; - -class TildeDeadlineDetectorNode : public rclcpp::Node -{ - using MessageTrackingTagSubscription = rclcpp::Subscription; - -public: - RCLCPP_PUBLIC - explicit TildeDeadlineDetectorNode( - const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - /// see corresponding rclcpp::Node constructor - RCLCPP_PUBLIC - explicit TildeDeadlineDetectorNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); - - RCLCPP_PUBLIC - explicit TildeDeadlineDetectorNode(const rclcpp::NodeOptions & options); - - RCLCPP_PUBLIC - virtual ~TildeDeadlineDetectorNode(); - - std::set get_message_tracking_tag_topics() const; - -private: - ForwardEstimator fe; - std::set sensor_topics_; - std::set target_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); -}; - -} // namespace tilde_deadline_detector - -#endif // TILDE_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ diff --git a/src/tilde_deadline_detector/package.xml b/src/tilde_deadline_detector/package.xml deleted file mode 100644 index 93a3182c..00000000 --- a/src/tilde_deadline_detector/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - tilde_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_msg - - - ament_cmake - - diff --git a/src/tilde_deadline_detector/src/forward_estimator.cpp b/src/tilde_deadline_detector/src/forward_estimator.cpp deleted file mode 100644 index 3e6a63bd..00000000 --- a/src/tilde_deadline_detector/src/forward_estimator.cpp +++ /dev/null @@ -1,296 +0,0 @@ -// 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_deadline_detector/forward_estimator.hpp" - -#include -#include -#include -#include -#include -#include - -namespace tilde_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_deadline_detector diff --git a/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp b/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp deleted file mode 100644 index 93583e8d..00000000 --- a/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp +++ /dev/null @@ -1,290 +0,0 @@ -// 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_deadline_detector/tilde_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 - -using std::chrono::milliseconds; -using tilde_msg::msg::MessageTrackingTag; - -namespace tilde_deadline_detector -{ -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++; -} - -TildeDeadlineDetectorNode::TildeDeadlineDetectorNode( - const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) -{ - init(); -} - -TildeDeadlineDetectorNode::TildeDeadlineDetectorNode( - const std::string & node_name, const std::string & namespace_, - const rclcpp::NodeOptions & options) -: Node(node_name, namespace_, options) -{ - init(); -} - -TildeDeadlineDetectorNode::TildeDeadlineDetectorNode(const rclcpp::NodeOptions & options) -: Node("tilde_deadline_detector_node", options) -{ - init(); -} - -TildeDeadlineDetectorNode::~TildeDeadlineDetectorNode() {} - -std::set TildeDeadlineDetectorNode::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; -} - -void TildeDeadlineDetectorNode::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_ - 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; - } - - // wait discovery done - std::set topics; - 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(); - } - - 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( - &TildeDeadlineDetectorNode::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 << "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 << std::endl; - this->fe.debug_print(); - } - }); - - 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 << 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; -} - -void TildeDeadlineDetectorNode::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; - - // 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; - } - - // send warning if overruns - if (print_report_) { - auto is = fe.get_input_sources(target, stamp); - print_report(target, stamp, is); - } - - 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; - } - - // check deadline and send notification if necessary - tilde_msg::msg::DeadlineNotification notification_msg; - notification_msg.header.stamp = this->now(); - notification_msg.topic_name = target; - notification_msg.stamp = stamp; - - 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; - auto elapsed = rclcpp::Time(pub_time_steady) - rclcpp::Time(src->output_info.pub_time_steady); - source_msg.elapsed = elapsed; - if (RCUTILS_MS_TO_NS(deadline_ms) <= elapsed.nanoseconds()) { - source_msg.is_overrun = true; - is_overrun = true; - } - notification_msg.sources.push_back(source_msg); - } - - if (is_overrun) { - notification_pub_->publish(notification_msg); - } - - // 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_deadline_detector - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_deadline_detector::TildeDeadlineDetectorNode) diff --git a/src/tilde_deadline_detector/test/test_forward_estimator.cpp b/src/tilde_deadline_detector/test/test_forward_estimator.cpp deleted file mode 100644 index 08a26073..00000000 --- a/src/tilde_deadline_detector/test/test_forward_estimator.cpp +++ /dev/null @@ -1,582 +0,0 @@ -// 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 "builtin_interfaces/msg/time.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tilde_deadline_detector/forward_estimator.hpp" -#include "tilde_msg/msg/message_tracking_tag.hpp" -#include "tilde_msg/msg/sub_topic_time_info.hpp" - -#include - -#include -#include -#include -#include - -using tilde_deadline_detector::ForwardEstimator; -using tilde_msg::msg::MessageTrackingTag; -using tilde_msg::msg::SubTopicTimeInfo; -using TimeMsg = builtin_interfaces::msg::Time; - -TimeMsg get_time(int sec, int nsec) -{ - TimeMsg t; - t.sec = sec; - t.nanosec = nsec; - return t; -} - -std::unique_ptr create_message_tracking_tag( - const std::string & topic, const TimeMsg & time) -{ - auto message_tracking_tag = std::make_unique(); - message_tracking_tag->output_info.topic_name = topic; - message_tracking_tag->output_info.has_header_stamp = true; - message_tracking_tag->output_info.header_stamp = time; - - return message_tracking_tag; -} - -void add_input_info( - MessageTrackingTag * target_message_tracking_tag, - const MessageTrackingTag * const in_message_tracking_tag) -{ - auto ii = SubTopicTimeInfo(); - ii.topic_name = in_message_tracking_tag->output_info.topic_name; - ii.has_header_stamp = true; - ii.header_stamp = in_message_tracking_tag->output_info.header_stamp; - target_message_tracking_tag->input_infos.push_back(ii); -} - -TEST(TestForwardEstimator, one_sensor) -{ - auto fe = ForwardEstimator(); - const std::string topic = "sensor"; - - const auto time1 = get_time(10, 100); - - auto is0 = fe.get_input_sources(topic, time1); - EXPECT_EQ(is0.size(), 0u); - - // msg 1 - auto message_tracking_tag1 = create_message_tracking_tag(topic, time1); - fe.add(std::move(message_tracking_tag1)); - - auto is1 = fe.get_input_sources(topic, time1); - EXPECT_NE(is1.find(topic), is1.end()); - EXPECT_EQ(is1[topic].size(), 1u); - EXPECT_EQ(*is1[topic].begin(), time1); - - // msg 2 - auto time2 = get_time(11, 110); - auto message_tracking_tag2 = create_message_tracking_tag(topic, time2); - - fe.add(std::move(message_tracking_tag2)); - - auto is2 = fe.get_input_sources(topic, time2); - EXPECT_NE(is2.find(topic), is2.end()); - EXPECT_EQ(is2[topic].size(), 1u); - EXPECT_EQ(*is2[topic].begin(), time2); -} - -TEST(TestForwardEstimator, two_sensors) -{ - auto fe = ForwardEstimator(); - - // sensor1 msg1 - const std::string topic1 = "sensor1"; - const auto time11 = get_time(10, 100); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - fe.add(std::move(message_tracking_tag11)); - - auto is11 = fe.get_input_sources(topic1, time11); - EXPECT_NE(is11.find(topic1), is11.end()); - EXPECT_EQ(is11[topic1].size(), 1u); - EXPECT_EQ(*is11[topic1].begin(), time11); - - // sensor2 msg1 - const std::string topic2 = "sensor2"; - const auto time21 = get_time(21, 210); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - - fe.add(std::move(message_tracking_tag21)); - - auto is21 = fe.get_input_sources(topic2, time21); - EXPECT_NE(is21.find(topic2), is21.end()); - EXPECT_EQ(is21[topic2].size(), 1u); - EXPECT_EQ(*is21[topic2].begin(), time21); - - // skew - auto is1_21 = fe.get_input_sources(topic1, time21); // topic1 of time21 - EXPECT_EQ(is1_21.find(topic1), is1_21.end()); -} - -TEST(TestForwardEstimator, simple_flow_stamp_preserved) -{ - // A -> B -> C. header.stamp is preserved - auto fe = ForwardEstimator(); - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - // sensor1 msg1 - const auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // sensor2 msg1 - const auto time21 = time11; - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - - // sensor3 msg1 - rclcpp::Time time31 = time11; - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - // check - auto is21 = fe.get_input_sources(topic2, time21); - EXPECT_EQ(is21.size(), 1u); - EXPECT_NE(is21.find(topic1), is21.end()); - EXPECT_EQ(is21[topic1].size(), 1u); - EXPECT_EQ(*is21[topic1].begin(), time11); - - auto is31 = fe.get_input_sources(topic3, time31); - EXPECT_EQ(is31.size(), 1u); - EXPECT_NE(is31.find(topic1), is31.end()); - EXPECT_EQ(is31[topic1].size(), 1u); - EXPECT_EQ(*is31[topic1].begin(), time11); -} - -TEST(TestForwardEstimator, simple_flow_stamp_update) -{ - // A -> B -> C. header.stamp is updated - auto fe = ForwardEstimator(); - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - // sensor1 msg1 - const auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // sensor2 msg1 - const auto time21 = get_time(21, 210); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - - // sensor3 msg1 - const auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - // check - auto is21 = fe.get_input_sources(topic2, time21); - EXPECT_EQ(is21.size(), 1u); - EXPECT_NE(is21.find(topic1), is21.end()); - EXPECT_EQ(is21[topic1].size(), 1u); - EXPECT_EQ(*is21[topic1].begin(), time11); - - auto is31 = fe.get_input_sources(topic3, time31); - EXPECT_EQ(is31.size(), 1u); - EXPECT_NE(is31.find(topic1), is31.end()); - EXPECT_EQ(is31[topic1].size(), 1u); - EXPECT_EQ(*is31[topic1].begin(), time11); -} - -TEST(TestForwardEstimator, merged_flow) -{ - // A,B -> C. header.stamp - auto fe = ForwardEstimator(); - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - // sensor1 msg1 - const auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // sensor2 msg1 - const auto time21 = get_time(21, 210); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - - // sensor2 msg2 - const auto time22 = get_time(22, 220); - auto message_tracking_tag22 = create_message_tracking_tag(topic2, time22); - - // sensor3 msg1 consists of msg 11, 21, 22 - const auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag22.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag22)); - fe.add(std::move(message_tracking_tag31)); - - // check - auto is31 = fe.get_input_sources(topic3, time31); - EXPECT_EQ(is31.size(), 2u); - EXPECT_NE(is31.find(topic1), is31.end()); - EXPECT_EQ(is31[topic1].size(), 1u); - EXPECT_EQ(*is31[topic1].begin(), time11); - - EXPECT_NE(is31.find(topic2), is31.end()); - EXPECT_EQ(is31[topic2].size(), 2u); - EXPECT_NE(is31[topic2].find(time21), is31[topic2].end()); - EXPECT_NE(is31[topic2].find(time22), is31[topic2].end()); -} - -TEST(TestForwardEstimator, reverse_order2) -{ - // DAG is "A -> B -> C", - // but MessageTrackingTag comes C -> A -> B - auto fe = ForwardEstimator(); - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - // MessageTrackingTag of A - const auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // MessageTrackingTag of B - const auto time21 = get_time(21, 210); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - - // MessageTrackingTag of C - const auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - // add MessageTrackingTag in C -> A -> B order - fe.add(std::move(message_tracking_tag31)); - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - - auto is31 = fe.get_input_sources(topic3, time31); - EXPECT_EQ(is31.size(), 1u); - EXPECT_NE(is31.find(topic1), is31.end()); - EXPECT_EQ(is31[topic1].size(), 1u); - EXPECT_EQ(*is31[topic1].begin(), time11); -} - -TEST(TestForwardEstimator, reverse_order) -{ - // DAG is "A -> B -> C", - // but MessageTrackingTag comes C -> B -> A - auto fe = ForwardEstimator(); - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - // MessageTrackingTag of A - const auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // MessageTrackingTag of B - const auto time21 = get_time(21, 210); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - - // MessageTrackingTag of C - const auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - // add MessageTrackingTag in C -> B -> A order - fe.add(std::move(message_tracking_tag31)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag11)); - - auto is31 = fe.get_input_sources(topic3, time31); - EXPECT_EQ(is31.size(), 1u); - EXPECT_NE(is31.find(topic1), is31.end()); - EXPECT_EQ(is31[topic1].size(), 1u); - EXPECT_EQ(*is31[topic1].begin(), time11); -} - -TEST(TestForwardEstimator, reverse_order_with_merge) -{ - // DAG - // 1 --> 3 --> 4 - // / - // 2 -- - // - // MessageTrackingTag order: 4 -> 3 -> 1 -> 2 - - auto fe = ForwardEstimator(); - - const std::string topic1 = "topic1"; - auto time11 = get_time(11, 110); - const std::string topic2 = "topic2"; - auto time21 = get_time(21, 210); - const std::string topic3 = "topic3"; - auto time31 = get_time(31, 310); - const std::string topic4 = "topic4"; - auto time41 = get_time(41, 410); - - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - auto message_tracking_tag41 = create_message_tracking_tag(topic4, time41); - add_input_info(message_tracking_tag41.get(), message_tracking_tag31.get()); - - // 4 -> 3 -> 1 -> 2 - fe.add(std::move(message_tracking_tag41)); - fe.add(std::move(message_tracking_tag31)); - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - - auto is41 = fe.get_input_sources(topic4, time41); - EXPECT_EQ(is41.size(), 2u); -} - -TEST(TestForwardEstimator, expire_at_the_same_time) -{ - // DAG - // A -> B -> C - // MessageTrackingTag: A -> B -> C - // expire at the same time - auto fe = ForwardEstimator(); - - const std::string topic1 = "topic1"; - const std::string topic2 = "topic2"; - const std::string topic3 = "topic3"; - auto time = get_time(11, 110); - - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - EXPECT_EQ(fe.get_input_sources(topic3, time).size(), 1u); - - fe.delete_expired(time); - - EXPECT_EQ(fe.get_input_sources(topic3, time).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time).size(), 0u); -} - -TEST(TestForwardEstimator, expire_step_by_step) -{ - // DAG - // A -> B -> C - // MessageTrackingTag: A -> B -> C - // expire from C to A - auto fe = ForwardEstimator(); - - const std::string topic1 = "topic1"; - const std::string topic2 = "topic2"; - const std::string topic3 = "topic3"; - auto time11 = get_time(11, 110); - auto time21 = get_time(21, 210); - auto time31 = get_time(31, 310); - - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 1u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); - - fe.delete_expired(time11); - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); - - // ForwardEstimator internal data is maintained - fe.delete_expired(time21); - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); - - fe.delete_expired(time31); - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); -} - -TEST(TestForwardEstimator, expire_step_by_step_skew) -{ - // DAG - // A -> B -> C - // MessageTrackingTag: A -> B -> C - // expire from C to A - auto fe = ForwardEstimator(); - - const std::string topic1 = "topic1"; - const std::string topic2 = "topic2"; - const std::string topic3 = "topic3"; - // skew timestamp for controlling timeout - auto time11 = get_time(31, 310); - auto time21 = get_time(21, 210); - auto time31 = get_time(11, 110); - - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 1u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); - - fe.delete_expired(time31); - - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); - - fe.delete_expired(time21); - - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); - - fe.delete_expired(time11); - EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); - EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); -} - -TEST(TestForwardEstimator, get_oldest_sensor_stamp) -{ - // DAG - // A, B -> C - - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - auto time12 = get_time(12, 120); - auto message_tracking_tag12 = create_message_tracking_tag(topic1, time12); - - auto time21 = get_time(21, 110); - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); - auto time22 = get_time(22, 120); - auto message_tracking_tag22 = create_message_tracking_tag(topic2, time22); - - auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - - add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag12.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - add_input_info(message_tracking_tag31.get(), message_tracking_tag22.get()); - - auto fe = ForwardEstimator(); - fe.add(std::move(message_tracking_tag11)); - fe.add(std::move(message_tracking_tag21)); - fe.add(std::move(message_tracking_tag31)); - - auto oldest = fe.get_oldest_sensor_stamp(topic3, time31); - if (!oldest) { - FAIL() << "nil oldest"; - } else { - EXPECT_EQ(*oldest, rclcpp::Time(time11)); - } - - fe.delete_expired(time11); - - auto oldest_without_11 = fe.get_oldest_sensor_stamp(topic3, time31); - if (!oldest_without_11) { - FAIL() << "nil oldest_without_11"; - } else { - EXPECT_EQ(*oldest_without_11, rclcpp::Time(time21)); - } - - fe.delete_expired(time21); - - auto oldest_without_21 = fe.get_oldest_sensor_stamp(topic3, time31); - if (!oldest_without_21) { - SUCCEED(); - } else { - FAIL() << "zombie oldest_without_21"; - } -} - -TEST(TestForwardEstimator, skip_topic) -{ - // DAG - // A -> B -> C - // skip B - - const std::string topic1 = "topicA"; - const std::string topic2 = "topicB"; - const std::string topic3 = "topicC"; - - std::map skip_out_to_in; - skip_out_to_in[topic2] = topic1; - - auto time11 = get_time(11, 110); - auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); - - // skipped message should have the same timestamp as input message - auto message_tracking_tag21 = create_message_tracking_tag(topic2, time11); - add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); - - auto time31 = get_time(31, 310); - auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); - add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); - - auto fe = ForwardEstimator(); - fe.set_skip_out_to_in(skip_out_to_in); - - fe.add(std::move(message_tracking_tag11)); - // ignore 21 to verify skip - fe.add(std::move(message_tracking_tag31)); - - auto input_sources31 = fe.get_input_sources(topic3, time31); - - EXPECT_EQ(input_sources31.size(), 1u); - auto s = input_sources31.begin(); - EXPECT_EQ(s->first, topic1); - EXPECT_EQ(s->second.size(), 1u); - EXPECT_EQ(*s->second.begin(), time11); -} diff --git a/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp b/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp deleted file mode 100644 index 1ab7f7d9..00000000 --- a/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp +++ /dev/null @@ -1,275 +0,0 @@ -// 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 "builtin_interfaces/msg/time.hpp" -#include "rclcpp/rclcpp.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde_deadline_detector/tilde_deadline_detector_node.hpp" -#include "tilde_msg/msg/message_tracking_tag.hpp" -#include "tilde_msg/msg/sub_topic_time_info.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include - -#include -#include -#include -#include -#include -#include - -using tilde_deadline_detector::TildeDeadlineDetectorNode; - -class TestTildeDeadlineDetectorNode : public ::testing::Test -{ -public: - void SetUp() override { rclcpp::init(0, nullptr); } - - void TearDown() override { rclcpp::shutdown(); } -}; - -builtin_interfaces::msg::Time get_time(int sec, int nsec) -{ - builtin_interfaces::msg::Time t; - t.sec = sec; - t.nanosec = nsec; - return t; -} - -builtin_interfaces::msg::Duration get_duration(int32_t sec, uint32_t nsec) -{ - return rclcpp::Duration(sec, nsec); -} - -TEST_F(TestTildeDeadlineDetectorNode, get_message_tracking_tag_topics) -{ - auto tilde_node = tilde::TildeNode("tilde_node"); - auto pub = tilde_node.create_tilde_publisher("topic", 1); - auto pub2 = tilde_node.create_publisher("topic2", 1); - auto pub3 = - tilde_node.create_publisher("topic2/message_tracking_tag", 1); - - auto det_node = TildeDeadlineDetectorNode("node"); - auto topics = det_node.get_message_tracking_tag_topics(); - - EXPECT_EQ(topics.size(), 1u); - EXPECT_EQ(*topics.begin(), "/topic/message_tracking_tag"); -} - -tilde_msg::msg::MessageTrackingTag get_message_tracking_tag( - const std::string & topic, builtin_interfaces::msg::Time stamp, - builtin_interfaces::msg::Time pub_steady) -{ - tilde_msg::msg::MessageTrackingTag tag; - tag.output_info.topic_name = topic; - tag.output_info.pub_time_steady = pub_steady; - tag.output_info.has_header_stamp = true; - tag.output_info.header_stamp = stamp; - return tag; -} - -void add_input_info( - tilde_msg::msg::MessageTrackingTag & tag, const tilde_msg::msg::MessageTrackingTag & in_tag) -{ - tilde_msg::msg::SubTopicTimeInfo sub_info; - sub_info.topic_name = in_tag.output_info.topic_name; - sub_info.has_header_stamp = true; - sub_info.header_stamp = in_tag.output_info.header_stamp; - tag.input_infos.push_back(sub_info); -} - -TEST_F(TestTildeDeadlineDetectorNode, test_deadline_detection) -{ - auto tag_sender_node = std::make_shared("tag_sender_node"); - auto sensor_pub = tag_sender_node->create_publisher( - "sensor/message_tracking_tag", rclcpp::QoS(1).best_effort()); - auto next_pub = tag_sender_node->create_publisher( - "next/message_tracking_tag", rclcpp::QoS(1).best_effort()); - - rclcpp::NodeOptions options; - options.append_parameter_override("target_topics", std::vector{"next"}); - int deadline_ms = 10; - options.append_parameter_override("deadline_ms", std::vector{deadline_ms}); - - auto deadline_detector_node = - std::make_shared("deadline_detector_node", options); - - auto checker_node = std::make_shared("checker_node"); - bool notification_called = false; - auto checker_sub = checker_node->create_subscription( - "deadline_notification", rclcpp::QoS(1).best_effort(), - [this, ¬ification_called, - deadline_ms](tilde_msg::msg::DeadlineNotification::UniquePtr msg) -> void { - notification_called = true; - EXPECT_EQ(msg->topic_name, "next"); - EXPECT_EQ(msg->stamp, get_time(200, 0)); - EXPECT_EQ(msg->deadline_setting, get_duration(0, deadline_ms * 1000 * 1000)); - auto & sources = msg->sources; - EXPECT_EQ(sources.size(), 1u); - auto & s = sources[0]; - EXPECT_EQ(s.topic, "sensor"); - EXPECT_EQ(s.stamp, get_time(200, 0)); - EXPECT_EQ(s.elapsed, get_duration(0, deadline_ms * 1000 * 1000)); - EXPECT_TRUE(s.is_overrun); - }); - - auto spin = [tag_sender_node, deadline_detector_node, checker_node]() -> void { - rclcpp::spin_some(tag_sender_node); - rclcpp::spin_some(deadline_detector_node); - rclcpp::spin_some(checker_node); - }; - - // case1: not overrun - { - auto t1 = get_time(100, 0); - auto t2 = get_time(100, deadline_ms * 1000 * 1000 - 1); // ms to ns - tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor", t1, t1); - - tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); - add_input_info(next_tag, sensor_tag); - - sensor_pub->publish(sensor_tag); - spin(); - - next_pub->publish(next_tag); - spin(); - - EXPECT_FALSE(notification_called); - } - - // case2: overrun - { - auto t1 = get_time(200, 0); - auto t2 = get_time(200, deadline_ms * 1000 * 1000); // ms to ns - tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor", t1, t1); - - tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); - add_input_info(next_tag, sensor_tag); - - sensor_pub->publish(sensor_tag); - spin(); - - next_pub->publish(next_tag); - spin(); - - EXPECT_TRUE(notification_called); - - // we check deadline notification message in subscription callback - } -} - -TEST_F(TestTildeDeadlineDetectorNode, test_deadline_detection_multiple_input) -{ - auto tag_sender_node = std::make_shared("tag_sender_node"); - auto sensor1_pub = tag_sender_node->create_publisher( - "sensor1/message_tracking_tag", rclcpp::QoS(1).best_effort()); - auto sensor2_pub = tag_sender_node->create_publisher( - "sensor2/message_tracking_tag", rclcpp::QoS(1).best_effort()); - auto next_pub = tag_sender_node->create_publisher( - "next/message_tracking_tag", rclcpp::QoS(1).best_effort()); - - rclcpp::NodeOptions options; - options.append_parameter_override("target_topics", std::vector{"next"}); - int deadline_ms = 10; - options.append_parameter_override("deadline_ms", std::vector{deadline_ms}); - - auto deadline_detector_node = - std::make_shared("deadline_detector_node", options); - - auto checker_node = std::make_shared("checker_node"); - bool notification_called = false; - auto checker_sub = checker_node->create_subscription( - "deadline_notification", rclcpp::QoS(1).best_effort(), - [this, ¬ification_called, - deadline_ms](tilde_msg::msg::DeadlineNotification::UniquePtr msg) -> void { - notification_called = true; - EXPECT_EQ(msg->topic_name, "next"); - EXPECT_EQ(msg->stamp, get_time(200, 0)); - EXPECT_EQ(msg->deadline_setting, get_duration(0, deadline_ms * 1000 * 1000)); - - auto & sources = msg->sources; - EXPECT_EQ(sources.size(), 2u); - - size_t sensor1_idx = 0; - size_t sensor2_idx = 1; - if (sources[sensor1_idx].topic == "sensor2") { - sensor1_idx = 1; - sensor2_idx = 0; - } - - auto & s1 = sources[sensor1_idx]; - EXPECT_EQ(s1.topic, "sensor1"); - EXPECT_EQ(s1.stamp, get_time(200, 0)); - EXPECT_EQ(s1.elapsed, get_duration(0, deadline_ms * 1000 * 1000)); - EXPECT_TRUE(s1.is_overrun); - - auto & s2 = sources[sensor2_idx]; - EXPECT_EQ(s2.topic, "sensor2"); - EXPECT_EQ(s2.stamp, get_time(200, (deadline_ms / 2) * 1000 * 1000)); - EXPECT_EQ(s2.elapsed, get_duration(0, (deadline_ms / 2) * 1000 * 1000)); - EXPECT_FALSE(s2.is_overrun); - }); - - auto spin = [tag_sender_node, deadline_detector_node, checker_node]() -> void { - rclcpp::spin_some(tag_sender_node); - rclcpp::spin_some(deadline_detector_node); - rclcpp::spin_some(checker_node); - }; - - // case1: not overrun - { - auto t1 = get_time(100, 0); - auto t2 = get_time(100, deadline_ms * 1000 * 1000 - 1); // ms to ns - tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor1", t1, t1); - - tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); - add_input_info(next_tag, sensor_tag); - - sensor1_pub->publish(sensor_tag); - spin(); - - next_pub->publish(next_tag); - spin(); - - EXPECT_FALSE(notification_called); - } - - // case2: overrun. There are 2 inputs, and sensor1 is overrun. - { - auto t1 = get_time(200, 0); - auto t2 = get_time(200, (deadline_ms / 2) * 1000 * 1000); - auto t3 = get_time(200, deadline_ms * 1000 * 1000); - - tilde_msg::msg::MessageTrackingTag sensor1_tag = get_message_tracking_tag("sensor1", t1, t1); - tilde_msg::msg::MessageTrackingTag sensor2_tag = get_message_tracking_tag("sensor2", t2, t2); - - tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t3); - add_input_info(next_tag, sensor1_tag); - add_input_info(next_tag, sensor2_tag); - - sensor1_pub->publish(sensor1_tag); - spin(); - - sensor2_pub->publish(sensor2_tag); - spin(); - - next_pub->publish(next_tag); - spin(); - - EXPECT_TRUE(notification_called); - - // we check deadline notification message in subscription callback - } -} diff --git a/src/tilde_early_deadline_detector/CMakeLists.txt b/src/tilde_early_deadline_detector/CMakeLists.txt deleted file mode 100644 index c8b8740c..00000000 --- a/src/tilde_early_deadline_detector/CMakeLists.txt +++ /dev/null @@ -1,45 +0,0 @@ -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 deleted file mode 100644 index 998b13ac..00000000 --- a/src/tilde_early_deadline_detector/README.md +++ /dev/null @@ -1,74 +0,0 @@ -# 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 deleted file mode 100644 index 7d8af5c9..00000000 --- a/src/tilde_early_deadline_detector/autoware_sensors.yaml +++ /dev/null @@ -1,28 +0,0 @@ -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 deleted file mode 100644 index 2f1dc23b..00000000 --- a/src/tilde_early_deadline_detector/default_path.svg +++ /dev/null @@ -1 +0,0 @@ -/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 deleted file mode 100644 index e61f884e..00000000 --- a/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp +++ /dev/null @@ -1,140 +0,0 @@ -// 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 deleted file mode 100644 index 16e4ca1f..00000000 --- a/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp +++ /dev/null @@ -1,156 +0,0 @@ -// 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 deleted file mode 100644 index aa9b70f7..00000000 --- a/src/tilde_early_deadline_detector/package.xml +++ /dev/null @@ -1,25 +0,0 @@ - - - - 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 deleted file mode 100644 index 71948c6a..00000000 --- a/src/tilde_early_deadline_detector/src/forward_estimator.cpp +++ /dev/null @@ -1,296 +0,0 @@ -// 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 deleted file mode 100644 index c71ae5b6..00000000 --- a/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp +++ /dev/null @@ -1,531 +0,0 @@ -// 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 -#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 != 0 || fp != 0 || fn != 0) { - accuracy = (tp + tn) / (tp + tn + fp + fn); - precision = tp / (tp + fp); - recall = tp / (tp + fn); - if (precision != 0 || 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_message_filters/CMakeLists.txt b/src/tilde_message_filters/CMakeLists.txt deleted file mode 100644 index 109d2c99..00000000 --- a/src/tilde_message_filters/CMakeLists.txt +++ /dev/null @@ -1,145 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_message_filters) - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(tilde) -find_package(message_filters) -find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -include_directories(include) -ament_export_include_directories(include) -install( - DIRECTORY include/ - DESTINATION include) - -add_library(sample_tilde_message_filter SHARED - src/sample_tilde_subscriber.cpp - src/sample_tilde_synchronizer.cpp - src/sample_sync_publisher.cpp) -ament_target_dependencies(sample_tilde_message_filter - "tilde" - "message_filters" - "rclcpp_components" - "std_msgs" - "sensor_msgs") -rclcpp_components_register_node(sample_tilde_message_filter - PLUGIN "sample_tilde_message_filter::SampleSubscriberWithHeader" - EXECUTABLE subscriber_with_header) -rclcpp_components_register_node(sample_tilde_message_filter - PLUGIN "sample_tilde_message_filter::SampleTildeSynchronizer2" - EXECUTABLE sample_synchronizer2) -rclcpp_components_register_node(sample_tilde_message_filter - PLUGIN "sample_tilde_message_filter::SampleTildeSynchronizer3" - EXECUTABLE sample_synchronizer3) -rclcpp_components_register_node(sample_tilde_message_filter - PLUGIN "sample_tilde_message_filter::SamplePublisherWithHeader" - EXECUTABLE publisher_with_header) - -install(TARGETS - sample_tilde_message_filter) -install( - TARGETS sample_tilde_message_filter EXPORT sample_tilde_message_filter - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) - -if(BUILD_TESTING) - find_package(tilde) - find_package(message_filters REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_lint_auto REQUIRED) - find_package(sensor_msgs REQUIRED) - find_package(std_msgs REQUIRED) - - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - # set(ament_cmake_cpplint_FOUND TRUE) - list(APPEND AMENT_LINT_AUTO_EXCLUDE - ament_cmake_cpplint - ament_cmake_cppcheck - ) - ament_lint_auto_find_test_dependencies() - - # we skip cpplint of borrowed files - find_package(ament_cmake_cpplint) - ament_cpplint(EXCLUDE - "test/test_from_original_subscriber.cpp" - "test/test_from_original_synchronizer.cpp") - - # TODO(y-okumura-isp): syntaxError but success to build & UT - find_package(ament_cmake_cppcheck) - ament_cppcheck(EXCLUDE - "tilde_subscriber.hpp" - ) - - ament_add_gtest(test_tilde_subscriber - test/test_tilde_subscriber.cpp) - target_include_directories(test_tilde_subscriber - PUBLIC - $ - $) - ament_target_dependencies(test_tilde_subscriber - "tilde" - "message_filters" - "sensor_msgs" - "std_msgs") - - ament_add_gtest(test_tilde_synchronizer - test/test_tilde_synchronizer.cpp) - target_include_directories(test_tilde_synchronizer - PUBLIC - $ - $) - ament_target_dependencies(test_tilde_synchronizer - "tilde" - "message_filters" - "sensor_msgs" - "std_msgs") - - ament_add_gtest(test_from_original_subscriber - test/test_from_original_subscriber.cpp) - target_include_directories(test_from_original_subscriber - PUBLIC - $ - $) - ament_target_dependencies(test_from_original_subscriber - "tilde" - "message_filters" - "rclcpp_lifecycle" - "sensor_msgs" - "std_msgs") - - ament_add_gtest(test_from_original_synchronizer - test/test_from_original_synchronizer.cpp) - target_include_directories(test_from_original_synchronizer - PUBLIC - $ - $) - ament_target_dependencies(test_from_original_synchronizer - "tilde" - "message_filters" - "sensor_msgs" - "std_msgs") -endif() - -ament_package() diff --git a/src/tilde_message_filters/README.md b/src/tilde_message_filters/README.md deleted file mode 100644 index ee902df3..00000000 --- a/src/tilde_message_filters/README.md +++ /dev/null @@ -1,51 +0,0 @@ -## TODO - -- [ ] consider pointer from TildeSynchronizer or TildeSubscriber to TildeNode - - originally, it is not required - - shared_ptr could be cyclic pointers - - now raw pointer is used (weak_pointer may be a good replacement) - -## UT - -### compatibility test - -To check compatibility, we use the following tests: - -- test_from_original_subscriber.cpp -- test_from_original_synchronizer.cpp - -See bellow to know how to maintenance. - -#### test_subscriber.cpp - -- Manually copy & rename corresponding files -- Prepare TildeSubscriber - - add `#include "tilde_message_filters/tilde_subscriber.hpp"` - - add `using namespace tilde_message_filters;` - - replace `Subscriber` to `TildeSubscriber` -- Use `tilde::TildeNode` instead of `rclcpp::Node` - - add `#include "tilde/tilde_node.hpp"` - - replace `rclcpp::Node` to `tilde::TildeNode` -- Comment tests using LifecycleNode - - `TEST(TildeSubscriber, lifecycle)` on 20220421 - -#### test_synchronizer.cpp - -- Manually copy & rename corresponding files -- Prepare TildeSynchronizer - - - add `#include "tilde_message_filters/tilde_synchronizer.hpp"` - - add `using namespace tilde_message_filters;` - - replace `Synchronizer` to `TildeSynchronizer` of TEST - - don't change `typedef Synchronizer Sync;` in `struct NullPolicy` - - Add `TildeNode` to TildeSynchronizer constructors - - `nullptr` may work - - non-null instance is needed for message handling tests. - (add2, add3, and so on) - If null, the results are unexpected. - -- Use `tilde::TildeNode` instead of `rclcpp::Node` - - add `#include "tilde/tilde_node.hpp"` - - replace `rclcpp::Node` to `tilde::TildeNode` -- Comment tests using LifecycleNode - - `TEST(TildeSubscriber, lifecycle)` on 20220421 diff --git a/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp b/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp deleted file mode 100644 index 0209b7d0..00000000 --- a/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp +++ /dev/null @@ -1,290 +0,0 @@ -// 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_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ -#define TILDE_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ - -#include "message_filters/connection.h" -#include "message_filters/subscriber.h" -#include "tilde/tilde_node.hpp" - -#include -#include -#include - -namespace tilde_message_filters -{ -template -inline constexpr bool always_false_v = false; - -template -inline constexpr bool is_subscription_message() -{ - using ConstRef = const MessageT &; - using UniquePtr = std::unique_ptr; - using SharedConstPtr = std::shared_ptr; - using ConstRefSharedConstPtr = const std::shared_ptr &; - using SharedPtr = std::shared_ptr; - - return std::is_same_v || std::is_same_v || - std::is_same_v || std::is_same_v || - std::is_same_v; -} - -template -class TildeSubscriber -{ - using NodePtr = std::shared_ptr; - using MConstPtr = std::shared_ptr; - using EventType = message_filters::MessageEvent; - -public: - TildeSubscriber() = default; - - // cppcheck syntaxError - TildeSubscriber( - NodeType * node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node, topic, qos); - } - - TildeSubscriber( - NodePtr node, const std::string & topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node, topic, qos); - } - - void subscribe( - NodePtr node, const std::string & topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - subscribe(node.get(), topic, qos); - node_raw_ = nullptr; - node_shared_ = node; - } - - void subscribe( - NodeType * node, const std::string & topic, - const rmw_qos_profile_t qos = rmw_qos_profile_default) - { - node_raw_ = node; - subscriber_.subscribe(node, topic, qos); - subscriber_.registerCallback(&TildeSubscriber::register_message_callback, this); - } - - void subscribe() { subscriber_.subscribe(); } - - void unsubscribe() { subscriber_.unsubscribe(); } - - /// get non-FQDN topic name as in message_filter::Subscriber, - std::string getTopic() const { return subscriber_.getTopic(); } - - /// \sa message_filters::Subscriber::add() - void add(const EventType & e) { (void)e; } - - /// \sa message_filters::Subscriber::connectInput() - template - void connectInput(F & f) - { - (void)f; - } - - /* - // original callbacks - template - message_filters::Connection registerCallback(const C& callback) - { - return subscriber_.registerCallback(callback); - } - - template - message_filters::Connection registerCallback(const std::function& callback) - { - return subscriber_.registerCallback(callback); - } - - template - message_filters::Connection registerCallback(void(*callback)(P)) - { - return subscriber_.registerCallback(callback); - } - - template - message_filters::Connection registerCallback(void(T::*callback)(P), T* t) - { - return subscriber_.registerCallback(callback, t); - } - */ - - template < - typename C, typename CallbackArgT = - typename rclcpp::function_traits::function_traits::template argument_type<0>> - message_filters::Connection registerCallback(const C & callback) - { - const auto topic = getTopicFQDN(); - - auto new_callback = [this, callback, topic](CallbackArgT msg) -> void { - auto pnode = get_node(); - - // msg.get() in below codes: - // we can use msg.get() because - // - message_filters::simple_filter assumes that - // typename C should be compatible with std::function - // - MConstPtr is std::shared_ptr - - // on TildeSubscriber, default callback saves sub_time - rclcpp::Time sub_time, sub_time_steady; - pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); - - // update implicit input info - pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - - callback(msg); - }; - return subscriber_.registerCallback(new_callback); - } - - template > - message_filters::Connection registerCallback(const std::function & callback) - { - // We may use SFINAE instead of if constexpr to shorten this function. - // But SFINAE may change the entry point. On the other hand, `if constexpr` does not change it. - if constexpr (!is_subscription_message()) { - return subscriber_.registerCallback(callback); - } else { - const auto topic = getTopicFQDN(); - // As std::function use vtable, we use auto type, - // not std::function new_callback = ... - // https://stackoverflow.com/questions/25848690/should-i-use-stdfunction-or-a-function-pointer-in-c - auto new_callback = [this, callback, topic](P msg) -> void { - auto pnode = get_node(); - - // we use msg.get() because - // even in this pattern, - // P looks to be const shared_ptr (see message_filters::MessageEvent). - // TODO(y-okumura-isp): Is ConstRef also possible?? - - // on TildeSubscriber, default callback saves sub_time - rclcpp::Time sub_time, sub_time_steady; - pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); - - // update implicit input info - pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - - callback(msg); - }; - return subscriber_.registerCallback(new_callback); - } - } - - template - message_filters::Connection registerCallback(void (*callback)(P)) - { - const auto topic = getTopicFQDN(); - // We use lambda not original function pointer - // because we cannot convert captured lambda to function pointer. - // https://stackoverflow.com/questions/28746744/passing-capturing-lambda-as-function-pointer - auto new_callback = [this, callback, topic](P msg) -> void { - auto pnode = get_node(); - - // we use msg.get() because - // even in this pattern, - // P looks to be const shared_ptr (see message_filters::MessageEvent). - // TODO(y-okumura-isp): Is ConstRef also possible?? - - // on TildeSubscriber, default callback saves sub_time - rclcpp::Time sub_time, sub_time_steady; - pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); - - // update implicit input info - pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - - callback(msg); - }; - return subscriber_.registerCallback(new_callback); - } - - template - message_filters::Connection registerCallback(void (T::*callback)(P), T * t) - { - const auto topic = getTopicFQDN(); - auto bind_callback = std::bind(callback, t, std::placeholders::_1); - auto new_callback = [this, bind_callback, topic](P msg) -> void { - auto pnode = get_node(); - - // we use msg.get() because - // even in this pattern, - // P looks to be const shared_ptr (see message_filters::MessageEvent). - // TODO(y-okumura-isp): Can the msg type be ConstRef? - - // on TildeSubscriber, default callback saves sub_time - rclcpp::Time sub_time, sub_time_steady; - pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); - - // update implicit input info - pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - - bind_callback(msg); - }; - // We need to wrap new_callback because P is sometimes non MConstPtr. - // If P is non MConstPtr, `registerCallback(const C& callback)` is called and - // subsequent `signal_.addCallback(Callback(callback))` fails because - // Callback = std::function. - // So we wrap new_callback by std::function, and use - // `registerCallback(const std::function& callback)`. - std::function new_callback2 = std::bind(new_callback, std::placeholders::_1); - return subscriber_.registerCallback(new_callback2); - } - -private: - NodePtr node_shared_; - NodeType * node_raw_{nullptr}; - message_filters::Subscriber subscriber_; - - NodeType * get_node() - { - if (node_shared_) { - return node_shared_.get(); - } else { - return node_raw_; - } - } - - /// get topic FQDN - std::string getTopicFQDN() - { - auto topic = subscriber_.getTopic(); - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(get_node()); - auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic); - return resolved_topic_name; - } - - /// TildeSubscriber 1st callback - /** - * Save sub_time for other callbacks or TildeSynchronizer - */ - void register_message_callback(const MConstPtr msg) - { - auto topic = getTopicFQDN(); - auto pnode = get_node(); - auto sub_time = pnode->now(); - auto sub_time_steady = pnode->get_steady_time(); - - pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - } -}; -} // namespace tilde_message_filters - -#endif // TILDE_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ diff --git a/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp b/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp deleted file mode 100644 index 50ff3ee4..00000000 --- a/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp +++ /dev/null @@ -1,1191 +0,0 @@ -// 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_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ -#define TILDE_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ - -#include "message_filters/subscriber.h" -#include "message_filters/time_synchronizer.h" -#include "tilde/tilde_node.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" - -#include -#include -#include - -namespace tilde_message_filters -{ - -template -constexpr bool true_v = true; - -template -constexpr bool false_v = false; - -template -constexpr bool is_subscriber() -{ - if constexpr (std::is_same_v>) { - return true_v; - } - return false_v; -} - -template -class TildeSynchronizer -{ - using Sync = message_filters::Synchronizer; - - typedef typename Policy::Messages Messages; - - typedef typename std::tuple_element<0, Messages>::type M0; - typedef typename std::tuple_element<1, Messages>::type M1; - typedef typename std::tuple_element<2, Messages>::type M2; - typedef typename std::tuple_element<3, Messages>::type M3; - typedef typename std::tuple_element<4, Messages>::type M4; - typedef typename std::tuple_element<5, Messages>::type M5; - typedef typename std::tuple_element<6, Messages>::type M6; - typedef typename std::tuple_element<7, Messages>::type M7; - typedef typename std::tuple_element<8, Messages>::type M8; - -public: - template - TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1) : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - } - - template - TildeSynchronizer(tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1) : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - } - - template - TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2) : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - } - - template - TildeSynchronizer(tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - } - - template - TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3) : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - } - - template - TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - } - - template - TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) - : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, - F5 & f5) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) - : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, - F5 & f5, F6 & f6) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) - : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6, f7); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - init_topic_name<7, M7>(f7); - } - - template - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, - F5 & f5, F6 & f6, F7 & f7) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6, f7); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - init_topic_name<7, M7>(f7); - } - - template < - class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> - TildeSynchronizer( - tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, - F8 & f8) - : node_(node) - { - sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6, f7, f8); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - init_topic_name<7, M7>(f7); - init_topic_name<8, M8>(f8); - } - - template < - class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> - TildeSynchronizer( - tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, - F5 & f5, F6 & f6, F7 & f7, F8 & f8) - : node_(node) - { - sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6, f7, f8); - init_topic_name<0, M0>(f0); - init_topic_name<1, M1>(f1); - init_topic_name<2, M2>(f2); - init_topic_name<3, M3>(f3); - init_topic_name<4, M4>(f4); - init_topic_name<5, M5>(f5); - init_topic_name<6, M6>(f6); - init_topic_name<7, M7>(f7); - init_topic_name<8, M8>(f8); - } - - explicit TildeSynchronizer(tilde::TildeNode * node) : node_(node) - { - sync_ptr_ = std::make_shared(); - } - - TildeSynchronizer(tilde::TildeNode * node, const Policy & policy) : node_(node) - { - sync_ptr_ = std::make_shared(policy); - } - - // (const C& callback) - template < - class C, std::size_t Arity = 2, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback](CallbackArgT0 msg0, CallbackArgT1 msg1) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - - callback(msg0, msg1); - }; - - return sync_ptr_->registerCallback( - std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); - } - - template < - class C, std::size_t Arity = 3, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - - callback(msg0, msg1, msg2); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } - - template < - class C, std::size_t Arity = 4, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - - callback(msg0, msg1, msg2, msg3); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4)); - } - - template < - class C, std::size_t Arity = 5, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - - callback(msg0, msg1, msg2, msg3, msg4); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5)); - } - - template < - class C, std::size_t Arity = 6, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - - callback(msg0, msg1, msg2, msg3, msg4, msg5); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); - } - - template < - class C, std::size_t Arity = 7, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); - } - - template < - class C, std::size_t Arity = 8, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8)); - } - - template < - class C, std::size_t Arity = 9, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>, - typename CallbackArgT8 = - typename rclcpp::function_traits::function_traits::template argument_type<8>> - message_filters::Connection registerCallback(const C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7, CallbackArgT8 msg8) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - register_ith_message_as_input<8>(msg8); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9)); - } - - // non-const C callback& - template < - class C, std::size_t Arity = 2, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback](CallbackArgT0 msg0, CallbackArgT1 msg1) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - - callback(msg0, msg1); - }; - - return sync_ptr_->registerCallback( - std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); - } - - template < - class C, std::size_t Arity = 3, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - - callback(msg0, msg1, msg2); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } - - template < - class C, std::size_t Arity = 4, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - - callback(msg0, msg1, msg2, msg3); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4)); - } - - template < - class C, std::size_t Arity = 5, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - - callback(msg0, msg1, msg2, msg3, msg4); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5)); - } - - template < - class C, std::size_t Arity = 6, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - - callback(msg0, msg1, msg2, msg3, msg4, msg5); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); - } - - template < - class C, std::size_t Arity = 7, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); - } - - template < - class C, std::size_t Arity = 8, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8)); - } - - template < - class C, std::size_t Arity = 9, - typename std::enable_if::value>::type * = - nullptr, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>, - typename CallbackArgT8 = - typename rclcpp::function_traits::function_traits::template argument_type<8>> - message_filters::Connection registerCallback(C & callback) - { - auto new_callback_lambda = [this, callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7, CallbackArgT8 msg8) { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - register_ith_message_as_input<8>(msg8); - - callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); - }; - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9)); - } - - // (const C& callback, T* t) - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 2, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind(callback, t, std::placeholders::_1, std::placeholders::_2); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - - bind_callback(msg0, msg1); - }; - - return sync_ptr_->registerCallback( - std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 3, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = - std::bind(callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); - auto new_callback_lambda = - [this, bind_callback](CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - - bind_callback(msg0, msg1, msg2); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 4, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - - bind_callback(msg0, msg1, msg2, msg3); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 5, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - - bind_callback(msg0, msg1, msg2, msg3, msg4); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 6, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, - CallbackArgT5 msg5) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - - bind_callback(msg0, msg1, msg2, msg3, msg4, msg5); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 7, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - - bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 8, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - - bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8)); - } - - template < - typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), - std::size_t Arity = 9, typename std::enable_if::type = true, - typename CallbackArgT0 = - typename rclcpp::function_traits::function_traits::template argument_type<0>, - typename CallbackArgT1 = - typename rclcpp::function_traits::function_traits::template argument_type<1>, - typename CallbackArgT2 = - typename rclcpp::function_traits::function_traits::template argument_type<2>, - typename CallbackArgT3 = - typename rclcpp::function_traits::function_traits::template argument_type<3>, - typename CallbackArgT4 = - typename rclcpp::function_traits::function_traits::template argument_type<4>, - typename CallbackArgT5 = - typename rclcpp::function_traits::function_traits::template argument_type<5>, - typename CallbackArgT6 = - typename rclcpp::function_traits::function_traits::template argument_type<6>, - typename CallbackArgT7 = - typename rclcpp::function_traits::function_traits::template argument_type<7>, - typename CallbackArgT8 = - typename rclcpp::function_traits::function_traits::template argument_type<8>> - message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) - { - auto bind_callback = std::bind( - callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9); - auto new_callback_lambda = [this, bind_callback]( - CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, - CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, - CallbackArgT6 msg6, CallbackArgT7 msg7, - CallbackArgT8 msg8) -> void { - register_ith_message_as_input<0>(msg0); - register_ith_message_as_input<1>(msg1); - register_ith_message_as_input<2>(msg2); - register_ith_message_as_input<3>(msg3); - register_ith_message_as_input<4>(msg4); - register_ith_message_as_input<5>(msg5); - register_ith_message_as_input<6>(msg6); - register_ith_message_as_input<7>(msg7); - register_ith_message_as_input<8>(msg8); - - bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); - }; - - return sync_ptr_->registerCallback(std::bind( - new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, - std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, - std::placeholders::_8, std::placeholders::_9)); - } - - // (C& callback, T* t) - // TODO(y-okumura-isp): implement me - -private: - std::shared_ptr sync_ptr_; - tilde::TildeNode * node_; - // message id vs topic name. - // 9 means the number of max messages. - std::vector topic_names_{9}; - - // helper function for topic name initialization. - // I: the index of message - // F: filter class such as Subscriber - // M: message type such as PointCloud2 - template - void init_topic_name(F & f) - { - // if constexpr (std::is_same_v>) { - if constexpr (std::is_same_v>) { - using rclcpp::node_interfaces::get_node_topics_interface; - auto node_topics_interface = get_node_topics_interface(node_); - auto topic_name = f.getTopic(); - auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); - // std::cout << "topic_name[ " << I << "]: "<< resolved_topic_name << std::endl; - topic_names_[I] = resolved_topic_name; - } - } - - // helper function for node->register_message_as_input - template - void register_ith_message_as_input(CallbackArgT msg) - { - static_assert(I < 9); - - using MessageT = typename std::tuple_element::type; - // TODO(y-okumura-isp): support custom deleter - using MessageDeleter = std::default_delete; - using ConstRef = const MessageT &; - using UniquePtr = std::unique_ptr; - using SharedConstPtr = std::shared_ptr; - using ConstRefSharedConstPtr = const std::shared_ptr &; - using SharedPtr = std::shared_ptr; - - const auto & topic = topic_names_[I]; - if (topic.empty()) { - return; - } - - rclcpp::Time sub_time, sub_time_steady; - if constexpr ( - std::is_same_v || std::is_same_v || - std::is_same_v || - std::is_same_v) { - node_->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); - - // update implicit input info - node_->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); - } else if constexpr (std::is_same_v) { - node_->find_subscription_time(&msg, topic, sub_time, sub_time_steady); - - // update implicit input info - node_->register_message_as_input(&msg, topic, sub_time, sub_time_steady); - } else { - // todo(y-okumura-isp): implement me - } - } -}; -} // namespace tilde_message_filters - -#endif // TILDE_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ diff --git a/src/tilde_message_filters/package.xml b/src/tilde_message_filters/package.xml deleted file mode 100644 index 5b524c7c..00000000 --- a/src/tilde_message_filters/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - tilde_message_filters - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - ament_cmake - - message_filters - rclcpp_lifecycle - sensor_msgs - std_msgs - tilde - - ament_lint_auto - caret_lint_common - - - ament_cmake - - diff --git a/src/tilde_message_filters/src/sample_sync_publisher.cpp b/src/tilde_message_filters/src/sample_sync_publisher.cpp deleted file mode 100644 index c07c2245..00000000 --- a/src/tilde_message_filters/src/sample_sync_publisher.cpp +++ /dev/null @@ -1,150 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -const int64_t TIMER_MS_DEFAULT = 1000; - -namespace sample_tilde_message_filter -{ - -/** - * This class sends messages with header.stamp field. - * see msg.header.frame_id to get the sequence number. - */ -class SamplePublisherWithHeader : public rclcpp::Node -{ - using PublisherPtr = rclcpp::Publisher::SharedPtr; - -public: - explicit SamplePublisherWithHeader(const rclcpp::NodeOptions & options) - : Node("talker_with_header", options) - { - const std::string TIMER_MS = "timer_ms"; - const std::string N_PUBLISHERS = "n_publishers"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - - declare_parameter(N_PUBLISHERS, n_publishers_); - n_publishers_ = get_parameter(N_PUBLISHERS).get_value(); - assert(n_publishers_ >= 2); - - // Create a publisher with a custom Quality of Service profile. - rclcpp::QoS qos(rclcpp::KeepLast(7)); - for (auto i = 0; i < n_publishers_; i++) { - std::string topic = "in"; - topic += std::to_string(i); - pub_pcs_.push_back(this->create_publisher(topic, qos)); - } - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto publish_message = [this]() -> void { - auto time_now = this->now(); - - sensor_msgs::msg::PointCloud2 msg_pc; - msg_pc.header.stamp = time_now; - msg_pc.header.frame_id = std::to_string(count_); - - for (const auto & pub : pub_pcs_) { - pub->publish(msg_pc); - } - - RCLCPP_INFO( - this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, - time_now.nanoseconds()); - - count_++; - }; - - // Use a timer to schedule periodic message publishing. - auto dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(dur, publish_message); - } - -private: - size_t count_ = 0; - int64_t n_publishers_ = 9; - std::vector pub_pcs_; - - rclcpp::TimerBase::SharedPtr timer_; -}; - -/** - * This class sends messages without header.stamp field. - */ -class SamplePublisherWithoutHeader : public rclcpp::Node -{ -public: - explicit SamplePublisherWithoutHeader(const rclcpp::NodeOptions & options) - : Node("talker_without_header", options) - { - const std::string TIMER_MS = "timer_ms"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto publish_message = [this]() -> void { - auto time_now = this->now(); - - msg_string_ = std::make_unique(); - msg_string_->data = std::to_string(count_); - pub_string_->publish(std::move(msg_string_)); - - RCLCPP_INFO( - this->get_logger(), "Publishing String: '%ld' at '%lu'", count_, time_now.nanoseconds()); - - count_++; - }; - - // Create a publisher with a custom Quality of Service profile. - rclcpp::QoS qos(rclcpp::KeepLast(7)); - pub_string_ = this->create_publisher("topic_without_header", qos); - - // Use a timer to schedule periodic message publishing. - auto dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(dur, publish_message); - } - -private: - size_t count_ = 0; - - // message without standard header, especially header.stamp - std::unique_ptr msg_string_; - rclcpp::Publisher::SharedPtr pub_string_; - - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace sample_tilde_message_filter - -RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SamplePublisherWithHeader) -RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SamplePublisherWithoutHeader) diff --git a/src/tilde_message_filters/src/sample_tilde_subscriber.cpp b/src/tilde_message_filters/src/sample_tilde_subscriber.cpp deleted file mode 100644 index 5926398c..00000000 --- a/src/tilde_message_filters/src/sample_tilde_subscriber.cpp +++ /dev/null @@ -1,164 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -typedef sensor_msgs::msg::PointCloud2 Msg; -typedef std::shared_ptr MsgConstPtr; -typedef std::shared_ptr MsgPtr; - -namespace sample_tilde_message_filter -{ -struct NonConstHelper -{ - explicit NonConstHelper(std::shared_ptr> pub) : pub_(pub) {} - - void cb(const MsgPtr msg) { pub_->publish(*msg); } - - MsgPtr msg_; - std::shared_ptr> pub_; -}; - -std::shared_ptr> g_pub_callback_fn_; - -void callback_fn(MsgConstPtr msg) { g_pub_callback_fn_->publish(*msg); } - -template -void func(CallbackT && callback) // add [[deprecated]] to show deduced type -{ - auto callback_addr = &callback; -} - -// Create a Talker class that subclasses the generic rclcpp::Node base class. -// The main function below will instantiate the class as a ROS node. -class SampleSubscriberWithHeader : public tilde::TildeNode -{ -public: - explicit SampleSubscriberWithHeader(const rclcpp::NodeOptions & options) - : TildeNode("sub_with_header", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - - sub_pc_.subscribe(this, "in1", qos.get_rmw_qos_profile()); - - // lambda lvalue - // registerCallback(const C& callback) with C = sample_tilde_message_filter::..::(lambda)... - pub_lambda_lvalue_ = create_tilde_publisher("out_lambda_lvalue", 1); - auto sub_callback = [this](MsgConstPtr msg) -> void { - RCLCPP_INFO(this->get_logger(), "sub_callback"); - // use reference because rclcpp::Publisher::publish does not have shared_ptr version - pub_lambda_lvalue_->publish(*msg); - }; - sub_pc_.registerCallback(sub_callback); - - // lambda rvalue - // const C& callback (be aware `C& callback` not defined) - pub_lambda_rvalue_ = create_tilde_publisher("out_lambda_rvalue", 1); - sub_pc_.registerCallback([this](MsgConstPtr msg) -> void { - RCLCPP_INFO(this->get_logger(), "rvalue lambda"); - pub_lambda_rvalue_->publish(*msg); - }); - - /* type check - std::cout << &sub_callback << std::endl; - func(sub_callback); - func([this](MsgConstPtr msg) -> void - { - (void) msg; - RCLCPP_INFO(this->get_logger(), "rvalue lambda"); - }); - func([this](MsgConstPtr msg) -> void - { - (void) msg; - RCLCPP_INFO(this->get_logger(), "rvalue lambda"); - }); - */ - - // std::function - // registerCallback(const std::function& callback) - pub_callback2_ = create_tilde_publisher("out_std_function_lvalue", 1); - std::function std_func = - std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1); - sub_pc_.registerCallback(std_func); - - /* - * We can not use unique_ptr because message_event.h does not accept - * registerCallback(const std::function& callback) - */ - // std::function)> std_func_unique_ptr = - // std::bind(&SampleSubscriberWithHeader::callback_unique_ptr, this, std::placeholders::_1); - // sub_pc_.registerCallback(std_func_unique_ptr); - - // std::bind rvalue - // const C& callback - sub_pc_.registerCallback( - std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1)); - - // std::bind lvalue but use auto - // const C& callback. => with C = std::_Bind - auto auto_func = std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1); - sub_pc_.registerCallback(auto_func); - - // registerCallback(void(*callback)(P)) - // void(*callback)(P) - g_pub_callback_fn_ = create_tilde_publisher("out_callback_fn", 1); - sub_pc_.registerCallback(callback_fn); - - // registerCallback(void(T::*callback)(P), T* t) - pub_non_const_helper_ = create_tilde_publisher("out_non_const_helper", 1); - non_const_helper_ = std::make_shared(pub_non_const_helper_); - sub_pc_.registerCallback(&NonConstHelper::cb, non_const_helper_.get()); - } - -private: - tilde_message_filters::TildeSubscriber sub_pc_; - std::shared_ptr> pub_lambda_lvalue_; - std::shared_ptr> pub_lambda_rvalue_; - std::shared_ptr> pub_std_function_lvalue; - std::shared_ptr> pub_callback2_; - std::shared_ptr> pub_non_const_helper_; - std::shared_ptr non_const_helper_; - - void callback2(MsgConstPtr msg) - { - RCLCPP_INFO(this->get_logger(), "callback2"); - pub_callback2_->publish(*msg); - } - - void callback_unique_ptr(std::unique_ptr msg) - { - RCLCPP_INFO(this->get_logger(), "callback_unique_ptr"); - pub_callback2_->publish(std::move(msg)); - } - - MsgPtr msg_; -}; - -} // namespace sample_tilde_message_filter - -RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleSubscriberWithHeader) diff --git a/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp b/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp deleted file mode 100644 index e35a1e3f..00000000 --- a/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp +++ /dev/null @@ -1,114 +0,0 @@ -// 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 "message_filters/sync_policies/exact_time.h" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" -#include "tilde_message_filters/tilde_synchronizer.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include - -typedef sensor_msgs::msg::PointCloud2 Msg; -typedef std::shared_ptr MsgConstPtr; -typedef std::shared_ptr MsgPtr; - -namespace sample_tilde_message_filter -{ -// 2 means the callback argc to simplify the template programming -class SampleTildeSynchronizer2 : public tilde::TildeNode -{ - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = tilde_message_filters::TildeSynchronizer; - using Subscriber = message_filters::Subscriber; - using Publisher = tilde::TildePublisher::SharedPtr; - -public: - explicit SampleTildeSynchronizer2(const rclcpp::NodeOptions & options) - : TildeNode("sample_tilde_sync2", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - auto rmw_qos = qos.get_rmw_qos_profile(); - - sub_pc1_.subscribe(this, "in1", rmw_qos); - sub_pc2_.subscribe(this, "in2", rmw_qos); - - sync_ptr_ = std::make_shared(this, SyncPolicy(5), sub_pc1_, sub_pc2_); - - pub_ = create_tilde_publisher("out2", 1); - - // registerCallback(const C& callback) version: - // <- (const C&) can bind rvalue - sync_ptr_->registerCallback(std::bind( - &SampleTildeSynchronizer2::sub_callback, this, std::placeholders::_1, std::placeholders::_2)); - } - -private: - Subscriber sub_pc1_, sub_pc2_; - std::shared_ptr sync_ptr_; - Publisher pub_; - - void sub_callback(const MsgConstPtr & msg1, const MsgConstPtr & msg2) - { - (void)msg2; - pub_->publish(*msg1); - } -}; - -// 3 means the callback argc to simplify the template programming -class SampleTildeSynchronizer3 : public tilde::TildeNode -{ - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = tilde_message_filters::TildeSynchronizer; - using Subscriber = message_filters::Subscriber; - -public: - explicit SampleTildeSynchronizer3(const rclcpp::NodeOptions & options) - : TildeNode("sample_tilde_sync3", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - auto rmw_qos = qos.get_rmw_qos_profile(); - - sub_pc1_.subscribe(this, "in1", rmw_qos); - sub_pc2_.subscribe(this, "in2", rmw_qos); - sub_pc3_.subscribe(this, "in3", rmw_qos); - - sync_ptr_ = std::make_shared(this, SyncPolicy(5), sub_pc1_, sub_pc2_, sub_pc3_); - - // registerCallback(const C& callback) version: - // <- (const C&) can bind rvalue - sync_ptr_->registerCallback(std::bind( - &SampleTildeSynchronizer3::sub_callback, this, std::placeholders::_1, std::placeholders::_2, - std::placeholders::_3)); - } - -private: - Subscriber sub_pc1_, sub_pc2_, sub_pc3_; - std::shared_ptr sync_ptr_; - - void sub_callback(const MsgConstPtr & msg1, const MsgConstPtr & msg2, const MsgConstPtr & msg3) - { - (void)msg1; - (void)msg2; - (void)msg3; - } -}; - -} // namespace sample_tilde_message_filter - -RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleTildeSynchronizer2) -RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleTildeSynchronizer3) diff --git a/src/tilde_message_filters/test/test_from_original_subscriber.cpp b/src/tilde_message_filters/test/test_from_original_subscriber.cpp deleted file mode 100644 index aef89254..00000000 --- a/src/tilde_message_filters/test/test_from_original_subscriber.cpp +++ /dev/null @@ -1,307 +0,0 @@ -// 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. -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include - -// see ros2/rclcpp#1619,1713 -// TODO: remove this comment, and the `NonConstHelper` tests // NOLINT -// once the deprecated signatures have been discontinued. -#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 -#include "message_filters/chain.h" -#include "tilde/tilde_node.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" - -#include -#include - -#include "sensor_msgs/msg/imu.hpp" - -using namespace message_filters; // NOLINT -using namespace tilde_message_filters; // NOLINT -typedef sensor_msgs::msg::Imu Msg; -typedef std::shared_ptr MsgConstPtr; -typedef std::shared_ptr MsgPtr; - -class Helper -{ -public: - Helper() : count_(0) {} - - void cb(const MsgConstPtr) { ++count_; } - - int32_t count_; -}; - -TEST(TildeSubscriber, simple) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -TEST(TildeSubscriber, simple_raw) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node.get(), "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -TEST(TildeSubscriber, sub_UnSub_Sub) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - - sub.unsubscribe(); - sub.subscribe(); - - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -TEST(TildeSubscriber, sub_UnSub_Sub_raw) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node.get(), "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - - sub.unsubscribe(); - sub.subscribe(); - - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -TEST(TildeSubscriber, switchRawAndShared) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic2", 10); - - sub.unsubscribe(); - sub.subscribe(node.get(), "test_topic2"); - - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -TEST(TildeSubscriber, subInChain) -{ - auto node = std::make_shared("test_node"); - Helper h; - Chain c; - c.addFilter(std::make_shared >(node, "test_topic")); - c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - } - - ASSERT_GT(h.count_, 0); -} - -struct ConstHelper -{ - void cb(const MsgConstPtr msg) { msg_ = msg; } - - MsgConstPtr msg_; -}; - -struct NonConstHelper -{ - void cb(const MsgPtr msg) { msg_ = msg; } - - MsgPtr msg_; -}; - -TEST(TildeSubscriber, singleNonConstCallback) -{ - auto node = std::make_shared("test_node"); - NonConstHelper h; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(&NonConstHelper::cb, &h); - auto pub = node->create_publisher("test_topic", 10); - Msg msg; - pub->publish(Msg()); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - - ASSERT_TRUE(h.msg_); - ASSERT_EQ(msg, *h.msg_.get()); -} - -TEST(TildeSubscriber, multipleNonConstCallbacksFilterTildeSubscriber) -{ - auto node = std::make_shared("test_node"); - NonConstHelper h, h2; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(&NonConstHelper::cb, &h); - sub.registerCallback(&NonConstHelper::cb, &h2); - auto pub = node->create_publisher("test_topic", 10); - auto msg = std::make_unique(); - pub->publish(std::move(msg)); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - - ASSERT_TRUE(h.msg_); - ASSERT_TRUE(h2.msg_); - EXPECT_NE(msg.get(), h.msg_.get()); - EXPECT_NE(msg.get(), h2.msg_.get()); - EXPECT_NE(h.msg_.get(), h2.msg_.get()); -} - -TEST(TildeSubscriber, multipleCallbacksSomeFilterSomeDirect) -{ - auto node = std::make_shared("test_node"); - NonConstHelper h, h2; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(&NonConstHelper::cb, &h); - auto sub2 = node->create_subscription( - "test_topic", 10, std::bind(&NonConstHelper::cb, &h2, std::placeholders::_1)); - - auto pub = node->create_publisher("test_topic", 10); - auto msg = std::make_unique(); - pub->publish(std::move(msg)); - - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node); - - ASSERT_TRUE(h.msg_); - ASSERT_TRUE(h2.msg_); - EXPECT_NE(msg.get(), h.msg_.get()); - EXPECT_NE(msg.get(), h2.msg_.get()); - EXPECT_NE(h.msg_.get(), h2.msg_.get()); -} - -/* -TEST(TildeSubscriber, lifecycle) -{ - auto node = std::make_shared("test_node"); - Helper h; - TildeSubscriber sub(node, "test_topic"); - sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); - auto pub = node->create_publisher("test_topic", 10); - pub->on_activate(); - rclcpp::Clock ros_clock; - auto start = ros_clock.now(); - while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) - { - pub->publish(Msg()); - rclcpp::Rate(50).sleep(); - rclcpp::spin_some(node->get_node_base_interface()); - } - - ASSERT_GT(h.count_, 0); -} -*/ - -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - - rclcpp::init(argc, argv); - - return RUN_ALL_TESTS(); -} diff --git a/src/tilde_message_filters/test/test_from_original_synchronizer.cpp b/src/tilde_message_filters/test/test_from_original_synchronizer.cpp deleted file mode 100644 index 7be82ea4..00000000 --- a/src/tilde_message_filters/test/test_from_original_synchronizer.cpp +++ /dev/null @@ -1,517 +0,0 @@ -// 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. -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2008, Willow Garage, Inc. - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the Willow Garage nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - *********************************************************************/ - -#include "tilde_message_filters/tilde_synchronizer.hpp" - -#include - -#include - -#include - -using namespace message_filters; // NOLINT -using namespace tilde_message_filters; // NOLINT -using namespace std::placeholders; // NOLINT - -struct Header -{ - rclcpp::Time stamp; -}; - -struct Msg -{ - Header header; - int data; -}; -typedef std::shared_ptr MsgPtr; -typedef std::shared_ptr MsgConstPtr; - -template < - typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType, - typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType> -struct NullPolicy : public PolicyBase -{ - typedef Synchronizer Sync; - typedef PolicyBase Super; - typedef typename Super::Messages Messages; - typedef typename Super::Signal Signal; - typedef typename Super::Events Events; - typedef typename Super::RealTypeCount RealTypeCount; - - NullPolicy() - { - for (int i = 0; i < RealTypeCount::value; ++i) { - added_[i] = 0; - } - } - - void initParent(Sync *) {} - - template - void add(const typename std::tuple_element::type &) - { - ++added_.at(i); - } - - std::array added_; -}; -typedef NullPolicy Policy2; -typedef NullPolicy Policy3; -typedef NullPolicy Policy4; -typedef NullPolicy Policy5; -typedef NullPolicy Policy6; -typedef NullPolicy Policy7; -typedef NullPolicy Policy8; -typedef NullPolicy Policy9; - -TEST(TildeSynchronizer, compile2) -{ - NullFilter f0, f1; - TildeSynchronizer sync(nullptr, f0, f1); -} - -TEST(TildeSynchronizer, compile3) -{ - NullFilter f0, f1, f2; - TildeSynchronizer sync(nullptr, f0, f1, f2); -} - -TEST(TildeSynchronizer, compile4) -{ - NullFilter f0, f1, f2, f3; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3); -} - -TEST(TildeSynchronizer, compile5) -{ - NullFilter f0, f1, f2, f3, f4; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4); -} - -TEST(TildeSynchronizer, compile6) -{ - NullFilter f0, f1, f2, f3, f4, f5; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5); -} - -TEST(TildeSynchronizer, compile7) -{ - NullFilter f0, f1, f2, f3, f4, f5, f6; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6); -} - -TEST(TildeSynchronizer, compile8) -{ - NullFilter f0, f1, f2, f3, f4, f5, f6, f7; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6, f7); -} - -TEST(TildeSynchronizer, compile9) -{ - NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; - TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6, f7, f8); -} - -void function2(const MsgConstPtr &, const MsgConstPtr &) {} -void function3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} -void function4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) -{ -} -void function5( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &) -{ -} -void function6( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &, const MsgConstPtr &) -{ -} -void function7( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) -{ -} -void function8( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) -{ -} -void function9( - const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, - const MessageEvent &, const MessageEvent &, const MsgConstPtr &) -{ -} - -TEST(TildeSynchronizer, compileFunction2) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function2); -} - -TEST(TildeSynchronizer, compileFunction3) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function3); -} - -TEST(TildeSynchronizer, compileFunction4) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function4); -} - -TEST(TildeSynchronizer, compileFunction5) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function5); -} - -TEST(TildeSynchronizer, compileFunction6) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function6); -} - -TEST(TildeSynchronizer, compileFunction7) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function7); -} - -TEST(TildeSynchronizer, compileFunction8) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function8); -} - -/* -TEST(TildeSynchronizer, compileFunction9) -{ - TildeSynchronizer sync(nullptr); - sync.registerCallback(function9); -} -*/ - -struct MethodHelper -{ - void method2(const MsgConstPtr &, const MsgConstPtr &) {} - void method3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} - void method4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) - { - } - void method5( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &) - { - } - void method6( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &, const MsgConstPtr &) - { - } - void method7( - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, - const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) - { - } - void method8( - const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, - const MessageEvent &, const MessageEvent &) - { - } - // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 -}; - -TEST(TildeSynchronizer, compileMethod2) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method2, &h); -} - -TEST(TildeSynchronizer, compileMethod3) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method3, &h); -} - -TEST(TildeSynchronizer, compileMethod4) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method4, &h); -} - -TEST(TildeSynchronizer, compileMethod5) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method5, &h); -} - -TEST(TildeSynchronizer, compileMethod6) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method6, &h); -} - -TEST(TildeSynchronizer, compileMethod7) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method7, &h); -} - -/// cannot build this. too many placeholders? -/* -TEST(TildeSynchronizer, compileMethod8) -{ - MethodHelper h; - TildeSynchronizer sync(nullptr); - sync.registerCallback(&MethodHelper::method8, &h); -} -*/ - -/// add() or cb() are called internally, -/// so we can skip these tests -/* -TEST(TildeSynchronizer, add2) -{ - TildeSynchronizer sync(nullptr); - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); -} - -TEST(TildeSynchronizer, add3) -{ - TildeSynchronizer sync(nullptr); - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); -} - -TEST(TildeSynchronizer, add4) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); -} - -TEST(TildeSynchronizer, add5) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); - ASSERT_EQ(sync.added_[4], 0); - sync.add<4>(m); - ASSERT_EQ(sync.added_[4], 1); -} - -TEST(TildeSynchronizer, add6) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); - ASSERT_EQ(sync.added_[4], 0); - sync.add<4>(m); - ASSERT_EQ(sync.added_[4], 1); - ASSERT_EQ(sync.added_[5], 0); - sync.add<5>(m); - ASSERT_EQ(sync.added_[5], 1); -} - -TEST(TildeSynchronizer, add7) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); - ASSERT_EQ(sync.added_[4], 0); - sync.add<4>(m); - ASSERT_EQ(sync.added_[4], 1); - ASSERT_EQ(sync.added_[5], 0); - sync.add<5>(m); - ASSERT_EQ(sync.added_[5], 1); - ASSERT_EQ(sync.added_[6], 0); - sync.add<6>(m); - ASSERT_EQ(sync.added_[6], 1); -} - -TEST(TildeSynchronizer, add8) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); - ASSERT_EQ(sync.added_[4], 0); - sync.add<4>(m); - ASSERT_EQ(sync.added_[4], 1); - ASSERT_EQ(sync.added_[5], 0); - sync.add<5>(m); - ASSERT_EQ(sync.added_[5], 1); - ASSERT_EQ(sync.added_[6], 0); - sync.add<6>(m); - ASSERT_EQ(sync.added_[6], 1); - ASSERT_EQ(sync.added_[7], 0); - sync.add<7>(m); - ASSERT_EQ(sync.added_[7], 1); -} - -TEST(TildeSynchronizer, add9) -{ - TildeSynchronizer sync; - MsgPtr m(std::make_shared()); - - ASSERT_EQ(sync.added_[0], 0); - sync.add<0>(m); - ASSERT_EQ(sync.added_[0], 1); - ASSERT_EQ(sync.added_[1], 0); - sync.add<1>(m); - ASSERT_EQ(sync.added_[1], 1); - ASSERT_EQ(sync.added_[2], 0); - sync.add<2>(m); - ASSERT_EQ(sync.added_[2], 1); - ASSERT_EQ(sync.added_[3], 0); - sync.add<3>(m); - ASSERT_EQ(sync.added_[3], 1); - ASSERT_EQ(sync.added_[4], 0); - sync.add<4>(m); - ASSERT_EQ(sync.added_[4], 1); - ASSERT_EQ(sync.added_[5], 0); - sync.add<5>(m); - ASSERT_EQ(sync.added_[5], 1); - ASSERT_EQ(sync.added_[6], 0); - sync.add<6>(m); - ASSERT_EQ(sync.added_[6], 1); - ASSERT_EQ(sync.added_[7], 0); - sync.add<7>(m); - ASSERT_EQ(sync.added_[7], 1); - ASSERT_EQ(sync.added_[8], 0); - sync.add<8>(m); - ASSERT_EQ(sync.added_[8], 1); -} -*/ -int main(int argc, char ** argv) -{ - testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); -} diff --git a/src/tilde_message_filters/test/test_tilde_subscriber.cpp b/src/tilde_message_filters/test/test_tilde_subscriber.cpp deleted file mode 100644 index b5ae4a62..00000000 --- a/src/tilde_message_filters/test/test_tilde_subscriber.cpp +++ /dev/null @@ -1,178 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" - -#include "rosgraph_msgs/msg/clock.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include - -#include -#include -#include - -using TildeNode = tilde::TildeNode; -template -using TildePublisher = tilde::TildePublisher; -template -using TildeSubscriber = tilde_message_filters::TildeSubscriber; - -using Node = rclcpp::Node; -using PointCloud2 = sensor_msgs::msg::PointCloud2; -using PointCloud2Ptr = std::shared_ptr; -using PointCloud2ConstPtr = std::shared_ptr; - -using Clock = rosgraph_msgs::msg::Clock; -using MessageTrackingTag = tilde_msg::msg::MessageTrackingTag; -using MessageTrackingTagPtr = MessageTrackingTag::UniquePtr; -using TimeMsg = builtin_interfaces::msg::Time; - -using PointCloudPublisher = std::shared_ptr>; -using PointCloudPublishers = std::vector; -using PointCloudTildeSubscriber = TildeSubscriber; -using PointCloudTildeSubscribers = std::vector; - -class TestTildeSubscriber : public ::testing::Test -{ -public: - void SetUp() override - { - rclcpp::init(0, nullptr); - - options.append_parameter_override("use_sim_time", true); - qos.keep_last(10); - - // setup pub node - pub_node = std::make_shared("pub_node", options); - for (auto i = 0u; i < pubs.size(); i++) { - auto topic = std::string("in") + std::to_string(i + 1); - pubs[i] = pub_node->create_publisher(topic, qos); - } - clock_pub = pub_node->create_publisher("/clock", qos); - - // setup sub node - sub_node = std::make_shared("sub_node", options); - // skip init subs. Get subs via init_and_get_sub(). - out_pub = sub_node->create_tilde_publisher("out", qos); - - // setup val node - val_node = std::make_shared("val_node", options); - } - - void TearDown() override { rclcpp::shutdown(); } - - Clock send_clock(int32_t sec, uint32_t nsec) - { - Clock clock_msg; - clock_msg.clock.sec = sec; - clock_msg.clock.nanosec = nsec; - clock_pub->publish(clock_msg); - return clock_msg; - } - - Clock send_clock_and_spin(int32_t sec, uint32_t nsec) - { - auto clock = send_clock(sec, nsec); - rclcpp::Rate(50).sleep(); - spin(); - return clock; - } - - void spin() - { - rclcpp::spin_some(pub_node); - rclcpp::spin_some(sub_node); - rclcpp::spin_some(val_node); - } - - rclcpp::NodeOptions options; - rclcpp::QoS qos{10}; - - std::shared_ptr pub_node; - PointCloudPublishers pubs{9}; - std::shared_ptr> clock_pub; - - std::shared_ptr sub_node; - PointCloudTildeSubscriber sub; - std::shared_ptr> out_pub; - - std::shared_ptr val_node; -}; - -void EXPECT_CLOCK(rclcpp::Time time, int sec, uint32_t nsec) -{ - TimeMsg t = time; - EXPECT_EQ(t.sec, sec); - EXPECT_EQ(t.nanosec, nsec); -} - -TEST_F(TestTildeSubscriber, no_arg_constructor) -{ - auto ts = TildeSubscriber(); - SUCCEED(); -} - -TEST_F(TestTildeSubscriber, shared_ptr_constructor) -{ - auto ts = TildeSubscriber(sub_node, "topic"); - SUCCEED(); -} - -TEST_F(TestTildeSubscriber, ptr_constructor) -{ - auto ts = TildeSubscriber(sub_node.get(), "topic"); - SUCCEED(); -} - -TEST_F(TestTildeSubscriber, hold_sub_time) -{ - auto & pub1 = pubs[0]; - sub.subscribe(sub_node, "in1", qos.get_rmw_qos_profile()); - - // time - const int32_t t1_sec{1}, t2_sec{1}, t3_sec{2}; - const uint32_t t1_nsec{50}, t2_nsec{100}, t3_nsec{150}; - - // PointCloud2 message to publish - PointCloud2 msg; - - // t=t1 - send_clock(t1_sec, t1_nsec); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), t1_sec, t1_nsec); - - // in1: stamp=t2 @ t1 - msg.header.stamp.sec = t2_sec; - msg.header.stamp.nanosec = t2_nsec; - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // t=t3 - send_clock(t3_sec, t3_nsec); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), t3_sec, t3_nsec); - - // check - rclcpp::Time sub_time, sub_time_steady; - sub_node->find_subscription_time(&msg, "/in1", sub_time, sub_time_steady); - (void)sub_time_steady; - EXPECT_CLOCK(sub_time, t1_sec, t1_nsec); -} diff --git a/src/tilde_message_filters/test/test_tilde_synchronizer.cpp b/src/tilde_message_filters/test/test_tilde_synchronizer.cpp deleted file mode 100644 index e9736a28..00000000 --- a/src/tilde_message_filters/test/test_tilde_synchronizer.cpp +++ /dev/null @@ -1,489 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "tilde_message_filters/tilde_subscriber.hpp" -#include "tilde_message_filters/tilde_synchronizer.hpp" - -#include "rosgraph_msgs/msg/clock.hpp" -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include - -#include -#include -#include -#include - -template -using Subscriber = message_filters::Subscriber; -template -using PassThrough = message_filters::PassThrough; - -using TildeNode = tilde::TildeNode; -template -using TildePublisher = tilde::TildePublisher; -template -using TildeSubscriber = tilde_message_filters::TildeSubscriber; -template -using TildeSynchronizer = tilde_message_filters::TildeSynchronizer; - -using Node = rclcpp::Node; -using PointCloud2 = sensor_msgs::msg::PointCloud2; -using PointCloud2Ptr = std::shared_ptr; -using PointCloud2ConstPtr = std::shared_ptr; - -using Clock = rosgraph_msgs::msg::Clock; -using MessageTrackingTag = tilde_msg::msg::MessageTrackingTag; -using MessageTrackingTagPtr = MessageTrackingTag::UniquePtr; -using TimeMsg = builtin_interfaces::msg::Time; - -using PointCloudPublisher = std::shared_ptr>; -using PointCloudPublishers = std::vector; -using PointCloudTildeSubscriber = TildeSubscriber; -using PointCloudTildeSubscribers = std::vector; - -class TestSynchronizer : public ::testing::Test -{ -public: - void SetUp() override - { - rclcpp::init(0, nullptr); - - options.append_parameter_override("use_sim_time", true); - qos.keep_last(10); - - // setup pub node - pub_node = std::make_shared("pub_node", options); - for (auto i = 0u; i < subs.size(); i++) { - auto topic = std::string("in") + std::to_string(i + 1); - pubs.push_back(pub_node->create_publisher(topic, qos)); - } - clock_pub = pub_node->create_publisher("/clock", qos); - - // setup sub node - sub_node = std::make_shared("sub_node", options); - // skip init subs. Get subs via init_and_get_sub(). - out_pub = sub_node->create_tilde_publisher("out", qos); - - // setup val node - val_node = std::make_shared("val_node", options); - } - - void TearDown() override { rclcpp::shutdown(); } - - Clock send_clock(int32_t sec, uint32_t nsec) - { - Clock clock_msg; - clock_msg.clock.sec = sec; - clock_msg.clock.nanosec = nsec; - clock_pub->publish(clock_msg); - return clock_msg; - } - - Clock send_clock_and_spin(int32_t sec, uint32_t nsec) - { - auto clock = send_clock(sec, nsec); - rclcpp::Rate(50).sleep(); - spin(); - return clock; - } - - void spin() - { - rclcpp::spin_some(pub_node); - rclcpp::spin_some(sub_node); - rclcpp::spin_some(val_node); - } - - template - PointCloudTildeSubscriber & init_and_get_sub() - { - static_assert(I < 9); - auto topic = std::string("in") + std::to_string(I + 1); - subs[I].subscribe(sub_node, topic, qos.get_rmw_qos_profile()); - return subs[I]; - } - - rclcpp::NodeOptions options; - rclcpp::QoS qos{10}; - - std::shared_ptr pub_node; - PointCloudPublishers pubs; - std::shared_ptr> clock_pub; - - std::shared_ptr sub_node; - PointCloudTildeSubscribers subs{9}; - std::shared_ptr> out_pub; - - std::shared_ptr val_node; -}; - -void EXPECT_CLOCK(rclcpp::Time time, int sec, uint32_t nsec) -{ - TimeMsg t = time; - EXPECT_EQ(t.sec, sec); - EXPECT_EQ(t.nanosec, nsec); -} - -TEST_F(TestSynchronizer, exact_policy_2msgs) -{ - auto pub1 = pubs[0]; - auto pub2 = pubs[1]; - auto & sub1 = init_and_get_sub<0>(); - auto & sub2 = init_and_get_sub<1>(); - - // setup sub_node synchronizer - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = TildeSynchronizer; - auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); - - bool sync_callback_called = false; - sync->registerCallback( - [this, &sync_callback_called]( - const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { - (void)msg1; - (void)msg2; - sync_callback_called = true; - out_pub->publish(*msg1); - }); - - // validation node - bool val_callback_called = false; - auto val_sub = val_node->create_subscription( - "out/message_tracking_tag", qos, - [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { - val_callback_called = true; - EXPECT_EQ(pi->output_info.header_stamp.sec, 123); - EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); - EXPECT_EQ(pi->input_infos.size(), 2u); - - std::map topic2idx; - for (size_t i = 0; i < pi->input_infos.size(); i++) { - topic2idx[pi->input_infos[i].topic_name] = i; - } - - EXPECT_EQ(topic2idx.size(), 2u); - for (size_t i = 1; i <= 2; i++) { - auto topic = std::string("/in") + std::to_string(i); - auto idx = topic2idx[topic]; - const auto & in_info = pi->input_infos[idx]; - - switch (i) { - case 1: - EXPECT_EQ(in_info.topic_name, "/in1"); - EXPECT_EQ(in_info.sub_time.sec, 123); - EXPECT_EQ(in_info.sub_time.nanosec, 456u); - break; - case 2: - EXPECT_EQ(in_info.topic_name, "/in2"); - EXPECT_EQ(in_info.sub_time.sec, 124); - EXPECT_EQ(in_info.sub_time.nanosec, 321u); - break; - default: - // never comes here - EXPECT_TRUE(false); - } - } - }); - - // apply "/clock" - send_clock(123, 456); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), 123, 456u); - - // PointCloud2 message to publish - PointCloud2 msg; - msg.header.stamp = pub_node->now(); - - // pub1 & sub - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // update clock - send_clock(124, 321); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), 124, 321); - - // pub2 - EXPECT_FALSE(val_callback_called); - msg.header.frame_id = 2; - pub2->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // verify - EXPECT_TRUE(sync_callback_called); - EXPECT_TRUE(val_callback_called); -} - -/// TildeSynchronizer only collaborates with TildeSynchronizer -TEST_F(TestSynchronizer, sub_and_tilde_sub) -{ - auto pub1 = pubs[0]; - auto pub2 = pubs[1]; - auto & sub1 = init_and_get_sub<0>(); - Subscriber sub2; - sub2.subscribe(sub_node, "in2", qos.get_rmw_qos_profile()); - - // setup sub_node synchronizer - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = TildeSynchronizer; - auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); - - bool sync_callback_called = false; - sync->registerCallback( - [this, &sync_callback_called]( - const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { - (void)msg1; - (void)msg2; - sync_callback_called = true; - out_pub->publish(*msg1); - }); - - // validation node - bool val_callback_called = false; - auto val_sub = val_node->create_subscription( - "out/message_tracking_tag", qos, - [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { - val_callback_called = true; - EXPECT_EQ(pi->output_info.header_stamp.sec, 123); - EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); - EXPECT_EQ(pi->input_infos.size(), 1u); - - for (const auto & in_info : pi->input_infos) { - if (in_info.topic_name == "/in1") { - EXPECT_EQ(in_info.sub_time.sec, 123); - EXPECT_EQ(in_info.sub_time.nanosec, 456u); - } else { - // never comes here - FAIL() << "invalid topic: " << in_info.topic_name; - } - } - }); - - // apply "/clock" - send_clock(123, 456); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), 123, 456u); - - // PointCloud2 message to publish - PointCloud2 msg; - msg.header.stamp = pub_node->now(); - - // pub1 & sub - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // update clock - send_clock(124, 321); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), 124, 321); - - // pub2 - EXPECT_FALSE(val_callback_called); - msg.header.frame_id = 2; - pub2->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // verify - EXPECT_TRUE(sync_callback_called); - EXPECT_TRUE(val_callback_called); -} - -/// ignore general filter type -TEST_F(TestSynchronizer, work_with_passthrough) -{ - // passthrough msg1 to msg2 - auto pub1 = pubs[0]; - auto & sub1 = init_and_get_sub<0>(); - PassThrough passthrough; - sub1.registerCallback( - [this, &passthrough](const PointCloud2ConstPtr msg) -> void { passthrough.add(msg); }); - - // setup sub_node synchronizer - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = TildeSynchronizer; - auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, passthrough); - - bool sync_callback_called = false; - sync->registerCallback( - [this, &sync_callback_called]( - const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { - (void)msg1; - (void)msg2; - sync_callback_called = true; - out_pub->publish(*msg1); - }); - - // validation node - bool val_callback_called = false; - auto val_sub = val_node->create_subscription( - "out/message_tracking_tag", qos, - [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { - val_callback_called = true; - EXPECT_EQ(pi->output_info.header_stamp.sec, 123); - EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); - EXPECT_EQ(pi->input_infos.size(), 1u); - - for (const auto & in_info : pi->input_infos) { - if (in_info.topic_name == "/in1") { - EXPECT_EQ(in_info.sub_time.sec, 123); - EXPECT_EQ(in_info.sub_time.nanosec, 456u); - } else { - // never comes here - FAIL() << "invalid topic: " << in_info.topic_name; - } - } - }); - - // apply "/clock" - send_clock(123, 456); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), 123, 456u); - - // PointCloud2 message to publish - PointCloud2 msg; - msg.header.stamp = pub_node->now(); - - // pub1 & sub - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // update clock - send_clock(124, 321); - spin(); - rclcpp::Rate(50).sleep(); - EXPECT_CLOCK(sub_node->now(), 124, 321); - - // verify - EXPECT_TRUE(sync_callback_called); - EXPECT_TRUE(val_callback_called); -} - -/// (in1.stamp=t2) -> (in1.stamp=t1) -> (in2.stamp=t2) -TEST_F(TestSynchronizer, order_inversion) -{ - auto pub1 = pubs[0]; - auto pub2 = pubs[1]; - auto & sub1 = init_and_get_sub<0>(); - auto & sub2 = init_and_get_sub<1>(); - - // time - const int32_t t1_sec{1}, t2_sec{1}, t3_sec{2}; - const uint32_t t1_nsec{50}, t2_nsec{100}, t3_nsec{150}; - - // setup sub_node synchronizer - using SyncPolicy = message_filters::sync_policies::ExactTime; - using Sync = TildeSynchronizer; - auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); - - bool sync_callback_called = false; - sync->registerCallback( - [this, &sync_callback_called]( - const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { - (void)msg1; - (void)msg2; - sync_callback_called = true; - out_pub->publish(*msg1); - }); - - bool val_callback_called = false; - auto val_sub = val_node->create_subscription( - "out/message_tracking_tag", qos, - [this, &val_callback_called, t1_sec, t2_sec, t3_sec, t1_nsec, t2_nsec, - t3_nsec](MessageTrackingTagPtr pi) -> void { - val_callback_called = true; - EXPECT_EQ(pi->output_info.header_stamp.sec, t2_sec); - EXPECT_EQ(pi->output_info.header_stamp.nanosec, t2_nsec); - EXPECT_EQ(pi->input_infos.size(), 2u); - - for (const auto & in_info : pi->input_infos) { - if (in_info.topic_name == "/in1") { - EXPECT_EQ(in_info.sub_time.sec, t1_sec); - EXPECT_EQ(in_info.sub_time.nanosec, t1_nsec); - } else if (in_info.topic_name == "/in2") { - EXPECT_EQ(in_info.sub_time.sec, t3_sec); - EXPECT_EQ(in_info.sub_time.nanosec, t3_nsec); - } else { - FAIL() << "invalid topic" << in_info.topic_name; - } - } - }); - - // PointCloud2 message to publish - PointCloud2 msg; - - // t=t1 - send_clock(t1_sec, t1_nsec); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), t1_sec, t1_nsec); - - // in1: stamp=t2 @ t1 - msg.header.stamp.sec = t2_sec; - msg.header.stamp.nanosec = t2_nsec; - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // t=t2 - send_clock(t2_sec, t2_nsec); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), t2_sec, t2_nsec); - - // in1: stamp=t1 @ t2 - EXPECT_FALSE(val_callback_called); - msg.header.stamp.sec = t1_sec; - msg.header.stamp.nanosec = t1_nsec; - msg.header.frame_id = 1; - pub1->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // t=t3 - send_clock(t3_sec, t3_nsec); - rclcpp::Rate(50).sleep(); - spin(); - EXPECT_CLOCK(sub_node->now(), t3_sec, t3_nsec); - - // in2: stamp=t2 @ t3 - EXPECT_FALSE(val_callback_called); - msg.header.stamp.sec = t2_sec; - msg.header.stamp.nanosec = t2_nsec; - msg.header.frame_id = 2; - pub2->publish(msg); - rclcpp::Rate(50).sleep(); - spin(); - - // verify - EXPECT_TRUE(sync_callback_called); - EXPECT_TRUE(val_callback_called); -} diff --git a/src/tilde_msg/CMakeLists.txt b/src/tilde_msg/CMakeLists.txt deleted file mode 100644 index a0064fd2..00000000 --- a/src/tilde_msg/CMakeLists.txt +++ /dev/null @@ -1,89 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_msg) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -find_package(rosidl_default_generators REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(nav_msgs REQUIRED) - -find_package(autoware_auto_geometry_msgs REQUIRED) -find_package(autoware_auto_perception_msgs REQUIRED) -find_package(autoware_auto_planning_msgs REQUIRED) -find_package(autoware_auto_control_msgs REQUIRED) - -set(msg_files - "msg/SubTopicTimeInfo.msg" - "msg/PubTopicTimeInfo.msg" - "msg/MessageTrackingTag.msg" - "msg/TestTopLevelStamp.msg" - "msg/Source.msg" - "msg/DeadlineNotification.msg" - - "msg/SteeSource.msg" - - # sensor_msgs - "msg/SteePointCloud2.msg" - "msg/SteeImu.msg" - - # geometry_msgs - "msg/SteePolygonStamped.msg" - "msg/SteePoseStamped.msg" - "msg/SteePoseWithCovarianceStamped.msg" - "msg/SteeTwistStamped.msg" - "msg/SteeTwistWithCovarianceStamped.msg" - - # nav_msgs - "msg/SteeOccupancyGrid.msg" - "msg/SteeOdometry.msg" - - # autoware_auto_perception_msgs - "msg/SteeDetectedObjects.msg" - "msg/SteePredictedObjects.msg" - "msg/SteeTrackedObjects.msg" - "msg/SteeTrafficLightRoiArray.msg" - "msg/SteeTrafficSignalArray.msg" - - # autoware_auto_planning_msgs - "msg/SteePathWithLaneId.msg" - "msg/SteePath.msg" - "msg/SteeTrajectory.msg" - - # autoware_auto_control_msgs - "msg/SteeAckermannControlCommand.msg" - "msg/SteeAckermannLateralCommand.msg" - "msg/SteeLongitudinalCommand.msg" - ) - -rosidl_generate_interfaces(${PROJECT_NAME} - ${msg_files} - DEPENDENCIES - builtin_interfaces - std_msgs - sensor_msgs - geometry_msgs nav_msgs - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_control_msgs -) -ament_export_dependencies( - rosidl_default_runtime - sensor_msgs - geometry_msgs - nav_msgs - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - autoware_auto_control_msgs - ) - -ament_package() diff --git a/src/tilde_msg/msg/DeadlineNotification.msg b/src/tilde_msg/msg/DeadlineNotification.msg deleted file mode 100644 index fab275f0..00000000 --- a/src/tilde_msg/msg/DeadlineNotification.msg +++ /dev/null @@ -1,5 +0,0 @@ -std_msgs/Header header -string topic_name -builtin_interfaces/Time stamp -builtin_interfaces/Duration deadline_setting -Source[] sources diff --git a/src/tilde_msg/msg/MessageTrackingTag.msg b/src/tilde_msg/msg/MessageTrackingTag.msg deleted file mode 100644 index cc43636f..00000000 --- a/src/tilde_msg/msg/MessageTrackingTag.msg +++ /dev/null @@ -1,4 +0,0 @@ -std_msgs/Header header - -PubTopicTimeInfo output_info -SubTopicTimeInfo[] input_infos diff --git a/src/tilde_msg/msg/PubTopicTimeInfo.msg b/src/tilde_msg/msg/PubTopicTimeInfo.msg deleted file mode 100644 index 40ebde3f..00000000 --- a/src/tilde_msg/msg/PubTopicTimeInfo.msg +++ /dev/null @@ -1,8 +0,0 @@ -string topic_name -string node_fqn -int64 seq - -builtin_interfaces/Time pub_time -builtin_interfaces/Time pub_time_steady -bool has_header_stamp -builtin_interfaces/Time header_stamp diff --git a/src/tilde_msg/msg/Source.msg b/src/tilde_msg/msg/Source.msg deleted file mode 100644 index db4efa68..00000000 --- a/src/tilde_msg/msg/Source.msg +++ /dev/null @@ -1,4 +0,0 @@ -string topic -builtin_interfaces/Time stamp -builtin_interfaces/Duration elapsed -bool is_overrun diff --git a/src/tilde_msg/msg/SteeAckermannControlCommand.msg b/src/tilde_msg/msg/SteeAckermannControlCommand.msg deleted file mode 100644 index 8752608a..00000000 --- a/src/tilde_msg/msg/SteeAckermannControlCommand.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_control_msgs/AckermannControlCommand body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeAckermannLateralCommand.msg b/src/tilde_msg/msg/SteeAckermannLateralCommand.msg deleted file mode 100644 index 52dd2a2d..00000000 --- a/src/tilde_msg/msg/SteeAckermannLateralCommand.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_control_msgs/AckermannLateralCommand body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeDetectedObjects.msg b/src/tilde_msg/msg/SteeDetectedObjects.msg deleted file mode 100644 index 838144ae..00000000 --- a/src/tilde_msg/msg/SteeDetectedObjects.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_perception_msgs/DetectedObjects body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeImu.msg b/src/tilde_msg/msg/SteeImu.msg deleted file mode 100644 index cb0e407a..00000000 --- a/src/tilde_msg/msg/SteeImu.msg +++ /dev/null @@ -1,2 +0,0 @@ -sensor_msgs/Imu body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeLongitudinalCommand.msg b/src/tilde_msg/msg/SteeLongitudinalCommand.msg deleted file mode 100644 index f87a458b..00000000 --- a/src/tilde_msg/msg/SteeLongitudinalCommand.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_control_msgs/LongitudinalCommand body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeOccupancyGrid.msg b/src/tilde_msg/msg/SteeOccupancyGrid.msg deleted file mode 100644 index 7fbf7046..00000000 --- a/src/tilde_msg/msg/SteeOccupancyGrid.msg +++ /dev/null @@ -1,2 +0,0 @@ -nav_msgs/OccupancyGrid body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeOdometry.msg b/src/tilde_msg/msg/SteeOdometry.msg deleted file mode 100644 index 67084887..00000000 --- a/src/tilde_msg/msg/SteeOdometry.msg +++ /dev/null @@ -1,2 +0,0 @@ -nav_msgs/Odometry body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePath.msg b/src/tilde_msg/msg/SteePath.msg deleted file mode 100644 index 650e1fb0..00000000 --- a/src/tilde_msg/msg/SteePath.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_planning_msgs/Path body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePathWithLaneId.msg b/src/tilde_msg/msg/SteePathWithLaneId.msg deleted file mode 100644 index 6881f629..00000000 --- a/src/tilde_msg/msg/SteePathWithLaneId.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_planning_msgs/PathWithLaneId body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePointCloud2.msg b/src/tilde_msg/msg/SteePointCloud2.msg deleted file mode 100644 index fabc368e..00000000 --- a/src/tilde_msg/msg/SteePointCloud2.msg +++ /dev/null @@ -1,2 +0,0 @@ -sensor_msgs/PointCloud2 body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePolygonStamped.msg b/src/tilde_msg/msg/SteePolygonStamped.msg deleted file mode 100644 index 4ec0b18c..00000000 --- a/src/tilde_msg/msg/SteePolygonStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/PolygonStamped body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePoseStamped.msg b/src/tilde_msg/msg/SteePoseStamped.msg deleted file mode 100644 index 1971ea36..00000000 --- a/src/tilde_msg/msg/SteePoseStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/PoseStamped body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg b/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg deleted file mode 100644 index adc4c3fd..00000000 --- a/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/PoseWithCovarianceStamped body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePredictedObjects.msg b/src/tilde_msg/msg/SteePredictedObjects.msg deleted file mode 100644 index 3637acd6..00000000 --- a/src/tilde_msg/msg/SteePredictedObjects.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_perception_msgs/PredictedObjects body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeSource.msg b/src/tilde_msg/msg/SteeSource.msg deleted file mode 100644 index 573e9f26..00000000 --- a/src/tilde_msg/msg/SteeSource.msg +++ /dev/null @@ -1,3 +0,0 @@ -string topic -builtin_interfaces/Time stamp -builtin_interfaces/Time first_subscription_steady_time diff --git a/src/tilde_msg/msg/SteeTrackedObjects.msg b/src/tilde_msg/msg/SteeTrackedObjects.msg deleted file mode 100644 index 9c3640ed..00000000 --- a/src/tilde_msg/msg/SteeTrackedObjects.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_perception_msgs/TrackedObjects body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg b/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg deleted file mode 100644 index 5adcc05f..00000000 --- a/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_perception_msgs/TrafficLightRoiArray body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrafficSignalArray.msg b/src/tilde_msg/msg/SteeTrafficSignalArray.msg deleted file mode 100644 index f12cf212..00000000 --- a/src/tilde_msg/msg/SteeTrafficSignalArray.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_perception_msgs/TrafficSignalArray body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrajectory.msg b/src/tilde_msg/msg/SteeTrajectory.msg deleted file mode 100644 index 2915da37..00000000 --- a/src/tilde_msg/msg/SteeTrajectory.msg +++ /dev/null @@ -1,2 +0,0 @@ -autoware_auto_planning_msgs/Trajectory body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTwistStamped.msg b/src/tilde_msg/msg/SteeTwistStamped.msg deleted file mode 100644 index b900140c..00000000 --- a/src/tilde_msg/msg/SteeTwistStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/TwistStamped body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg b/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg deleted file mode 100644 index ca28f413..00000000 --- a/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg +++ /dev/null @@ -1,2 +0,0 @@ -geometry_msgs/TwistWithCovarianceStamped body -SteeSource[] sources diff --git a/src/tilde_msg/msg/SubTopicTimeInfo.msg b/src/tilde_msg/msg/SubTopicTimeInfo.msg deleted file mode 100644 index a1df91a6..00000000 --- a/src/tilde_msg/msg/SubTopicTimeInfo.msg +++ /dev/null @@ -1,5 +0,0 @@ -string topic_name -builtin_interfaces/Time sub_time -builtin_interfaces/Time sub_time_steady -bool has_header_stamp -builtin_interfaces/Time header_stamp diff --git a/src/tilde_msg/msg/TestTopLevelStamp.msg b/src/tilde_msg/msg/TestTopLevelStamp.msg deleted file mode 100644 index 4b20d319..00000000 --- a/src/tilde_msg/msg/TestTopLevelStamp.msg +++ /dev/null @@ -1 +0,0 @@ -builtin_interfaces/Time stamp diff --git a/src/tilde_msg/package.xml b/src/tilde_msg/package.xml deleted file mode 100644 index a58c79d3..00000000 --- a/src/tilde_msg/package.xml +++ /dev/null @@ -1,31 +0,0 @@ - - - - tilde_msg - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - ament_cmake - - rosidl_default_generators - rosidl_default_runtime - rosidl_interface_packages - - autoware_auto_control_msgs - autoware_auto_geometry_msgs - autoware_auto_perception_msgs - autoware_auto_planning_msgs - geometry_msgs - nav_msgs - sensor_msgs - std_msgs - - ament_lint_auto - caret_lint_common - - - ament_cmake - - diff --git a/src/tilde_sample/CMakeLists.txt b/src/tilde_sample/CMakeLists.txt deleted file mode 100644 index 4180bfaa..00000000 --- a/src/tilde_sample/CMakeLists.txt +++ /dev/null @@ -1,80 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_sample) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -find_package(tilde_cmake REQUIRED) -tilde_package() -find_package(tilde REQUIRED) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -include_directories(include) - -add_library(tilde_sample SHARED - src/sample_publisher.cpp - src/relay_timer.cpp - src/relay_timer_with_buffer.cpp - src/goal.cpp - src/sample_stee_publisher.cpp) -ament_target_dependencies(tilde_sample - "rclcpp" - "rclcpp_components" - "std_msgs" - "tilde" - "sensor_msgs") - -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::SamplePublisherWithStamp" - EXECUTABLE publisher_with_stamp) -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::SamplePublisherWithoutStamp" - EXECUTABLE publisher_without_stamp) -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::RelayTimer" - EXECUTABLE relay_timer) -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::RelayTimerWithBuffer" - EXECUTABLE relay_timer_with_buffer) -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::Goal" - EXECUTABLE goal) -rclcpp_components_register_node(tilde_sample - PLUGIN "tilde_sample::SampleSteePublisherNode" - EXECUTABLE sample_stee_publisher) - -install(TARGETS - tilde_sample) -install( - TARGETS tilde_sample EXPORT tilde_sample - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - - -ament_package() diff --git a/src/tilde_sample/README.md b/src/tilde_sample/README.md deleted file mode 100644 index ee6788ce..00000000 --- a/src/tilde_sample/README.md +++ /dev/null @@ -1,355 +0,0 @@ -# TILDE Samples - -## About - -Sample classes for TILDE. - -## Nodes - -As TILDE uses header.stamp, we have publishers/subscriptions with and without a header field. - -| class | about | -| --------------------------- | ------------------------------------------------------------------ | -| SamplePublisherWithStamp | Publish message with standard header (PointCloud2) | -| SamplePublisherWithoutStamp | Publish message without standard header (String) | -| RelayTimer | Subscribe PointCloud2 and String, and relay them | -| RelayTimerWithBuffer | Similar with RelayTimer, but it has buffers and calls explicit API | - -## Run - -### (1) Publisher and Subscription with standard header - -```bash -$ ros2 launch tilde_sample publisher_relay_with_header.launch.py -(snip) -[publisher_with_stamp-1] [INFO] [1646122380.746137604] [talker_with_stamp]: Publishing PointCloud2: 7 stamp: 1646122380745891914 -[relay_timer-2] [INFO] [1646122380.746543541] [relay_timer]: RelayTimer sub PointCloud2 seq: 7 stamp: 1646122380745891914 -[relay_timer-2] [INFO] [1646122380.757507728] [relay_timer]: RelayTimer pub PointCloud2 seq: 7 stamp: 1646122380745891914 -(snip) -``` - -See MessageTrackingTag by `ros2 topic echo /message_tracking_tag`. -In this example, you can see `MessageTrackingTag.output_info.stamp` is filled. - -```bash -$ ros2 topic echo /relay_with_stamp/message_tracking_tag -header: - stamp: - sec: 1646122380 - nanosec: 757608873 - frame_id: '' -output_info: - topic_name: /relay_with_stamp - node_fqn: '' - seq: 7 - pub_time: - sec: 1646122380 - nanosec: 757615453 - pub_time_steady: - sec: 6934823 - nanosec: 865332706 - has_header_stamp: true - header_stamp: # matches stamp of seq 7 of relay_timer - sec: 1646122380 - nanosec: 745891914 -input_infos: -- topic_name: /topic_with_stamp - sub_time: - sec: 1646122380 - nanosec: 746446909 - sub_time_steady: - sec: 6934823 - nanosec: 854165185 - has_header_stamp: true - header_stamp: # matches stamp of seq 7 of talker_with_stamp - sec: 1646122380 - nanosec: 745891914 ---- -``` - -### (2) Publisher and Subscription without standard header - -```bash -$ ros2 launch tilde_sample publisher_relay_without_header.launch.py -[publisher_without_stamp-1] [INFO] [1646122584.868273517] [talker_without_stamp]: Publishing String: '3' at '1646122584868032019' -[relay_timer-2] [INFO] [1646122584.868610655] [relay_timer]: RelayTimer sub String seq: 3 -[relay_timer-2] [INFO] [1646122584.882195449] [relay_timer]: RelayTimer pub String seq: 3 -``` - -In this example, you can see `MessageTrackingTag.output_info.has_header` is false, -and `stamp` has no meaning. - -```bash -ros2 topic echo /relay_without_stamp/message_tracking_tag -``` - -
-Result - -```bash -header: - stamp: - sec: 1646122584 - nanosec: 882317785 - frame_id: '' -output_info: - topic_name: /relay_without_stamp - node_fqn: '' - seq: 3 - pub_time: - sec: 1646122584 - nanosec: 882326545 - pub_time_steady: - sec: 6935027 - nanosec: 983860696 - has_header_stamp: false # no stamp - header_stamp: - sec: 0 - nanosec: 0 -input_infos: -- topic_name: /topic_without_stamp - sub_time: - sec: 1646122584 - nanosec: 868535499 - sub_time_steady: - sec: 6935027 - nanosec: 970071208 - has_header_stamp: false # no stamp - header_stamp: - sec: 0 - nanosec: 0 ---- -``` - -
- -### (3) Multiple Publishers and Subscription - -```bash -ros2 launch tilde_sample multi_publisher_relay.launch.py -``` - -In this example, two publishers are launched. -One has the standard header field, and another doesn't. - -RelayTimer is launched for the subscription, and relays -both publisher messages. - -You can see: - -- MessageTrackingTag has multiple input_info field - -#### multiple input + with stamp - -```bash -$ ros2 topic echo /relay_with_stamp/message_tracking_tag -[publisher_with_stamp-1] [INFO] [1646122873.266387175] [talker_with_stamp]: Publishing PointCloud2: 5 stamp: 1646122873266132674 -[relay_timer-3] [INFO] [1646122873.266819191] [relay_timer]: RelayTimer sub PointCloud2 seq: 5 stamp: 1646122873266132674 -[publisher_without_stamp-2] [INFO] [1646122873.270087090] [talker_without_stamp]: Publishing String: '5' at '1646122873269881680' -[relay_timer-3] [INFO] [1646122873.270392454] [relay_timer]: RelayTimer sub String seq: 5 -[relay_timer-3] [INFO] [1646122873.287324103] [relay_timer]: RelayTimer pub PointCloud2 seq: 5 stamp: 1646122873266132674 -[relay_timer-3] [INFO] [1646122873.287641253] [relay_timer]: RelayTimer pub String seq: 5 -``` - -Result - -```bash -header: - stamp: - sec: 1646122873 - nanosec: 287424898 - frame_id: '' -output_info: - topic_name: /relay_with_stamp - node_fqn: '' - seq: 5 - pub_time: - sec: 1646122873 - nanosec: 287431161 - pub_time_steady: - sec: 6935316 - nanosec: 380230053 - has_header_stamp: true - header_stamp: # matches stamp of relay_timer seq 5 - sec: 1646122873 - nanosec: 266132674 -input_infos: -- topic_name: /topic_with_stamp - sub_time: - sec: 1646122873 - nanosec: 266725249 - sub_time_steady: - sec: 6935316 - nanosec: 359525561 - has_header_stamp: true - header_stamp: # matches stamp of talker_with_stamp seq 5 - sec: 1646122873 - nanosec: 266132674 -- topic_name: /topic_without_stamp - sub_time: - sec: 1646122873 - nanosec: 270354306 - sub_time_steady: - sec: 6935316 - nanosec: 363153994 - has_header_stamp: false # no stamp - header_stamp: - sec: 0 - nanosec: 0 -``` - -#### multiple input + without stamp - -Now, let's see `/relay_without_stamp` MessageTrackingTag - -```bash -ros2 topic echo /relay_without_stamp/message_tracking_tag -``` - -Result: - -```bash -header: - stamp: - sec: 1646122966 - nanosec: 781916993 - frame_id: '' -output_info: - topic_name: /relay_without_stamp - node_fqn: '' - seq: 2 - pub_time: - sec: 1646122966 - nanosec: 781919913 - pub_time_steady: - sec: 6935409 - nanosec: 871887129 - has_header_stamp: false # no stamp - header_stamp: - sec: 0 - nanosec: 0 -input_infos: -- topic_name: /topic_with_stamp - sub_time: - sec: 1646122966 - nanosec: 768327652 - sub_time_steady: - sec: 6935409 - nanosec: 858296849 - has_header_stamp: true - header_stamp: - sec: 1646122966 - nanosec: 767682758 -- topic_name: /topic_without_stamp - sub_time: - sec: 1646122966 - nanosec: 771331749 - sub_time_steady: - sec: 6935409 - nanosec: 861299976 - has_header_stamp: false - header_stamp: - sec: 0 - nanosec: 0 -``` - -### (4) Multiple Publishers and buffered Subscription - -```bash -ros2 launch tilde_sample multi_publisher_buffered_relay.launch.py -``` - -In this example, two publishers are launched as in (3). - -For the subscription side, RelayTimerWithBuffer is launched. - -As RelayTimerWithBuffer uses explicit API, -you can see MessageTrackingTag has single input_info field. - -### buffer with stamp - -```bash -$ ros2 topic echo /relay_buffer_with_stamp/message_tracking_tag -header: - stamp: - sec: 1646123155 - nanosec: 485587145 - frame_id: '' -output_info: - topic_name: /relay_buffer_with_stamp - node_fqn: '' - seq: 5 - pub_time: - sec: 1646123155 - nanosec: 485593383 - pub_time_steady: - sec: 6935598 - nanosec: 569846981 - has_header_stamp: true - header_stamp: - sec: 1646123155 - nanosec: 472929593 -input_infos: # only one entry -- topic_name: /topic_with_stamp - sub_time: - sec: 1646123155 - nanosec: 473451355 - sub_time_steady: - sec: 6935598 - nanosec: 557705720 - has_header_stamp: true - header_stamp: - sec: 1646123155 - nanosec: 472929593 -``` - -### buffer without stamp - -We cannot call explicit API because this topic has no `header.stamp`. -So, there are two input info entries. - -```bash -$ ros2 topic echo /relay_buffer_without_stamp/message_tracking_tag -header: - stamp: - sec: 1646123029 - nanosec: 102143530 - frame_id: '' -output_info: - topic_name: /relay_buffer_without_stamp - node_fqn: '' - seq: 9 - pub_time: - sec: 1646123029 - nanosec: 102146186 - pub_time_steady: - sec: 6935472 - nanosec: 190226558 - has_header_stamp: false - header_stamp: - sec: 0 - nanosec: 0 -input_infos: # multiple input because explicit API is not called -- topic_name: /topic_with_stamp - sub_time: - sec: 1646123029 - nanosec: 88254671 - sub_time_steady: - sec: 6935472 - nanosec: 176335881 - has_header_stamp: true - header_stamp: - sec: 1646123029 - nanosec: 87745929 -- topic_name: /topic_without_stamp - sub_time: - sec: 1646123029 - nanosec: 78947692 - sub_time_steady: - sec: 6935472 - nanosec: 167029850 - has_header_stamp: false - header_stamp: - sec: 0 - nanosec: 0 -``` diff --git a/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py b/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py deleted file mode 100644 index 22ec7e21..00000000 --- a/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='tilde_sample', - executable='publisher_with_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='publisher_without_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='relay_timer_with_buffer', - output='screen'), - ]) diff --git a/src/tilde_sample/launch/multi_publisher_relay.launch.py b/src/tilde_sample/launch/multi_publisher_relay.launch.py deleted file mode 100644 index bcc1d049..00000000 --- a/src/tilde_sample/launch/multi_publisher_relay.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='tilde_sample', - executable='publisher_with_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='publisher_without_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='relay_timer', - output='screen'), - ]) diff --git a/src/tilde_sample/launch/publisher_relay_with_header.launch.py b/src/tilde_sample/launch/publisher_relay_with_header.launch.py deleted file mode 100644 index 912bc10a..00000000 --- a/src/tilde_sample/launch/publisher_relay_with_header.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='tilde_sample', - executable='publisher_with_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='relay_timer', - output='screen'), - ]) diff --git a/src/tilde_sample/launch/publisher_relay_without_header.launch.py b/src/tilde_sample/launch/publisher_relay_without_header.launch.py deleted file mode 100644 index 1394a678..00000000 --- a/src/tilde_sample/launch/publisher_relay_without_header.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='tilde_sample', - executable='publisher_without_stamp', - output='screen'), - Node( - package='tilde_sample', - executable='relay_timer', - output='screen'), - ]) diff --git a/src/tilde_sample/package.xml b/src/tilde_sample/package.xml deleted file mode 100644 index 1852e335..00000000 --- a/src/tilde_sample/package.xml +++ /dev/null @@ -1,27 +0,0 @@ - - - - tilde_sample - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - ament_cmake - - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tilde - tilde_cmake - - ament_lint_auto - caret_lint_common - - ros2launch - - - ament_cmake - - diff --git a/src/tilde_sample/src/goal.cpp b/src/tilde_sample/src/goal.cpp deleted file mode 100644 index f28539a6..00000000 --- a/src/tilde_sample/src/goal.cpp +++ /dev/null @@ -1,55 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" - -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -namespace tilde_sample -{ -// Create a Talker class that subclasses the generic rclcpp::Node base class. -// The main function below will instantiate the class as a ROS node. -class Goal : public tilde::TildeNode -{ -public: - explicit Goal(const rclcpp::NodeOptions & options) : TildeNode("talker", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto sub_callback = [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - (void)msg; - RCLCPP_INFO(this->get_logger(), "received"); - }; - sub_pc_ = - this->create_tilde_subscription("in", qos, sub_callback); - } - -private: - rclcpp::Subscription::SharedPtr sub_pc_; -}; - -} // namespace tilde_sample - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::Goal) diff --git a/src/tilde_sample/src/relay_timer.cpp b/src/tilde_sample/src/relay_timer.cpp deleted file mode 100644 index e2c68389..00000000 --- a/src/tilde_sample/src/relay_timer.cpp +++ /dev/null @@ -1,135 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -const int64_t TIMER_MS_DEFAULT = 1000; -const int64_t PROC_MS_DEFAULT = 10; - -namespace tilde_sample -{ - -// Create a Talker class that subclasses the generic rclcpp::Node base class. -// The main function below will instantiate the class as a ROS node. -class RelayTimer : public tilde::TildeNode -{ -public: - explicit RelayTimer(const rclcpp::NodeOptions & options) : TildeNode("relay_timer", options) - { - const std::string TIMER_MS = "timer_ms"; - const std::string PROC_MS = "proc_ms"; - const std::string UPDATE_STAMP = "update_stamp"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - declare_parameter(PROC_MS, PROC_MS_DEFAULT); - declare_parameter(UPDATE_STAMP, false); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - auto proc_ms = get_parameter(PROC_MS).get_value(); - auto update_stamp = get_parameter(UPDATE_STAMP).get_value(); - - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - sub_pc_ = this->create_tilde_subscription( - "topic_with_stamp", qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - RCLCPP_INFO( - this->get_logger(), "RelayTimer sub PointCloud2 seq: %s stamp: %lu", - msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); - msg_pc_ = std::move(msg); - }); - - sub_str_ = this->create_tilde_subscription( - "topic_without_stamp", qos, [this](std_msgs::msg::String::UniquePtr msg) -> void { - RCLCPP_INFO(this->get_logger(), "RelayTimer sub String seq: %s", msg->data.c_str()); - msg_str_ = std::move(msg); - }); - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto proc_dur = std::chrono::duration(proc_ms); - auto timer_callback = [this, proc_dur, update_stamp]() -> void { - std::this_thread::sleep_for(proc_dur); - - if (msg_pc_) { - // We should call explicit API because we don't use String msg. - // See RelayTimerWithBuffer to know how explicit API changes the MessageTrackingTag - - auto stamp = msg_pc_->header.stamp; - - if (update_stamp) { - stamp = this->now(); - msg_pc_->header.stamp = stamp; - } - - RCLCPP_INFO( - this->get_logger(), "RelayTimer pub PointCloud2 seq: %s stamp: %lu", - msg_pc_->header.frame_id.c_str(), nanoseconds(msg_pc_->header.stamp)); - - pub_pc_->publish(std::move(msg_pc_)); - msg_pc_.reset(nullptr); - } - - if (msg_str_) { - // We omit the explicit API. See `if(msg_pc_)` sentence above. - - RCLCPP_INFO(this->get_logger(), "RelayTimer pub String seq: %s", msg_str_->data.c_str()); - - pub_str_->publish(std::move(msg_str_)); - msg_str_.reset(nullptr); - } - }; - - // Create a publisher with a custom Quality of Service profile. - pub_pc_ = this->create_tilde_publisher("relay_with_stamp", qos); - pub_str_ = this->create_tilde_publisher("relay_without_stamp", qos); - - // Use a timer to schedule periodic message publishing. - auto timer_dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(timer_dur, timer_callback); - } - -private: - std::unique_ptr msg_pc_; - rclcpp::Subscription::SharedPtr sub_pc_; - tilde::TildePublisher::SharedPtr pub_pc_; - - std::unique_ptr msg_str_; - rclcpp::Subscription::SharedPtr sub_str_; - tilde::TildePublisher::SharedPtr pub_str_; - - rclcpp::TimerBase::SharedPtr timer_; - - uint64_t nanoseconds(const builtin_interfaces::msg::Time & time_msg) - { - rclcpp::Time time(time_msg); - return time.nanoseconds(); - } -}; - -} // namespace tilde_sample - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimer) diff --git a/src/tilde_sample/src/relay_timer_with_buffer.cpp b/src/tilde_sample/src/relay_timer_with_buffer.cpp deleted file mode 100644 index 95918054..00000000 --- a/src/tilde_sample/src/relay_timer_with_buffer.cpp +++ /dev/null @@ -1,142 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -const int64_t TIMER_MS_DEFAULT = 1000; -const int64_t PROC_MS_DEFAULT = 10; - -namespace tilde_sample -{ -// Create a Talker class that subclasses the generic rclcpp::Node base class. -// The main function below will instantiate the class as a ROS node. -class RelayTimerWithBuffer : public tilde::TildeNode -{ -public: - explicit RelayTimerWithBuffer(const rclcpp::NodeOptions & options) - : TildeNode("relay_timer_with_buffer", options) - { - const std::string TIMER_MS = "timer_ms"; - const std::string PROC_MS = "proc_ms"; - const std::string UPDATE_STAMP = "update_stamp"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - declare_parameter(PROC_MS, PROC_MS_DEFAULT); - declare_parameter(UPDATE_STAMP, false); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - auto proc_ms = get_parameter(PROC_MS).get_value(); - auto update_stamp = get_parameter(UPDATE_STAMP).get_value(); - - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - sub_pc_ = this->create_tilde_subscription( - "topic_with_stamp", qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - RCLCPP_INFO( - this->get_logger(), "RelayTimer sub PointCloud2 seq: %s stamp: %lu", - msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); - msgs_pc_.push_back(std::move(msg)); - }); - - sub_str_ = this->create_tilde_subscription( - "topic_without_stamp", qos, [this](std_msgs::msg::String::UniquePtr msg) -> void { - RCLCPP_INFO(this->get_logger(), "RelayTimer sub String seq: %s", msg->data.c_str()); - msgs_str_.push_back(std::move(msg)); - }); - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto proc_dur = std::chrono::duration(proc_ms); - auto timer_callback = [this, proc_dur, update_stamp]() -> void { - std::this_thread::sleep_for(proc_dur); - - if (msgs_pc_.size() > 0) { - auto msg = std::move(this->msgs_pc_.back()); - msgs_pc_.pop_back(); - - // We call explicit API in this example unlike RelayTimer - pub_pc_->add_explicit_input_info(this->sub_pc_->get_topic_name(), msg->header.stamp); - - auto stamp = msg->header.stamp; - - if (update_stamp) { - stamp = this->now(); - msg->header.stamp = stamp; - } - - RCLCPP_INFO( - this->get_logger(), "RelayTimer pub PointCloud2 seq: %s stamp: %lu", - msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); - - pub_pc_->publish(std::move(msg)); - } - - if (msgs_str_.size() > 0) { - auto msg = std::move(this->msgs_str_.back()); - msgs_str_.pop_back(); - - // We can not call explicit API because header.stamp does not exist - - RCLCPP_INFO(this->get_logger(), "RelayTimer pub String seq: %s", msg->data.c_str()); - - pub_str_->publish(std::move(msg)); - } - }; - - // Create a publisher with a custom Quality of Service profile. - pub_pc_ = - this->create_tilde_publisher("relay_buffer_with_stamp", qos); - pub_str_ = - this->create_tilde_publisher("relay_buffer_without_stamp", qos); - - // Use a timer to schedule periodic message publishing. - auto timer_dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(timer_dur, timer_callback); - } - -private: - std::vector> msgs_pc_; - rclcpp::Subscription::SharedPtr sub_pc_; - tilde::TildePublisher::SharedPtr pub_pc_; - - std::vector> msgs_str_; - rclcpp::Subscription::SharedPtr sub_str_; - tilde::TildePublisher::SharedPtr pub_str_; - - rclcpp::TimerBase::SharedPtr timer_; - - uint64_t nanoseconds(const builtin_interfaces::msg::Time & time_msg) - { - rclcpp::Time time(time_msg); - return time.nanoseconds(); - } -}; - -} // namespace tilde_sample - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimerWithBuffer) diff --git a/src/tilde_sample/src/sample_publisher.cpp b/src/tilde_sample/src/sample_publisher.cpp deleted file mode 100644 index 423d0b9c..00000000 --- a/src/tilde_sample/src/sample_publisher.cpp +++ /dev/null @@ -1,139 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -const int64_t TIMER_MS_DEFAULT = 1000; - -namespace tilde_sample -{ - -/** - * This class sends messages with header.stamp field. - * see msg.header.frame_id to get the sequence number. - */ -class SamplePublisherWithStamp : public tilde::TildeNode -{ -public: - explicit SamplePublisherWithStamp(const rclcpp::NodeOptions & options) - : TildeNode("talker_with_stamp", options) - { - const std::string TIMER_MS = "timer_ms"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - std::cout << "timer_ms: " << timer_ms << std::endl; - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto publish_message = [this]() -> void { - auto time_now = this->now(); - - msg_pc_ = std::make_unique(); - msg_pc_->header.stamp = time_now; - msg_pc_->header.frame_id = std::to_string(count_); - pub_pc_->publish(std::move(msg_pc_)); - - RCLCPP_INFO( - this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, - time_now.nanoseconds()); - - count_++; - }; - - // Create a publisher with a custom Quality of Service profile. - rclcpp::QoS qos(rclcpp::KeepLast(7)); - pub_pc_ = this->create_tilde_publisher("topic_with_stamp", qos); - - // Use a timer to schedule periodic message publishing. - auto dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(dur, publish_message); - } - -private: - size_t count_ = 0; - // message with standard header - std::unique_ptr msg_pc_; - tilde::TildePublisher::SharedPtr pub_pc_; - - rclcpp::TimerBase::SharedPtr timer_; -}; - -/** - * This class sends messages without header.stamp field. - */ -class SamplePublisherWithoutStamp : public tilde::TildeNode -{ -public: - explicit SamplePublisherWithoutStamp(const rclcpp::NodeOptions & options) - : TildeNode("talker_without_stamp", options) - { - const std::string TIMER_MS = "timer_ms"; - - declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); - auto timer_ms = get_parameter(TIMER_MS).get_value(); - std::cout << "timer_ms: " << timer_ms << std::endl; - - // Create a function for when messages are to be sent. - setvbuf(stdout, NULL, _IONBF, BUFSIZ); - auto publish_message = [this]() -> void { - auto time_now = this->now(); - - msg_string_ = std::make_unique(); - msg_string_->data = std::to_string(count_); - pub_string_->publish(std::move(msg_string_)); - - RCLCPP_INFO( - this->get_logger(), "Publishing String: '%ld' at '%lu'", count_, time_now.nanoseconds()); - - count_++; - }; - - // Create a publisher with a custom Quality of Service profile. - rclcpp::QoS qos(rclcpp::KeepLast(7)); - pub_string_ = this->create_tilde_publisher("topic_without_stamp", qos); - - // Use a timer to schedule periodic message publishing. - auto dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(dur, publish_message); - } - -private: - size_t count_ = 0; - - // message without standard header, especially header.stamp - std::unique_ptr msg_string_; - tilde::TildePublisher::SharedPtr pub_string_; - - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace tilde_sample - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithStamp) -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithoutStamp) diff --git a/src/tilde_sample/src/sample_stee_publisher.cpp b/src/tilde_sample/src/sample_stee_publisher.cpp deleted file mode 100644 index 1dbefa5d..00000000 --- a/src/tilde_sample/src/sample_stee_publisher.cpp +++ /dev/null @@ -1,86 +0,0 @@ -// 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/stee_node.hpp" -#include "tilde/stee_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -const int64_t g_timer_ms_default = 1000; - -namespace tilde_sample -{ - -class SampleSteePublisherNode : public tilde::SteeNode -{ -public: - explicit SampleSteePublisherNode(const rclcpp::NodeOptions & options) - : SteeNode("sample_stee_publisher_node", options) - { - const std::string timer_ms_name = "timer_ms"; - - declare_parameter(timer_ms_name, g_timer_ms_default); - auto timer_ms = get_parameter(timer_ms_name).get_value(); - std::cout << "timer_ms: " << timer_ms << std::endl; - - // Create a function for when messages are to be sent. - setvbuf(stdout, nullptr, _IONBF, BUFSIZ); - - auto publish_message = [this]() -> void { - auto time_now = this->now(); - - msg_pc_ = std::make_unique(); - msg_pc_->header.stamp = time_now; - msg_pc_->header.frame_id = std::to_string(count_); - pub_pc_->publish(std::move(msg_pc_)); - - RCLCPP_INFO( - this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, - time_now.nanoseconds()); - - count_++; - }; - - // Create a publisher with a custom Quality of Service profile. - rclcpp::QoS qos(rclcpp::KeepLast(7)); - pub_pc_ = this->create_stee_publisher("topic_with_stamp", qos); - - // Use a timer to schedule periodic message publishing. - auto dur = std::chrono::duration(timer_ms); - timer_ = this->create_wall_timer(dur, publish_message); - } - -private: - size_t count_ = 0; - // message with standard header - std::unique_ptr msg_pc_; - tilde::SteePublisher::SharedPtr pub_pc_; - - rclcpp::TimerBase::SharedPtr timer_; -}; - -} // namespace tilde_sample - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SampleSteePublisherNode) diff --git a/src/tilde_vis/README.md b/src/tilde_vis/README.md deleted file mode 100644 index 8c8f5988..00000000 --- a/src/tilde_vis/README.md +++ /dev/null @@ -1,194 +0,0 @@ -# tilde_vis - -## Description - -Sample application which uses topic info or MessageTrackingTag. - -## Requirement - -- ros2 galactic -- tilde enabled application - -## Applications - -To use this package, you need the system which uses tilde::TildeNode instead of rclcpp::Node. -Also you should call `create_tilde_subscription` and `create_tilde_publisher`. - -You can use some tools. - -- (1) latency viewer: online latency viewer - - run your application - - run `latency_viewer` -- (2) parse_message_tracking_tag & ipython: analyze latency from rosbag - - run your application and record rosbag2 - - run `parse_message_tracking_tag` to get MessageTrackingTag pkl. - - run ipython and do what you want -- (3) tilde_vis (not well maintained) - - save `/*/message_tracking_tag` to rosbag - - then, run `tilde_vis` -- (4) visualization by CARET - - you can visualize message flows by CARET jupyter notebook - -## (1) latency viewer - -### About - -The latency viewer outputs E2E latency online, just like `vmstat` or `top`. - -### Run - -Run the latency viewer **after** running the target application. -Latency viewer grasps your application topic graph in constructor by discovering and listening `MessageTrackingTag` type topics. - -Latency viewer shows latencies from `target_topic` to source topics. -Here is the example command. - -```bash -# (1) run your application -ros2 launch ... - -# (2) run latency viewer -ros2 run tilde_vis latency_viewer \ - --ros-args \ - -p target_topic:=/localization/pose_twist_fusion_filter/twist_with_covariance -``` - -In default, you see output just like `top`. Internally [curses](https://docs.python.org/3/library/curses.html) is used. -To save the results to a file, use `--batch` option and redirect or `tee` stdout. - -```bash -ros2 run tilde_vis latency_viewer --batch ... -``` - -### Parameters - -#### General parameters - -| name | values | about | -| ------------------------ | ------------------- | ------------------------------------------ | -| `mode` | `one_hot` or `stat` | Calculation rule. See below. | -| `wait_sec_to_init_graph` | positive integer | Initial wait seconds to grasp topic graph. | - -- mode - - one_hot: prints the specific flow latency - - stat: calculates statistics per second - -#### topic graph specific parameters - -You can use topic graph specific options. - -| name | about | -| ----------------- | ------------------------------------------------------------------------------- | -| `excludes_topics` | Topics you don't want to watch, such as `debug` topic. | -| `stops` | Stop traversal if latency viewer reaches these topics to prevent infinite loop. | - -Additionally, you can specify "skip" setting to skip non-TILDE node. -But you need to directly edit `LatencyViewerNode.init_skips()` to do it. - -### Result - -You can see the following lines are displayed repeatedly. -Columns are "topic name", "header stamp", "latency in ROS time", "latency in steady time". - -```text -/control/trajectory_follower/lateral/control_cmd 1618559274.549348133 0 0 - /localization/twist 1618559274.544330488 5 3 - /planning/scenario_planning/trajectory 1618559274.479350019 15 12 - (snip) - /sensing/lidar/concatenated/pointcloud 1618559273.951109000 289 288 - /sensing/lidar/left/outlier_filtered/pointcloud 1618559274.168087000 305 302 - /sensing/lidar/left/mirror_cropped/pointcloud_ex 1618559274.168087000 305 304 - /sensing/lidar/left/self_cropped/pointcloud_ex 1618559274.168087000 305 304 - /sensing/lidar/left/pointcloud_raw_ex* NA NA NA - /sensing/lidar/right/outlier_filtered/pointcloud 1618559274.170810000 300 301 - /sensing/lidar/right/mirror_cropped/pointcloud_ex 1618559274.170810000 305 302 - /sensing/lidar/right/self_cropped/pointcloud_ex 1618559274.170810000 305 303 - /sensing/lidar/right/pointcloud_raw_ex* NA NA NA - /sensing/lidar/top/outlier_filtered/pointcloud 1618559273.951109000 334 333 - /sensing/lidar/top/mirror_cropped/pointcloud_ex 1618559273.951109000 410 407 - /sensing/lidar/top/self_cropped/pointcloud_ex 1618559273.951109000 435 433 - /sensing/lidar/top/pointcloud_raw_ex* NA NA NA - /vehicle/status/twist* NA NA NA -``` - -See [latency_viewer.md](../../doc/latency_viewer.md) in detail (in Japanese). - -### key binding in ncurses view - -In ncurses view, you can use interactive commands. -When you enter "move" or "filter" commands, periodic update of view stops. -Hit - -| keys | about | -| ------------------- | ----------------------------------------------------- | -| down/up arrow, j, k | move lines. | -| d, u | page up/down. | -| g, G | move to top or bottom of lines | -| r, / | filter lines by regex. Hit Enter for periodic update. | -| q, F | stop "move" mode and restart periodic update | - -## (2) parse_message_tracking_tag - -### About - -Read rosbag2 file, and create pkl file. - -### Run - -```bash -ros2 run tilde_vis parse_message_tracking_tag -``` - -`topic_infos.pkl` is created. -You can get `message_tracking_tag.MessageTrackingTags` by reading this pkl file. - -## (3) tilde_vis - -Here are some examples. We use customized autoware.proj logging simulator where we use tilde at `/sensing` module. - -(1) Print subscribers topic info. - -There are five topics, so tilde_vis shows four per-callback latencies(from some subscription callback to the next subscription callback) and e2e latency. - -```bash -$ ros2 run tilde_vis tilde_vis - -/sensing/lidar/top/self_cropped/pointcloud_ex -> /sensing/lidar/top/rectified/pointcloud_ex -> /sensing/lidar/top/outlier_filtered/pointcloud -> /sensing/lidar/concatenated/pointcloud -> /sensing/lidar/measurement_range_cropped/pointcloud -max: 65.2 39.9 15.0 10.0 -avg: 53.0 28.5 9.0 5.5 -min: 40.0 19.9 5.0 0.0 -e2e: 110.0 96.0 80.0 - -max: 60.0 35.0 20.0 10.0 -avg: 55.0 24.0 9.6 5.4 -min: 50.0 19.7 5.0 0.0 -e2e: 110.0 94.0 80.0 -``` - -(2) print publisher topic info - -In the publisher topic info, timestamp means published topic header timestamp. -/sensing nodes conveys the lidar timestamp, we get latency is 0.0. - -```bash -$ ros2 run tilde_vis tilde_vis --ros-args -p watches_pub:=True - -/sensing/lidar/top/self_cropped/pointcloud_ex -> /sensing/lidar/top/mirror_cropped/pointcloud_ex -> /sensing/lidar/top/outlier_filtered/pointcloud -> /sensing/lidar/concatenated/pointcloud -> /sensing/lidar/measurement_range_cropped/pointcloud -max: 0.0 0.0 0.0 0.0 -avg: 0.0 0.0 0.0 0.0 -min: 0.0 0.0 0.0 0.0 -e2e: 0.0 0.0 0.0 - -max: 0.0 0.0 0.0 0.0 -avg: 0.0 0.0 0.0 0.0 -min: 0.0 0.0 0.0 0.0 -e2e: 0.0 0.0 0.0 -``` - -You can set log level by `LOG_LEVEL=DEBUG`. -See source codes for parameters. - -## (4) visualization by CARET - -You can draw message flows from a rosbag file. -Run tilde_vis/caret_vis_tilde in a CARET jupyter notebook. diff --git a/src/tilde_vis/package.xml b/src/tilde_vis/package.xml deleted file mode 100644 index c853da2d..00000000 --- a/src/tilde_vis/package.xml +++ /dev/null @@ -1,20 +0,0 @@ - - - - tilde_vis - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - rclpy - tilde_msg - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest - - - ament_python - - diff --git a/src/tilde_vis/resource/tilde_vis b/src/tilde_vis/resource/tilde_vis deleted file mode 100644 index e69de29b..00000000 diff --git a/src/tilde_vis/setup.cfg b/src/tilde_vis/setup.cfg deleted file mode 100644 index 7601a6d4..00000000 --- a/src/tilde_vis/setup.cfg +++ /dev/null @@ -1,4 +0,0 @@ -[develop] -script_dir=$base/lib/tilde_vis -[install] -install_scripts=$base/lib/tilde_vis diff --git a/src/tilde_vis/setup.py b/src/tilde_vis/setup.py deleted file mode 100644 index dfd93206..00000000 --- a/src/tilde_vis/setup.py +++ /dev/null @@ -1,30 +0,0 @@ -"""Visualization tool for TILDE.""" -from setuptools import setup - -package_name = 'tilde_vis' - -setup( - name=package_name, - version='0.0.0', - packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], - install_requires=['setuptools'], - zip_safe=True, - maintainer='y-okumura', - maintainer_email='y-okumura@todo.todo', - description='TODO: Package description', - license='TODO: License declaration', - tests_require=['pytest'], - entry_points={ - 'console_scripts': [ - 'tilde_vis = tilde_vis.tilde_vis:main', - 'latency_viewer = tilde_vis.latency_viewer:main', - 'message_tracking_tag_traverse = tilde_vis.message_tracking_tag_traverse:main', - 'parse_message_tracking_tag = tilde_vis.parse_message_tracking_tag:main', - ], - }, -) diff --git a/src/tilde_vis/test/test_copyright.py b/src/tilde_vis/test/test_copyright.py deleted file mode 100644 index cc8ff03f..00000000 --- a/src/tilde_vis/test/test_copyright.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, 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. - -from ament_copyright.main import main -import pytest - - -@pytest.mark.copyright -@pytest.mark.linter -def test_copyright(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found errors' diff --git a/src/tilde_vis/test/test_data_as_tree.py b/src/tilde_vis/test/test_data_as_tree.py deleted file mode 100644 index 5153f470..00000000 --- a/src/tilde_vis/test/test_data_as_tree.py +++ /dev/null @@ -1,192 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -from collections import deque -import unittest - -from tilde_vis.data_as_tree import TreeNode - - -def dict2tree(dict_): - """ - Convert dictionary to TreeNode. - - Parameters - ---------- - dict_: dictionary. If value is a dictionary, - then sub tree is created. - Otherwise, value are stored on data. - - Return - ------ - TreeNode starts from "root". - - """ - Q = deque() - root = TreeNode('root') - Q.append((root, dict_)) - - while len(Q) > 0: - current_node, sub_dict = Q.pop() - - for k, v in sub_dict.items(): - child = current_node.get_child(k) - if isinstance(v, dict): - Q.append((child, v)) - else: - child.add_data(v) - - return root - - -def get_complex_tree(): - """ - Get complex tree. - - Return: - ------ - see the following `sample` variable. - The name of the top node is "root". - So, call `ret.get_chile("p1")` and so on to access nodes. - - """ - sample = { - 'p1': { - 'c1': 'c11', - 'c2': { - 'c21': { - 'c211': 'c2111', - }, - 'c22': 'c221', - } - } - } - - # (TreeNode, sub dictionary) - Q = deque() - root = TreeNode('root') - Q.append((root, sample)) - - while len(Q) > 0: - current_node, sub_dict = Q.pop() - - if not isinstance(sub_dict, dict): - current_node.get_child(sub_dict) - continue - - for k in sub_dict.keys(): - child_node = current_node.get_child(k) - Q.append((child_node, sub_dict[k])) - - return root - - -class TestTreeNode(unittest.TestCase): - - def test_construct(self): - node = TreeNode('foo') - self.assertTrue(node is not None) - - def test_construct_complex(self): - root = get_complex_tree() - - # check tree structure - self.assertEqual(len(root.children), 1) - p1 = root.name2child['p1'] - self.assertEqual(len(p1.children), 2) - c1 = p1.name2child['c1'] - self.assertEqual(len(c1.children), 1) - c2 = p1.name2child['c2'] - self.assertEqual(len(c2.children), 2) - - def test_apply(self): - root = get_complex_tree() - ret = root.apply(lambda n: n.name) - - self.assertEqual(len(ret), 10) - self.assertEqual(ret, - ['root', - 'p1', - 'c1', - 'c11', - 'c2', - 'c21', - 'c211', - 'c2111', - 'c22', - 'c221']) - - def test_apply_with_depth(self): - root = get_complex_tree() - ret = root.apply_with_depth(lambda n, depth: f'{n.name}_{depth}') - - self.assertEqual(len(ret), 10) - self.assertEqual(ret, - ['root_0', - 'p1_1', - 'c1_2', - 'c11_3', - 'c2_2', - 'c21_3', - 'c211_4', - 'c2111_5', - 'c22_3', - 'c221_4']) - - def test_merge(self): - lhs_dict = { - 'c1': { - 'c11': 1, - 'c13': 2, - }, - 'c2': { - 'c21': 3, - }, - } - rhs_dict = { - 'c1': { - 'c11': 10, - 'c12': { - 'c121': 11, - }, - }, - 'c2': 12, - } - lhs = dict2tree(lhs_dict) - rhs = dict2tree(rhs_dict) - - def to_s(tree_node): - name = tree_node.name - data = tree_node.data - return f'{name}: {data}' - - lhs.apply(lambda x: print(to_s(x))) - lhs.merge(rhs) - lhs.apply(lambda x: print(to_s(x))) - - self.assertEqual(len(lhs.children), 2) - c1 = lhs.get_child('c1') - self.assertEqual(len(c1.children), 3) - self.assertEqual(c1.get_child('c11').data, [1, 10]) - self.assertEqual(c1.get_child('c12').get_child('c121').data, [11]) - self.assertEqual(c1.get_child('c13').data, [2]) - - c2 = lhs.get_child('c2') - self.assertEqual(c2.data, [12]) - self.assertEqual(c2.get_child('c21').data, [3]) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/tilde_vis/test/test_flake8.py b/src/tilde_vis/test/test_flake8.py deleted file mode 100644 index 27ee1078..00000000 --- a/src/tilde_vis/test/test_flake8.py +++ /dev/null @@ -1,25 +0,0 @@ -# Copyright 2017 Open Source Robotics Foundation, 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. - -from ament_flake8.main import main_with_errors -import pytest - - -@pytest.mark.flake8 -@pytest.mark.linter -def test_flake8(): - rc, errors = main_with_errors(argv=[]) - assert rc == 0, \ - 'Found %d code style errors / warnings:\n' % len(errors) + \ - '\n'.join(errors) diff --git a/src/tilde_vis/test/test_latency_viewer.py b/src/tilde_vis/test/test_latency_viewer.py deleted file mode 100644 index f372f701..00000000 --- a/src/tilde_vis/test/test_latency_viewer.py +++ /dev/null @@ -1,272 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -import unittest - -from builtin_interfaces.msg import Time as TimeMsg -import rclpy -from rclpy.time import Time - -from tilde_msg.msg import ( - MessageTrackingTag as MessageTrackingTagMsg, - ) -from tilde_vis.data_as_tree import TreeNode -from tilde_vis.latency_viewer import ( - calc_stat, - LatencyViewerNode, - update_stat, - ) -from tilde_vis.message_tracking_tag import ( - MessageTrackingTag - ) - - -class TestCalcStat(unittest.TestCase): - - def test_none(self): - root = TreeNode('root') - root.data = [(None, None), (None, None), (None, None)] - ret = calc_stat(root) - - self.assertEqual(len(ret), 1) - self.assertEqual(ret[0]['dur_min'], None) - self.assertEqual(ret[0]['dur_mean'], None) - - def test_simple(self): - root = TreeNode('root') - root.data = [(1, 10), (2, 20), (3, 30)] - ret = calc_stat(root) - - self.assertEqual(len(ret), 1) - self.assertEqual(ret[0]['dur_min'], 1) - self.assertEqual(ret[0]['dur_mean'], 2) - self.assertEqual(ret[0]['dur_max'], 3) - self.assertEqual(ret[0]['dur_min_steady'], 10) - self.assertEqual(ret[0]['dur_mean_steady'], 20) - self.assertEqual(ret[0]['dur_max_steady'], 30) - - -def time_msg(sec, ms): - """Get builtin.msg.Time.""" - return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() - - -def get_solver_result_simple( - t1_pub, - t1_sub, t2_pub, - t2_sub, t3_pub): - """ - Generate solver result. - - Graph: topic1 --> topic2 --> topic3 - - You can specify each pub/sub time. - Assume: - - header_stamp is preserved at topic1 pub time. - - stamp and stamp_steady are same - """ - tree_node3 = TreeNode('topic3') - tree_node2 = tree_node3.get_child('topic2') - tree_node1 = tree_node2.get_child('topic1') - - stamp1 = t1_pub - message_tracking_tag3 = MessageTrackingTag( - 'topic3', - t3_pub, t3_pub, True, stamp1) - message_tracking_tag3.add_input_info( - 'topic2', t2_sub, t2_sub, - True, stamp1) - tree_node3.add_data(message_tracking_tag3) - - message_tracking_tag2 = MessageTrackingTag( - 'topic2', - t2_pub, t2_pub, True, stamp1) - message_tracking_tag2.add_input_info( - 'topic1', t1_sub, t1_sub, - True, stamp1) - tree_node2.add_data(message_tracking_tag2) - - message_tracking_tag1 = MessageTrackingTag( - 'topic1', - t1_pub, t1_pub, True, stamp1) - tree_node1.add_data(message_tracking_tag1) - - return tree_node3 - - -class TestUpdateStat(unittest.TestCase): - - def test_simple(self): - """ - Simple case. - - pub/sub time - (case1) 10 11 12 13 14 - (case2) 20 24 28 30 35 - - t1 means: - topic1 pub at t=10 and subscribed at t=11 - topic2 pub at t=12 and subscribed at t=13 - topic3 pub at t=14 - """ - results_case1 = get_solver_result_simple( - time_msg(0, 10), - time_msg(0, 11), time_msg(0, 12), - time_msg(0, 13), time_msg(0, 14)) - - update_stat_case1 = update_stat(results_case1) - topic3_result_data = update_stat_case1.data - self.assertEqual(topic3_result_data[0], (0, 0)) - - topic2_result_data = update_stat_case1.name2child['topic2'].data - self.assertEqual(topic2_result_data[0], (2, 2)) - - topic1_result_data = \ - update_stat_case1.name2child['topic2'].name2child['topic1'].data - self.assertEqual(topic1_result_data[0], (4, 4)) - - -class TestListenerCallback(unittest.TestCase): - - @classmethod - def setUpClass(cls): - rclpy.init() - - @classmethod - def tearDownClass(cls): - rclpy.shutdown() - - def test_seq(self): - topic_name = 'topic' - node = LatencyViewerNode() - msg = MessageTrackingTagMsg() - msg.output_info.topic_name = topic_name - - msg.output_info.seq = 0 - msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) - node.listener_callback(msg) - self.assertTrue(topic_name in node.topic_seq) - self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 1) - self.assertEqual(node.topic_seq[topic_name], 0) - - msg.output_info.seq = 1 - msg.output_info.header_stamp = TimeMsg(sec=11, nanosec=0) - node.listener_callback(msg) - self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 2) - self.assertEqual(node.topic_seq[topic_name], 1) - - msg.output_info.seq = 3 - msg.output_info.header_stamp = TimeMsg(sec=13, nanosec=0) - node.listener_callback(msg) - self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 3) - self.assertEqual(node.topic_seq[topic_name], 3) - - msg.output_info.seq = 2 - msg.output_info.header_stamp = TimeMsg(sec=12, nanosec=0) - node.listener_callback(msg) - self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 4) - self.assertEqual(node.topic_seq[topic_name], 3) - - node.destroy_node() - - def test_seq_non_zero_start(self): - topic_name = 'topic' - node = LatencyViewerNode() - msg = MessageTrackingTagMsg() - msg.output_info.topic_name = topic_name - - msg.output_info.seq = 10 - msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) - node.listener_callback(msg) - self.assertTrue(topic_name in node.topic_seq) - self.assertEqual(node.topic_seq[topic_name], 10) - - msg.output_info.seq = 20 - msg.output_info.header_stamp = TimeMsg(sec=20, nanosec=0) - node.listener_callback(msg) - self.assertTrue(topic_name in node.topic_seq) - self.assertEqual(node.topic_seq[topic_name], 20) - - msg.output_info.seq = 15 - msg.output_info.header_stamp = TimeMsg(sec=15, nanosec=0) - node.listener_callback(msg) - self.assertTrue(topic_name in node.topic_seq) - self.assertEqual(node.topic_seq[topic_name], 20) - - node.destroy_node() - - -class TestTimerCallback(unittest.TestCase): - - def setUp(self): - rclpy.init() - - def tearDown(self): - rclpy.shutdown() - - def test_issues23_1(self): - topic_name = 'topic' - node = LatencyViewerNode() - node.target_topic = topic_name - - msg = MessageTrackingTagMsg() - msg.output_info.topic_name = topic_name - msg.output_info.seq = 0 - msg.output_info.has_header_stamp = False - msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) - - node.listener_callback(msg) - self.assertEqual(len(node.message_tracking_tags.topics()), 1) - self.assertEqual(node.message_tracking_tags.topics()[0], topic_name) - stamps = node.message_tracking_tags.stamps(topic_name) - self.assertEqual(len(stamps), 1) - - node.wait_init = node.wait_sec_to_init_graph + 1 - - try: - node.timer_callback() - self.assertTrue(True) - except AttributeError: - self.fail(msg='timer_callback causes AttributeError') - - def test_issues23_2(self): - topic_name = 'topic' - node = LatencyViewerNode() - - node.target_topic = topic_name - - msg = MessageTrackingTagMsg() - msg.output_info.topic_name = topic_name - msg.output_info.seq = 0 - msg.output_info.has_header_stamp = True - msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) - - node.listener_callback(msg) - self.assertEqual(len(node.message_tracking_tags.topics()), 1) - self.assertEqual(node.message_tracking_tags.topics()[0], topic_name) - stamps = node.message_tracking_tags.stamps(topic_name) - self.assertEqual(len(stamps), 1) - - node.wait_init = node.wait_sec_to_init_graph + 1 - - try: - node.timer_callback() - self.assertTrue(True) - except AttributeError: - self.fail(msg='timer_callback causes AttributeError') - - -if __name__ == '__main__': - unittest.main() diff --git a/src/tilde_vis/test/test_message_tracking_tag.py b/src/tilde_vis/test/test_message_tracking_tag.py deleted file mode 100644 index 88ad5726..00000000 --- a/src/tilde_vis/test/test_message_tracking_tag.py +++ /dev/null @@ -1,119 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -import unittest - -from rclpy.time import Time - -from tilde_vis.message_tracking_tag import ( - MessageTrackingTag, - MessageTrackingTags - ) - - -def time_msg(sec, ms): - """Get builtin.msg.Time.""" - return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() - - -class TestMessageTrackingTags(unittest.TestCase): - - def test_erase_until(self): - infos = MessageTrackingTags() - infos.add(MessageTrackingTag( - 'topic1', - time_msg(8, 0), - time_msg(9, 0), - True, - time_msg(10, 0) - )) - infos.add(MessageTrackingTag( - 'topic2', - time_msg(18, 0), - time_msg(19, 0), - True, - time_msg(20, 0) - )) - infos.add(MessageTrackingTag( - 'topic1', - time_msg(28, 0), - time_msg(29, 0), - True, - time_msg(30, 0) - )) - - self.assertEqual(len(infos.topic_vs_message_tracking_tags.keys()), 2) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) - - infos.erase_until(time_msg(9, 999)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) - - # boundary condition - not deleted - infos.erase_until(time_msg(10, 0)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) - - infos.erase_until(time_msg(10, 1)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 1) - self.assertTrue('30.000000000' in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) - - # topic2 at t=30.0 is deleted, but key is preserved - infos.erase_until(time_msg(20, 1)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags.keys()), 2) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 1) - self.assertTrue('30.000000000' in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2']), 0) - - def test_erase_until_many(self): - infos = MessageTrackingTags() - for i in range(0, 10): - infos.add(MessageTrackingTag( - 'topic1', - time_msg(i, 0), - time_msg(i, 0), - True, - time_msg(i, 0) - )) - - infos.erase_until(time_msg(3, 1)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 6) - self.assertTrue('0.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('1.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('2.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('3.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - - infos.erase_until(time_msg(7, 1)) - self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) - self.assertTrue('4.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('5.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('6.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - self.assertTrue('7.000000000' not in - infos.topic_vs_message_tracking_tags['topic1'].keys()) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/tilde_vis/test/test_message_tracking_tag_traverse.py b/src/tilde_vis/test/test_message_tracking_tag_traverse.py deleted file mode 100644 index 141bb877..00000000 --- a/src/tilde_vis/test/test_message_tracking_tag_traverse.py +++ /dev/null @@ -1,558 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -import unittest - -from rclpy.clock import ClockType -from rclpy.duration import Duration -from rclpy.time import Time - -from tilde_vis.latency_viewer import ( - calc_one_hot, - update_stat, - ) -from tilde_vis.message_tracking_tag import MessageTrackingTag, MessageTrackingTags -from tilde_vis.message_tracking_tag_traverse import ( - InputSensorStampSolver, - TopicGraph, - ) - - -def get_scenario1(): - """ - Get the following scenario. - - topic1 --> topic2 --> topic3 - - topic1 is published in 10Hz. - topic2 takes 10ms to execute callback. - - - """ - def get_topic2_times(start_time, start_time_steady): - sub_time = start_time + nw_dur - pub_time = sub_time + cb_dur - - sub_time_steady = start_time_steady + nw_dur - pub_time_steady = sub_time_steady + cb_dur - - return {'sub': sub_time, - 'pub': pub_time, - 'sub_steady': sub_time_steady, - 'pub_steady': pub_time_steady} - - def get_topic3_times(start_time, start_time_steady): - sub_time = start_time + nw_dur + cb_dur + nw_dur - pub_time = sub_time + cb_dur - - sub_time_steady = start_time_steady + nw_dur + cb_dur + nw_dur - pub_time_steady = sub_time_steady + cb_dur - - return {'sub': sub_time, - 'pub': pub_time, - 'sub_steady': sub_time_steady, - 'pub_steady': pub_time_steady} - - t1 = Time(seconds=10, nanoseconds=0) - t1_steady = Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME) - nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] - cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] - - message_tracking_tag_topic1_t1 = \ - MessageTrackingTag( - 'topic1', - t1.to_msg(), - t1_steady.to_msg(), - True, - t1.to_msg()) - - topic2_t1_times = get_topic2_times(t1, t1_steady) - message_tracking_tag_topic2_t1 = \ - MessageTrackingTag( - 'topic2', - topic2_t1_times['pub'].to_msg(), - topic2_t1_times['pub_steady'].to_msg(), - True, - t1.to_msg()) - message_tracking_tag_topic2_t1.add_input_info( - 'topic1', - topic2_t1_times['sub'], - topic2_t1_times['sub_steady'], - True, - t1.to_msg()) - - topic3_t1_times = get_topic3_times(t1, t1_steady) - message_tracking_tag_topic3_t1 = \ - MessageTrackingTag( - 'topic3', - topic3_t1_times['pub'].to_msg(), - topic3_t1_times['pub_steady'].to_msg(), - True, - t1.to_msg()) - message_tracking_tag_topic3_t1.add_input_info( - 'topic2', - topic3_t1_times['sub'], - topic3_t1_times['sub_steady'], - True, - t1.to_msg()) - - return [message_tracking_tag_topic1_t1, - message_tracking_tag_topic2_t1, - message_tracking_tag_topic3_t1] - - -def dur_plus(lhs, rhs): - """Plus of Duration.""" - return Duration(nanoseconds=lhs.nanoseconds + rhs.nanoseconds) - - -def gen_scenario2( - st, st_steady, - nw_dur, cb_dur): - """ - Get the following scenario. - - topic1 --> topic3 --> topic4 - topic2 ---/ - - Args: - st: Time - st_steady: Time - nw_dur: Duration - cb_dur: Duration - - Returns - ------- - list of MessageTrackingTags such as - (message_tracking_tag of topic1, message_tracking_tag of topic2, ...) - - """ - message_tracking_tag_topic1 = \ - MessageTrackingTag( - 'topic1', - st.to_msg(), - st_steady.to_msg(), - True, - st.to_msg()) - - topic2_stamp = st + nw_dur - message_tracking_tag_topic2 = \ - MessageTrackingTag( - 'topic2', - st.to_msg(), - st_steady.to_msg(), - True, - topic2_stamp.to_msg()) - - sub_dur3 = nw_dur - pub_dur3 = dur_plus(sub_dur3, cb_dur) - topic3_stamp = st + pub_dur3 - message_tracking_tag_topic3 = \ - MessageTrackingTag( - 'topic3', - (st + pub_dur3).to_msg(), - (st_steady + pub_dur3).to_msg(), - True, - topic3_stamp.to_msg()) - message_tracking_tag_topic3.add_input_info( - 'topic1', - (st + sub_dur3).to_msg(), - (st_steady + sub_dur3).to_msg(), - True, - st.to_msg() - ) - message_tracking_tag_topic3.add_input_info( - 'topic2', - (st + sub_dur3).to_msg(), - (st_steady + sub_dur3).to_msg(), - True, - topic2_stamp.to_msg()) - - sub_dur4 = dur_plus(pub_dur3, nw_dur) - pub_dur4 = dur_plus(sub_dur4, cb_dur) - topic4_stamp = st + pub_dur4 - message_tracking_tag_topic4 = \ - MessageTrackingTag( - 'topic4', - (st + pub_dur4).to_msg(), - (st_steady + pub_dur4).to_msg(), - True, - topic4_stamp.to_msg()) - message_tracking_tag_topic4.add_input_info( - 'topic3', - (st + sub_dur4).to_msg(), - (st_steady + sub_dur4).to_msg(), - True, - topic3_stamp.to_msg()) - - return [ - message_tracking_tag_topic1, - message_tracking_tag_topic2, - message_tracking_tag_topic3, - message_tracking_tag_topic4, - ] - - -class TestTopicGraph(unittest.TestCase): - - def test_straight(self): - infos = get_scenario1() - message_tracking_tags = MessageTrackingTags() - for info in infos: - message_tracking_tags.add(info) - - graph = TopicGraph(message_tracking_tags, skips={}) - - self.assertEqual(graph.topics[0], 'topic1') - self.assertEqual(graph.topics[1], 'topic2') - self.assertEqual(graph.topics[2], 'topic3') - - edges = graph.topic_edges - self.assertEqual(edges[0], {1}) - self.assertEqual(edges[1], {2}) - self.assertEqual(edges[2], set()) - - rev_edges = graph.rev_edges - self.assertEqual(rev_edges[0], set()) - self.assertEqual(rev_edges[1], {0}) - self.assertEqual(rev_edges[2], {1}) - - def test_scenario2(self): - t0 = Time(seconds=10, nanoseconds=0) - t0_steady = Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME) - nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] - cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] - - infos = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) - message_tracking_tags = MessageTrackingTags() - for info in infos: - message_tracking_tags.add(info) - - graph = TopicGraph(message_tracking_tags, skips={}) - - self.assertEqual(graph.topics[0], 'topic1') - self.assertEqual(graph.topics[1], 'topic2') - self.assertEqual(graph.topics[2], 'topic3') - self.assertEqual(graph.topics[3], 'topic4') - - edges = graph.topic_edges - self.assertEqual(edges[0], {2}) - self.assertEqual(edges[1], {2}) - self.assertEqual(edges[2], {3}) - self.assertEqual(edges[3], set()) - - rev_edges = graph.rev_edges - self.assertEqual(rev_edges[0], set()) - self.assertEqual(rev_edges[1], set()) - self.assertEqual(rev_edges[2], {0, 1}) - self.assertEqual(rev_edges[3], {2}) - - def test_scenario2_with_loss(self): - """ - Graph creation with lossy MessageTrackingTags. - - t0: message_tracking_tag only topic1 and topic3 - t1: only topic2 - t2: only topic4 - """ - t0 = Time(seconds=10, nanoseconds=0) - t0_steady = Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME) - nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] - cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] - period = Duration(seconds=1) - t1, t1_steady = t0 + period, t0_steady + period - t2, t2_steady = t1 + period, t1_steady + period - - infos_t0 = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) - infos_t1 = gen_scenario2(t1, t1_steady, nw_dur, cb_dur) - infos_t2 = gen_scenario2(t2, t2_steady, nw_dur, cb_dur) - message_tracking_tags = MessageTrackingTags() - - message_tracking_tags.add(infos_t0[0]) - message_tracking_tags.add(infos_t0[2]) - message_tracking_tags.add(infos_t1[1]) - message_tracking_tags.add(infos_t2[3]) - - graph = TopicGraph(message_tracking_tags, skips={}) - - self.assertEqual(graph.topics[0], 'topic1') - self.assertEqual(graph.topics[1], 'topic2') - self.assertEqual(graph.topics[2], 'topic3') - self.assertEqual(graph.topics[3], 'topic4') - - edges = graph.topic_edges - self.assertEqual(edges[0], {2}) - self.assertEqual(edges[1], {2}) - self.assertEqual(edges[2], {3}) - self.assertEqual(edges[3], set()) - - rev_edges = graph.rev_edges - self.assertEqual(rev_edges[0], set()) - self.assertEqual(rev_edges[1], set()) - self.assertEqual(rev_edges[2], {0, 1}) - self.assertEqual(rev_edges[3], {2}) - - -class TestTreeNode(unittest.TestCase): - - def test_solve2_straight(self): - infos = get_scenario1() - message_tracking_tags = MessageTrackingTags() - for info in infos: - message_tracking_tags.add(info) - graph = TopicGraph(message_tracking_tags, skips={}) - solver = InputSensorStampSolver(graph) - - tgt_stamp = sorted(message_tracking_tags.stamps('topic3'))[0] - results = solver.solve2( - message_tracking_tags, - 'topic3', - tgt_stamp - ) - - r3 = results - self.assertEqual(r3.name, 'topic3') - self.assertEqual(len(r3.data), 1) - self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) - self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp.sec, 10) - - r2 = r3.name2child['topic2'] - self.assertEqual(r2.name, 'topic2') - self.assertEqual(len(r2.data), 1) - self.assertEqual(len(r2.data[0].in_infos['topic1']), 1) - self.assertEqual(r2.data[0].in_infos['topic1'][0].stamp.sec, 10) - - r1 = r2.name2child['topic1'] - self.assertEqual(r1.name, 'topic1') - self.assertEqual(len(r1.data), 1) - self.assertEqual(len(r1.data[0].in_infos.keys()), 0) - - def test_solve2_branched(self): - t0 = Time(seconds=10, nanoseconds=0) - t0_steady = Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME) - period = Duration(nanoseconds=100 * 10**6) - nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] - cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] - - message_tracking_tags = MessageTrackingTags() - st = t0 - st_steady = t0_steady - - # st: 10.0, 10.1, 10.2, ... - - for i in range(100): - if i == 75: - test_st = Time.from_msg(st.to_msg()) - - infos = gen_scenario2(st, st_steady, nw_dur, cb_dur) - for info in infos: - message_tracking_tags.add(info) - st += period - st_steady += period - - graph = TopicGraph(message_tracking_tags, skips={}) - solver = InputSensorStampSolver(graph) - - sorted_stamps = sorted(message_tracking_tags.stamps('topic4')) - - tgt_stamp = sorted_stamps[75] - results = solver.solve2( - message_tracking_tags, - 'topic4', - tgt_stamp) - - test_stamp1 = test_st - test_stamp2 = test_st + nw_dur - test_stamp3 = test_st + nw_dur + cb_dur - test_stamp4 = test_stamp3 + nw_dur + cb_dur - - r4 = results - self.assertEqual(r4.name, 'topic4') - self.assertEqual(r4.data[0].out_info.stamp, - test_stamp4.to_msg()) - self.assertEqual(len(r4.data), 1) - self.assertEqual(len(r4.data[0].in_infos['topic3']), 1) - self.assertEqual(r4.data[0].in_infos['topic3'][0].stamp, - test_stamp3.to_msg()) - - r3 = r4.name2child['topic3'] - self.assertEqual(r3.name, 'topic3') - self.assertEqual(len(r3.data), 1) - self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) - self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp, - test_stamp2.to_msg()) - self.assertEqual(len(r3.data[0].in_infos['topic1']), 1) - self.assertEqual(r3.data[0].in_infos['topic1'][0].stamp, - test_stamp1.to_msg()) - - one_hot_durations = calc_one_hot(results) - self.assertEqual(len(one_hot_durations), 4) - dur4 = one_hot_durations[0] - self.assertEqual(dur4[1], 'topic4') - self.assertEqual(dur4[3], 0) - - dur3 = one_hot_durations[1] - self.assertEqual(dur3[1], 'topic3') - self.assertEqual(dur3[3], 11) - - dur2 = one_hot_durations[3] - self.assertEqual(dur2[1], 'topic2') - self.assertEqual(dur2[3], 22) - - dur1 = one_hot_durations[2] - self.assertEqual(dur1[1], 'topic1') - self.assertEqual(dur1[3], 22) - - def setup_only_topic4_scenario(self): - """Test lossy scenario.""" - t0 = Time(seconds=10, nanoseconds=0) - t0_steady = Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME) - nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] - cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] - period = Duration(seconds=1) - - def init_solver(): - infos = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) - message_tracking_tags = MessageTrackingTags() - for info in infos: - message_tracking_tags.add(info) - graph = TopicGraph(message_tracking_tags, skips={}) - solver = InputSensorStampSolver(graph) - return solver - - def get_message_tracking_tags_with_only_topic4(): - t1 = t0 + period - t1_steady = t0_steady + period - infos = gen_scenario2(t1, t1_steady, nw_dur, cb_dur) - message_tracking_tags = MessageTrackingTags() - message_tracking_tags.add(infos[-1]) - return message_tracking_tags - - solver = init_solver() - tgt_message_tracking_tags = get_message_tracking_tags_with_only_topic4() - - return (solver, tgt_message_tracking_tags) - - def test_solve_empty(self): - # setup - solver, _ = self.setup_only_topic4_scenario() - tgt_topic = 'topic4' - - results = solver._solve_empty(tgt_topic) - - result_topic4 = results - self.assertEqual(result_topic4.name, 'topic4') - self.assertEqual(len(result_topic4.data), 0) - - result_topic3 = result_topic4.name2child['topic3'] - self.assertEqual(result_topic3.name, 'topic3') - self.assertEqual(len(result_topic3.data), 0) - - result_topic2 = result_topic3 .name2child['topic2'] - self.assertEqual(result_topic2.name, 'topic2') - self.assertEqual(len(result_topic2.data), 0) - - result_topic1 = result_topic3.name2child['topic1'] - self.assertEqual(result_topic1.name, 'topic1') - self.assertEqual(len(result_topic1.data), 0) - - def test_solve2_with_loss(self): - """Solve with lossy MessageTrackingTags.""" - solver, tgt_message_tracking_tags = self.setup_only_topic4_scenario() - - tgt_topic = 'topic4' - tgt_stamps = tgt_message_tracking_tags.stamps(tgt_topic) - - self.assertEqual(len(tgt_stamps), 1) - results = solver.solve2(tgt_message_tracking_tags, tgt_topic, tgt_stamps[0]) - - # is there full graph? - result_topic4 = results - self.assertEqual(result_topic4.name, 'topic4') - self.assertEqual(len(result_topic4.data), 1) - self.assertTrue(isinstance(result_topic4.data[0], MessageTrackingTag)) - - result_topic3 = result_topic4.name2child['topic3'] - self.assertEqual(result_topic3.name, 'topic3') - self.assertEqual(len(result_topic3.data), 0) - - result_topic2 = result_topic3.name2child['topic2'] - self.assertEqual(result_topic2.name, 'topic2') - self.assertEqual(len(result_topic2.data), 0) - - result_topic1 = result_topic3.name2child['topic1'] - self.assertEqual(result_topic1.name, 'topic1') - self.assertEqual(len(result_topic1.data), 0) - - def test_update_stat(self): - t = [] - ts = [] - nw_dur = [] - cb_dur = [] - - t.append(Time(seconds=10, nanoseconds=0)) - ts.append(Time(seconds=0, nanoseconds=1, - clock_type=ClockType.STEADY_TIME)) - nw_dur.append(Duration(nanoseconds=10 * 10**6)) - cb_dur.append(Duration(nanoseconds=1 * 10**6)) - - period1 = Duration(nanoseconds=100 * 10**6) - t.append(t[-1] + period1) - ts.append(ts[-1] + period1) - nw_dur.append(Duration(nanoseconds=20 * 10**6)) - cb_dur.append(Duration(nanoseconds=2 * 10**6)) - - period2 = Duration(nanoseconds=100 * 10**6) - t.append(t[-1] + period2) - ts.append(ts[-1] + period2) - nw_dur.append(Duration(nanoseconds=30 * 10**6)) - cb_dur.append(Duration(nanoseconds=3 * 10**6)) - - message_tracking_tags = MessageTrackingTags() - - for i in range(len(t)): - infos = gen_scenario2(t[i], ts[i], - nw_dur[i], cb_dur[i]) - for info in infos: - message_tracking_tags.add(info) - - graph = TopicGraph(message_tracking_tags, skips={}) - solver = InputSensorStampSolver(graph) - - tgt_stamp = sorted(message_tracking_tags.stamps('topic4'))[-1] - results = solver.solve2( - message_tracking_tags, - 'topic4', - tgt_stamp) - results = update_stat(results) - - self.assertEqual(results.data, [(0, 0)]) - topic3 = results.get_child('topic3') - self.assertEqual(topic3.data, [(33, 33)]) - - topic2 = topic3.get_child('topic2') - self.assertEqual(topic2.data, [(66, 66)]) - - topic1 = topic3.get_child('topic1') - self.assertEqual(topic1.data, [(66, 66)]) - - -if __name__ == '__main__': - unittest.main() diff --git a/src/tilde_vis/test/test_ncurse_printer.py b/src/tilde_vis/test/test_ncurse_printer.py deleted file mode 100644 index c111dc34..00000000 --- a/src/tilde_vis/test/test_ncurse_printer.py +++ /dev/null @@ -1,35 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2021 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. - -import curses -import sys -import time - -from tilde_vis.printer import ( - NcursesPrinter - ) - - -def main(stdscr, args=None): - printer = NcursesPrinter(stdscr) - while True: - now = time.time() - lines = [f'{now}: {i}' for i in range(100)] - printer.print_(lines) - time.sleep(1) - - -if __name__ == '__main__': - curses.wrapper(main, args=sys.argv) diff --git a/src/tilde_vis/test/test_pep257.py b/src/tilde_vis/test/test_pep257.py deleted file mode 100644 index b234a384..00000000 --- a/src/tilde_vis/test/test_pep257.py +++ /dev/null @@ -1,23 +0,0 @@ -# Copyright 2015 Open Source Robotics Foundation, 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. - -from ament_pep257.main import main -import pytest - - -@pytest.mark.linter -@pytest.mark.pep257 -def test_pep257(): - rc = main(argv=['.', 'test']) - assert rc == 0, 'Found code style errors / warnings' diff --git a/src/tilde_vis/tilde_vis/__init__.py b/src/tilde_vis/tilde_vis/__init__.py deleted file mode 100644 index 141c8594..00000000 --- a/src/tilde_vis/tilde_vis/__init__.py +++ /dev/null @@ -1,15 +0,0 @@ -# Copyright 2021 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. - -"""Package tilde_vis.""" diff --git a/src/tilde_vis/tilde_vis/caret_vis_tilde.py b/src/tilde_vis/tilde_vis/caret_vis_tilde.py deleted file mode 100755 index e7819aaa..00000000 --- a/src/tilde_vis/tilde_vis/caret_vis_tilde.py +++ /dev/null @@ -1,277 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -"""Check CARET vs TILDE results.""" - -from collections import defaultdict - -from bokeh.models import CrosshairTool -from bokeh.plotting import figure, show -import numpy as np -import pandas as pd - -from tilde_vis.message_tracking_tag import ( - MessageTrackingTag, - MessageTrackingTags -) -from tilde_vis.message_tracking_tag_traverse import ( - InputSensorStampSolver, - TopicGraph -) - - -def time2str(t): - """Convert builtin_interfaces.msg.Time to string.""" - return f'{t.sec}.{t.nanosec:09d}' - - -def time2int(t): - """Convert builtin_interfaces.msg.Time to int.""" - return t.sec * 10**9 + t.nanosec - - -def _rosbag_iter(rosbag_path): - from rclpy.serialization import deserialize_message - from rosidl_runtime_py.utilities import get_message - import rosbag2_py - - storage_option = rosbag2_py.StorageOptions(rosbag_path, 'sqlite3') - - converter_option = rosbag2_py.ConverterOptions('cdr', 'cdr') - reader = rosbag2_py.SequentialReader() - - reader.open(storage_option, converter_option) - - topic_types = reader.get_all_topics_and_types() - type_map = {topic_types[i].name: topic_types[i].type - for i in range(len(topic_types))} - - while reader.has_next(): - (topic, data, t) = reader.read_next() - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - yield topic, msg, t - - -def read_msgs(rosbag_path): - """ - Read demo messages from rosbag. - - Returns - ------- - dictionary such as - { - : [] - } - - """ - msg_topics = defaultdict(list) - for topic, msg, t in _rosbag_iter(rosbag_path): - msg_topics[topic].append(msg) - return msg_topics - - -def read_message_tracking_tag(raw_msgs): - """Convert raw messages to MessageTrackingTags.""" - message_tracking_tags = MessageTrackingTags() - for i in range(5): - msgs = raw_msgs[f'/topic{i+1}/message_tracking_tag'] - for msg in msgs: - message_tracking_tags.add(MessageTrackingTag.fromMsg(msg)) - return message_tracking_tags - - -def get_uuid2msg(raw_msgs): - """Convert raw messages to dictionary.""" - hashed_msgs = {} - hash_target_keys = ['/topic1', '/topic2', '/topic3', '/topic4', '/topic5'] - for hash_target_key in hash_target_keys: - hashed_msgs.update({ - msg.uuid: msg for msg in raw_msgs[hash_target_key] - }) - return hashed_msgs - - -def build_latency_table(traces, ds): - """ - Calculate latencies. - - Parameters - ---------- - traces: list - CARET traces - ds: list - list to output - - """ - traces_dict = {trace.uuid: trace for trace in traces} - - from copy import deepcopy - - def search_flow(start_uuid): - flows = [] - - # 後続のトレースを取得 - def get_subsequence(uuid): - return [trace for trace in traces if uuid in trace.used_uuids] - - def search_local(uuid, flow): - flow.append(uuid) - - subsequence = get_subsequence(uuid) - if len(subsequence) == 0: - flows.append(flow) - - for sub in subsequence: - search_local(sub.uuid, deepcopy(flow)) - - search_local(start_uuid, []) - - return flows - - # start_msgs = msgs_topics['/topic1'] - start_msgs = [trace.uuid for trace in traces - if '/topic1' in trace.uuid and trace.trace_type == 'publish'] - - uid_flows = [] - for msg in start_msgs: - uid_flows += search_flow(msg) - - def trace_to_column(trace): - return f'{trace.node_name}_{trace.trace_type}' - - from collections import defaultdict - for uid_flow in uid_flows: - try: - flow = [traces_dict[uid] for uid in uid_flow] - except KeyError: - flow = [] - d = defaultdict() - - d.update({trace_to_column(trace): trace.steady_t for trace in flow}) - - ds.append(d) - - return ds - -############################ -# visualize tilde -############################ - - -class Trace(object): - """Trace data for CARET.""" - - def __init__(self, node_name, uuid, stamp, is_publish, uuids): - """Initialize trace.""" - self.node_name = node_name # "" - self.uuid = uuid # "_" - self.steady_t = stamp - self.trace_type = 'publish' if is_publish else 'callback_start' - self.used_uuids = uuids - - def __repr__(self): - """Print self.""" - return ( - ' ' - f'node_name={self.node_name} uuid={self.uuid} ' - f'steady_t={self.steady_t} trace_type={self.trace_type} ' - f'used_uuids={self.used_uuids}') - - -def vis_tilde(message_tracking_tags): - """Visualize tilde result.""" - tgt_topic = '/topic5' - topic5_stamps = message_tracking_tags.stamps(tgt_topic) - - graph = TopicGraph(message_tracking_tags) - solver = InputSensorStampSolver(graph) - - ds = [] - for stamp in topic5_stamps: - tilde_path = solver.solve2(message_tracking_tags, tgt_topic, stamp) - - traces = [] - - def update_traces(node): - topic = node.name - for d in node.data: - stamp = time2int(d.out_info.stamp) - pub_time = time2int(d.out_info.pubsub_stamp) - uuid = f'{topic}_{stamp}' - uuids = [] - for in_topic, infos in d.in_infos.items(): - for i in infos: - i_stamp = time2int(i.stamp) - sub_time = time2int(i.pubsub_stamp) - i_uuid = f'{in_topic}_{i_stamp}' - uuids.append(i_uuid) - - # append callback_start - trace = Trace(in_topic, i_uuid, sub_time, False, []) - traces.append(trace) - # append publish - traces.append(Trace(topic, uuid, pub_time, True, uuids)) - - tilde_path.apply(update_traces) - - ds = build_latency_table(traces, ds) - - return ds - - -def plot_latency_table(df): - """Plot flows from df.""" - from tqdm import tqdm - from bokeh.palettes import Bokeh8 - - def get_color(i): - cmap = Bokeh8 - i = i % (len(cmap)) - return cmap[i] - - fig = figure( - x_axis_label='Time [s]', - y_axis_label='', - plot_width=1000, - plot_height=400, - active_scroll='wheel_zoom', - ) - fig.add_tools(CrosshairTool(line_alpha=0.4)) - - for i, row in tqdm(df.iterrows()): - row_ = row.dropna() - - y = np.array(list(range(len(row_)))) * -1 - x = list(row_.values) - - fig.line(x, y, line_color=get_color(i), line_alpha=0.4) - show(fig) - - -def main(): - """Run main.""" - bagfile = 'rosbag2_2022_02_16-18_12_46' - raw_msgs = read_msgs(bagfile) - message_tracking_tags = read_message_tracking_tag(raw_msgs) - ds = vis_tilde(message_tracking_tags) - df = pd.DataFrame.from_dict(ds) - - plot_latency_table(df) - - -if __name__ == '__main__': - """Main.""" - main() diff --git a/src/tilde_vis/tilde_vis/data_as_tree.py b/src/tilde_vis/tilde_vis/data_as_tree.py deleted file mode 100644 index 0ca7d43d..00000000 --- a/src/tilde_vis/tilde_vis/data_as_tree.py +++ /dev/null @@ -1,128 +0,0 @@ -# Copyright 2021 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. - -"""Tree like data structure for MessageTrackingTag traversal.""" - - -class TreeNode(object): - """ - Node of DataAsTree. - - This hold data list and children. - - """ - - def __init__(self, name): - """Initialize tree structure.""" - self.name = name - self.children = [] - self.name2child = {} - self.data = [] - self.value = None - - def num_children(self): - """Get the number of children.""" - return len(self.children) - - def get_child(self, child_name): - """ - Get child. - - If not exists, then append. - - """ - children = self.children - name2child = self.name2child - - if child_name in name2child.keys(): - return name2child[child_name] - - c = TreeNode(child_name) - children.append(c) - children.sort(key=lambda x: x.name) - name2child[child_name] = c - - return c - - def add_data(self, d): - """ - Add data. - - :param d: any value or object - - """ - self.data.append(d) - - def apply(self, fn): - """ - Apply fn recursively. - - Internally, pre-order DFS is used. - - :param fn: callable, which takes TreeNode - :return list of fn return - - """ - children = self.children - ret = [] - - ret.append(fn(self)) - for c in children: - ret.extend(c.apply(fn)) - - return ret - - def apply_with_depth(self, fn, depth=0): - """ - Apply with depth option. - - Internally, pre-order DFS is used. - - :param fn: callback such as fn(tree_node_obj, depth) - :return list of fn return - - """ - children = self.children - ret = [] - - ret.append(fn(self, depth)) - for c in children: - ret.extend(c.apply_with_depth(fn, depth + 1)) - - return ret - - def merge(self, rhs): - """ - Merge data of another tree. - - If self does not have some keys which rhs has, - then new nodes are added. - - :param rhs: another TreeNode - - """ - def _merge(lhs, rhs): - """ - Merge data. - - lhs: TreeNode which is updated - rhs: TreeNode which is const - """ - lhs.data.extend(rhs.data) - - for rhs_c in rhs.children: - lhs_c = lhs.get_child(rhs_c.name) - _merge(lhs_c, rhs_c) - - _merge(self, rhs) diff --git a/src/tilde_vis/tilde_vis/latency_viewer.py b/src/tilde_vis/tilde_vis/latency_viewer.py deleted file mode 100644 index 1c305f57..00000000 --- a/src/tilde_vis/tilde_vis/latency_viewer.py +++ /dev/null @@ -1,792 +0,0 @@ -#!env python3 -# Copyright 2021 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. - -"""Latency viewer node and its main function.""" - -import argparse -import curses -import os -import pickle -from statistics import mean -import sys -import time - -from builtin_interfaces.msg import Time as TimeMsg -import rclpy -from rclpy.node import Node -from rclpy.qos import ( - QoSHistoryPolicy, - QoSProfile, - QoSReliabilityPolicy - ) -from rclpy.time import Time - -from tilde_msg.msg import ( - MessageTrackingTag, - ) -from tilde_vis.message_tracking_tag import ( - MessageTrackingTag as MessageTrackingTagObj, - MessageTrackingTags as MessageTrackingTagsObj, - time2str - ) -from tilde_vis.message_tracking_tag_traverse import ( - InputSensorStampSolver, - TopicGraph - ) -from tilde_vis.printer import ( - NcursesPrinter, - Printer - ) - - -EXCLUDES_TOPICS = [ - '/diagnostics/message_tracking_tag', - '/control/trajectory_follower/mpc_follower/debug/markers/message_tracking_tag', # noqa: #501 - '/control/trajectory_follower/mpc_follower/debug/steering_cmd/message_tracking_tag', # noqa: #501 - '/localization/debug/ellipse_marker/message_tracking_tag', - '/localization/pose_twist_fusion_filter/debug/message_tracking_tag', - '/localization/pose_twist_fusion_filter/debug/measured_pose/message_tracking_tag', # noqa: #501 - '/localization/pose_twist_fusion_filter/debug/stop_flag/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/area_with_objects/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/clearance_map/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/object_clearance_map/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/smoothed_points/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw/message_tracking_tag', # noqa: #501 - '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled/message_tracking_tag', # noqa: #501 - ] -LEAVES = [ - '/initialpose', - '/map/pointcloud_map', - '/sensing/lidar/top/rectified/pointcloud', - '/sensing/imu/imu_data', - '/vehicle/status/twist', - ] -MESSAGE_TRACKING_TAG = 'topic_infos.pkl' -TIMER_SEC = 1.0 -TARGET_TOPIC = '/sensing/lidar/concatenated/pointcloud' -STOPS = [ - '/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias', # noqa: #501 - '/perception/occupancy_grid_map/map', - '/perception/object_recognition/detection/detection_by_tracker/objects', - '/perception/object_recognition/tracking/objects', - '/localization/pose_twist_fusion_filter/biased_pose_with_covariance', - ] -DUMP_DIR = 'dump.d' - - -def truncate(s, left_length=20, n=80): - """ - Truncate string. - - :param s: string - :param left_length: first part length - :param n: total length - :return truncated string such that "abc...edf" - - """ - assert left_length + 5 < n - - if len(s) < n: - return s - - pre = s[:left_length] - post = s[len(s) - n + left_length + 5:] - - return pre + '...' + post - - -class LatencyStat(object): - """Latency statistics.""" - - def __init__(self): - """Initialize data.""" - self.dur_ms_list = [] - self.dur_pub_ms_list = [] - self.dur_pub_ms_steady_list = [] - self.is_leaf_list = [] - - def add(self, r): - """ - Add single result. - - :param r: message_tracking_tag_traverse.SolverResults - - """ - self.dur_ms_list.append(r.dur_ms) - self.dur_pub_ms_list.append(r.dur_pub_ms) - self.dur_pub_ms_steady_list.append(r.dur_pub_ms_steady) - self.is_leaf_list.append(r.is_leaf) - - def report(self): - """Report statistics.""" - dur_ms_list = self.dur_ms_list - is_leaf_list = self.is_leaf_list - dur_pub_ms_list = self.dur_pub_ms_list - dur_pub_ms_steady_list = self.dur_pub_ms_steady_list - - dur_min = float(min(dur_ms_list)) - dur_mean = float(mean(dur_ms_list)) - dur_max = float(max(dur_ms_list)) - - dur_pub_min = float(min(dur_pub_ms_list)) - dur_pub_mean = float(mean(dur_pub_ms_list)) - dur_pub_max = float(max(dur_pub_ms_list)) - - dur_pub_steady_min = float(min(dur_pub_ms_steady_list)) - dur_pub_steady_mean = float(mean(dur_pub_ms_steady_list)) - dur_pub_steady_max = float(max(dur_pub_ms_list)) - - is_all_leaf = all(is_leaf_list) - - return { - 'dur_min': dur_min, - 'dur_mean': dur_mean, - 'dur_max': dur_max, - 'dur_pub_min': dur_pub_min, - 'dur_pub_mean': dur_pub_mean, - 'dur_pub_max': dur_pub_max, - 'dur_pub_steady_min': dur_pub_steady_min, - 'dur_pub_steady_mean': dur_pub_steady_mean, - 'dur_pub_steady_max': dur_pub_steady_max, - 'is_all_leaf': is_all_leaf, - } - - -class PerTopicLatencyStat(object): - """Per topic latency statistics.""" - - def __init__(self): - """Initialize data.""" - self.data = {} - - def add(self, r): - """ - Add single result. - - :param r: message_tracking_tag_traverse.SolverResults - - """ - self.data.setdefault(r.topic, LatencyStat()).add(r) - - def report(self): - """Get report as dictionary.""" - ret = {} - for (topic, stat) in self.data.items(): - ret[topic] = stat.report() - return ret - - def print_report(self, printer): - """ - Print report. - - :param printer: see printer.py - - """ - logs = [] - reports = self.report() - logs.append('{:80} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}'.format( - 'topic', 'dur', 'dur', 'dur', 'e2e', 'e2e', 'e2e', 'e2e_s', 'e2e_s', 'e2e_s' - )) - - def p(v): - if v > 1000: - return ' inf' - else: - return '{:>6.1f}'.format(v) - - for (topic, report) in reports.items(): - s = f'{topic:80} ' - s += f'{p(report["dur_min"])} ' - s += f'{p(report["dur_mean"])} ' - s += f'{p(report["dur_max"])} ' - s += f'{p(report["dur_pub_min"])} ' - s += f'{p(report["dur_pub_mean"])} ' - s += f'{p(report["dur_pub_max"])} ' - s += f'{p(report["dur_pub_steady_min"])} ' - s += f'{p(report["dur_pub_steady_mean"])} ' - s += f'{p(report["dur_pub_steady_max"])} ' - s += f'{report["is_all_leaf"]}' - logs.append(s) - - printer.print_(logs) - - -def calc_one_hot(results): - """ - Calculate one hot result. - - :param results: see InputSensorStampSolver.solve2 - :return list of (depth, topic name, dur, dur_steady, is_leaf) - depth: depth of watched topic - topic_name: watched topic - dur: final topic pub_time - sensor pub_time - dur_steady: steady version of dur - is_leaf: leaf of not [bool] - stamp: target topic output stamp - - """ - last_pub_time = Time.from_msg(results.data[0].out_info.pubsub_stamp) - last_pub_time_steady = Time.from_msg( - results.data[0].out_info.pubsub_stamp_steady) - - def calc(node, depth): - name = node.name - is_leaf = node.num_children() == 0 - - if len(node.data) == 0 or node.data[0] is None: - return (depth, name, None, None, is_leaf, None) - - stamp = node.data[0].out_info.stamp - pub_time = Time.from_msg(node.data[0].out_info.pubsub_stamp) - dur = last_pub_time - pub_time - dur_ms = dur.nanoseconds // (10 ** 6) - - pub_time_steady = Time.from_msg( - node.data[0].out_info.pubsub_stamp_steady) - dur_steady = last_pub_time_steady - pub_time_steady - dur_ms_steady = dur_steady.nanoseconds // (10 ** 6) - - return (depth, name, dur_ms, dur_ms_steady, is_leaf, stamp) - - return results.apply_with_depth(calc) - - -def handle_stat(stamps, message_tracking_tags, target_topic, solver, stops, - dumps=False): - """ - Calculate latency statistics. - - :param stamps: stamp - :param message_tracking_tags: message tracking tag - :param target_topic: target topic - :param solver: InputSensorStampSolver - :param stops: list of stop targets - :param dumps: dump pkl file if True for debug - :return TreeNode. - .data: see calc_stat - - """ - idx = -3 - if len(stamps) == 0: - return - elif len(stamps) < 3: - idx = 1 - - merged = None - print(f'idx: {idx}') - for target_stamp in stamps[:idx]: - results = solver.solve2( - message_tracking_tags, target_topic, target_stamp, - stops=stops) - - if dumps: - pickle.dump(results, - open(f'{DUMP_DIR}/stat_results_{target_stamp}.pkl', - 'wb'), - protocol=pickle.HIGHEST_PROTOCOL) - - results = update_stat(results) - if merged is None: - merged = results - else: - merged.merge(results) - - stats = calc_stat(merged) - return stats - - -def update_stat(results): - """ - Update results to have durations. - - :param results: TreeNode see InputSensorStampSolver.solve2 - :return updated results with - results.data: [(dur_ms, dur_ms_steady)] - results.data_orig = results.data - - """ - last_pub_time = Time.from_msg( - results.data[0].out_info.pubsub_stamp) - last_pub_time_steady = Time.from_msg( - results.data[0].out_info.pubsub_stamp_steady) - - def _update_stat(node): - if len(node.data) == 0 or node.data[0] is None: - node.data_orig = node.data - node.data = [(None, None)] - return - - pub_time = Time.from_msg( - node.data[0].out_info.pubsub_stamp) - dur = last_pub_time - pub_time - dur_ms = dur.nanoseconds // (10 ** 6) - - pub_time_steady = Time.from_msg( - node.data[0].out_info.pubsub_stamp_steady) - dur_steady = last_pub_time_steady - pub_time_steady - dur_ms_steady = dur_steady.nanoseconds // (10 ** 6) - - node.data_orig = node.data - node.data = [(dur_ms, dur_ms_steady)] - - results.apply(_update_stat) - - return results - - -def calc_stat(results): - """ - Calculate statistics. - - :param results: TreeNode which is updated by update_stat - :return list of dictionary whose keys are: - "depth" - "name" - "dur_min" - "dur_mean" - "dur_max" - "dur_min_steady" - "dur_mean_steady" - "dur_max_steady" - "is_leaf" - - """ - def _calc_stat(node, depth): - duration = [] - duration_steady = [] - is_leaf = True if node.num_children() == 0 else False - for d in node.data: - if d[0] is not None: - duration.append(d[0]) - if d[1] is not None: - duration_steady.append(d[1]) - - def _calc(arr, fn): - if len(arr) == 0: - return None - return fn(arr) - - return { - 'depth': depth, - 'name': node.name, - 'dur_min': _calc(duration, min), - 'dur_mean': _calc(duration, mean), - 'dur_max': _calc(duration, max), - 'dur_min_steady': _calc(duration_steady, min), - 'dur_mean_steady': _calc(duration_steady, mean), - 'dur_max_steady': _calc(duration_steady, max), - 'is_leaf': is_leaf - } - - return results.apply_with_depth(_calc_stat) - - -class LatencyViewerNode(Node): - """Latency viewer node.""" - - def __init__(self, stdscr=None): - """ - Initialize Node. - - :param stdscr: if not None, use ncurses - - """ - super().__init__('latency_viewer_node') - self.declare_parameter('excludes_topics', EXCLUDES_TOPICS) - self.declare_parameter('leaves', LEAVES) - self.declare_parameter('graph_pkl', '') - self.declare_parameter('timer_sec', TIMER_SEC) - self.declare_parameter('target_topic', TARGET_TOPIC) - self.declare_parameter('keep_info_sec', 3) - self.declare_parameter('wait_sec_to_init_graph', 10) - self.declare_parameter('mode', 'stat') - self.declare_parameter('stops', STOPS) - # whether to dump solver.solve() result - self.declare_parameter('dumps', False) - - print(stdscr) - if stdscr is not None: - self.printer = NcursesPrinter(stdscr) - else: - self.printer = Printer() - - self.subs = {} - # topic vs the latest sequence numbers - self.topic_seq = {} - - excludes_topic = ( - self.get_parameter('excludes_topics') - .get_parameter_value().string_array_value) - topics = self.get_message_tracking_tag_topics(excludes=excludes_topic) - logs = [] - - qos = QoSProfile( - history=QoSHistoryPolicy.KEEP_LAST, - depth=3, - reliability=QoSReliabilityPolicy.BEST_EFFORT, - ) - for topic in topics: - logs.append(topic) - sub = self.create_subscription( - MessageTrackingTag, - topic, - self.listener_callback, - qos) - self.subs[topic] = sub - - self.solver = None - self.stops = ( - self.get_parameter('stops') - .get_parameter_value().string_array_value) - graph_pkl = ( - self.get_parameter('graph_pkl') - .get_parameter_value().string_value) - if graph_pkl: - graph = pickle.load(open(graph_pkl, 'rb')) - self.solver = InputSensorStampSolver(graph) - - self.message_tracking_tags = MessageTrackingTagsObj() - - timer_sec = ( - self.get_parameter('timer_sec') - .get_parameter_value().double_value) - self.timer = self.create_timer(timer_sec, - self.timer_callback) - - self.target_topic = ( - self.get_parameter('target_topic') - .get_parameter_value().string_value) - self.keep_info_sec = ( - self.get_parameter('keep_info_sec') - .get_parameter_value().integer_value) - self.wait_sec_to_init_graph = ( - self.get_parameter('wait_sec_to_init_graph'). - get_parameter_value().integer_value) - self.wait_init = 0 - - self.init_skips() - - self.printer.print_(logs) - - self.dumps = ( - self.get_parameter('dumps') - .get_parameter_value().bool_value) - if self.dumps: - os.makedirs(DUMP_DIR, exist_ok=True) - - def init_skips(self): - """ - Define skips. - - See TopicGraph.__init__ comment. - """ - skips = {} - RECT_OUT_EX = '/sensing/lidar/{}/rectified/pointcloud_ex' - RECT_OUT = '/sensing/lidar/{}/rectified/pointcloud' - RECT_IN = '/sensing/lidar/{}/mirror_cropped/pointcloud_ex' - - # top - for pos in ['top', 'left', 'right']: - skips[RECT_OUT_EX.format(pos)] = RECT_IN.format(pos) - skips[RECT_OUT.format(pos)] = RECT_IN.format(pos) - - skips['/sensing/lidar/no_ground/pointcloud'] = \ - '/sensing/lidar/concatenated/pointcloud' - - self.skips = skips - - def listener_callback(self, message_tracking_tag_msg): - """Handle MessageTrackingTag message.""" - st = time.time() - output_info = message_tracking_tag_msg.output_info - - topic = output_info.topic_name - stamp = time2str(output_info.header_stamp) - - this_seq = output_info.seq - if topic not in self.topic_seq.keys(): - self.topic_seq[topic] = this_seq - seq = self.topic_seq[topic] - - if this_seq < seq: # skew - s = f'skew topic={topic} ' + \ - f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ - f' saved_seq={seq}' - stamps = self.message_tracking_tags.stamps(topic) - if stamps or len(stamps) > 0: - last_stamp = sorted(stamps)[-1] - s += f'({last_stamp})' - self.get_logger().info(s) - elif seq + 1 < this_seq: # message drop happens - s = f'may drop topic={topic} ' + \ - f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ - f' saved_seq={seq}' - stamps = self.message_tracking_tags.stamps(topic) - if stamps or len(stamps) > 0: - last_stamp = sorted(stamps)[-1] - s += f'({last_stamp})' - self.get_logger().info(s) - self.topic_seq[topic] = this_seq - else: - self.topic_seq[topic] = this_seq - - # add message_tracking_tag - message_tracking_tag = MessageTrackingTagObj( - output_info.topic_name, - output_info.pub_time, - output_info.pub_time_steady, - output_info.has_header_stamp, - output_info.header_stamp) - for input_info in message_tracking_tag_msg.input_infos: - message_tracking_tag.add_input_info( - input_info.topic_name, - input_info.sub_time, - input_info.sub_time_steady, - input_info.has_header_stamp, - input_info.header_stamp) - self.message_tracking_tags.add(message_tracking_tag) - - et = time.time() - - elapsed_ms = (et - st) * 1000 - if elapsed_ms > 1: - self.get_logger().info( - f'sub {topic} at {stamp}@ {elapsed_ms} [ms]') - - def handle_stat(self, stamps): - """ - Report statistics. - - :param stamps: stamps sorted by old-to-new order - :return list of string - - """ - message_tracking_tags = self.message_tracking_tags - target_topic = self.target_topic - solver = self.solver - stops = self.stops - dumps = self.dumps - - stats = handle_stat(stamps, - message_tracking_tags, - target_topic, - solver, - stops, - dumps=dumps) - - logs = [] - - fmt = '{:80} ' + \ - '{:>6} {:>6} {:>6} ' + \ - '{:>6} {:>6} {:>6}' - logs.append(fmt.format( - 'topic', - 'min', 'mean', 'max', - 'min_s', 'mean_s', 'max_s' - )) - - for stat in stats: - name = (' ' * stat['depth'] + - stat['name'] + - ('*' if stat['is_leaf'] else '')) - name = truncate(name) - - def p(v): - if v is None: - s = 'NA' - return f'{s:>6}' - if v > 1000: - s = 'inf' - return f'{s:>6}' - else: - return f'{v:>6.1f}' - - s = f'{name:80} ' - s += f'{p(stat["dur_min"]):>6} ' - s += f'{p(stat["dur_mean"]):>6} ' - s += f'{p(stat["dur_max"]):>6} ' - s += f'{p(stat["dur_min_steady"]):>6} ' - s += f'{p(stat["dur_mean_steady"]):>6} ' - s += f'{p(stat["dur_max_steady"]):>6} ' - - logs.append(s) - - return logs - - def handle_one_hot(self, stamps): - """ - Report only one message. - - :param stamps: stamps sorted by old-to-new order - :return array of strings for log - - """ - message_tracking_tags = self.message_tracking_tags - target_topic = self.target_topic - solver = self.solver - stops = self.stops - - # [-3] has no specific meaning. - # But [-1] may not have full info. So we look somewhat old info. - idx = -3 - if len(stamps) == 0: - return - elif len(stamps) < 3: - idx = 0 - - target_stamp = stamps[idx] - results = solver.solve2(message_tracking_tags, target_topic, - target_stamp, stops=stops) - - if self.dumps: - pickle.dump(results, - open(f'{DUMP_DIR}/one_hot_results_{target_stamp}.pkl', - 'wb'), - protocol=pickle.HIGHEST_PROTOCOL) - - one_hot_durations = calc_one_hot(results) - logs = [] - for e in one_hot_durations: - (depth, name, dur_ms, dur_ms_steady, is_leaf, stamp) = e - name = truncate(' ' * depth + name + ('*' if is_leaf else '')) - if dur_ms is None: - dur_ms = 'NA' - if dur_ms_steady is None: - dur_ms_steady = 'NA' - stamp_s = 'NA' - if stamp: - stamp_s = time2str(stamp) - - s = f'{name:80} {stamp_s:>20} {dur_ms:>6} {dur_ms_steady:>6}' - logs.append(s) - - return logs - - def timer_callback(self): - """Clear old data.""" - st = time.time() - message_tracking_tags = self.message_tracking_tags - target_topic = self.target_topic - - stamps = sorted(message_tracking_tags.stamps(target_topic)) - logs = [] - str_stamp = stamps[0] if len(stamps) > 0 else '' - logs.append(f'stamps: {len(stamps)}, {str_stamp}') - if len(stamps) == 0: - self.printer.print_(logs) - return - - # check header.stamp field existence - if not message_tracking_tags.get(target_topic, stamps[0]).out_info.has_stamp: - logs.append('**WARNING** target topic has no stamp field') - self.printer.print_(logs) - return - - if not self.solver: - if self.wait_init < self.wait_sec_to_init_graph: - self.printer.print_(logs) - self.wait_init += 1 - return - logs.append('init solver') - graph = TopicGraph(message_tracking_tags, skips=self.skips) - self.solver = InputSensorStampSolver(graph) - - mode = self.get_parameter('mode').get_parameter_value().string_value - if mode == 'stat': - logs.extend(self.handle_stat(stamps)) - elif mode == 'one_hot': - logs.extend(self.handle_one_hot(stamps)) - else: - logs.append('unknown mode') - et1 = time.time() - handle_ms = (et1 - st) * 1000 - - # cleanup MessageTrackingTags - (latest_sec, latest_ns) = (int(x) for x in stamps[-1].split('.')) - until_stamp = TimeMsg(sec=latest_sec - self.keep_info_sec, - nanosec=latest_ns) - message_tracking_tags.erase_until(until_stamp) - et2 = time.time() - cleanup_ms = (et2 - et1) * 1000 - - self.printer.print_(logs) - - if handle_ms + cleanup_ms > 30: - s = f'timer handle_ms={handle_ms}' + \ - f' cleanup_ms={cleanup_ms}' - self.get_logger().info(s) - - def get_message_tracking_tag_topics(self, excludes=[]): - """ - Get all topic infos. - - :param excludes: topic names - :return a list of message_tracking_tag topics - - """ - # short sleep between node creation and get_topic_names_and_types - # https://github.com/ros2/ros2/issues/1057 - # We need this sleep with autoware, - # but don't need in dev environment(i.e. MessageTrackingTag in rosbag) - time.sleep(0.5) - - msg_type = 'tilde_msg/msg/MessageTrackingTag' - topic_and_types = self.get_topic_names_and_types() - filtered_topic_and_types = \ - filter(lambda x: msg_type in x[1], topic_and_types) - topics = (x[0] for x in filtered_topic_and_types) - topics = filter(lambda x: x not in excludes, topics) - - return topics - - -def main_curses(stdscr, args=None): - """Wrap main function for ncurses.""" - rclpy.init(args=args) - - node = LatencyViewerNode(stdscr=stdscr) - rclpy.spin(node) - rclpy.shutdown() - - -def main(args=None): - """Run main.""" - parser = argparse.ArgumentParser() - parser.add_argument( - '--batch', action='store_true', - help='Run as batch mode, just like top command `-b` option') - parsed = parser.parse_known_args()[0] - - print(parsed) - - if not parsed.batch: - print('not batch') - curses.wrapper(main_curses, args=sys.argv) - else: - print('batch') - main_curses(None, args=sys.argv) - - -if __name__ == '__main__': - """Main.""" - main(sys.argv) diff --git a/src/tilde_vis/tilde_vis/message_tracking_tag.py b/src/tilde_vis/tilde_vis/message_tracking_tag.py deleted file mode 100644 index a8c46aa5..00000000 --- a/src/tilde_vis/tilde_vis/message_tracking_tag.py +++ /dev/null @@ -1,269 +0,0 @@ -# Copyright 2021 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. - -"""Internal data structures for MessageTrackingTag.""" - -from rclpy.time import Time - - -def time2str(t): - """Convert builtin_interfaces.msg.Time to string.""" - return f'{t.sec}.{t.nanosec:09d}' - - -class TopicInfo(object): - """Represent output_info and input_infos of MessageTrackingTag message.""" - - def __init__(self, topic, pubsub_stamp, pubsub_stamp_steady, - has_stamp, stamp): - """ - Hold information similar to MessageTrackingTag in/out info. - - :param topic: target topic [string] - :param pubsub_stamp: - when publish or subscription callback is called - [builtin_interfaces.msg.Time] - :param pubsub_stamp_steady: - same with above but in steady time - [builtin_interfaces.mg.Time] - :param has_stamp: whether main topic has header.stamp or not [bool] - :param stamp: header.stamp [builtin_interfaces.msg.Time] - - """ - self.topic = topic - self.pubsub_stamp = pubsub_stamp - self.pubsub_stamp_steady = pubsub_stamp_steady - self.has_stamp = has_stamp - self.stamp = stamp - - def __str__(self): - """Get string.""" - stamp_s = time2str(self.stamp) if self.has_stamp else 'NA' - return f'TopicInfo(topic={self.topic}, stamp={stamp_s})' - - -class MessageTrackingTag(object): - """ - Hold information similar to MessageTrackingTagMsg. - - TODO(y-okumura-isp): Can we use MessageTrackingTagMsg directly? - - """ - - def __init__(self, out_topic, pub_time, pub_time_steady, - has_stamp, out_stamp): - """ - Initialize data. - - :param out_topic: topic name - :param pub_time: when publish main topic [builtin_interfaces.msg.Time] - :param has_stamp: whether main topic has header.stamp [bool] - :param out_stamp: main topic header.stamp [builtin_interfaces.msg.Time] - - """ - self.out_info = TopicInfo(out_topic, pub_time, pub_time_steady, - has_stamp, out_stamp) - # topic name vs TopicInfo - self.in_infos = {} - - def add_input_info(self, in_topic, sub_stamp, sub_stamp_steady, - has_stamp, stamp): - """ - Add input info. - - :param in_topic: topic string - :param has_stamp: bool - :param stamp: header stamp [builtin_interfaces.msg.Time] - - """ - info = TopicInfo(in_topic, sub_stamp, sub_stamp_steady, - has_stamp, stamp) - self.in_infos.setdefault(in_topic, []).append(info) - - def __str__(self): - """Get string.""" - s = 'MessageTrackingTag: \n' - s += f' out_info={self.out_info}\n' - for _, infos in self.in_infos.items(): - for info in infos: - s += f' in_infos={info}\n' - return s - - @property - def out_topic(self): - """Get output topic.""" - return self.out_info.topic - - @staticmethod - def fromMsg(message_tracking_tag_msg): - """ - Convert MessageTrackingTagMsg to MessageTrackingTag. - - :param message_tracking_tag_msg: MessageTrackingTagMsg - :return MessageTrackingTag - - """ - output_info = message_tracking_tag_msg.output_info - message_tracking_tag = MessageTrackingTag( - output_info.topic_name, - output_info.pub_time, - output_info.pub_time_steady, - output_info.has_header_stamp, - output_info.header_stamp) - for input_info in message_tracking_tag_msg.input_infos: - message_tracking_tag.add_input_info( - input_info.topic_name, - input_info.sub_time, - input_info.sub_time_steady, - input_info.has_header_stamp, - input_info.header_stamp) - return message_tracking_tag - - -class MessageTrackingTags(object): - """ - Hold topic vs MessageTrackingTag. - - We have double-key dictionary internally, i.e. - we can get MessageTrackingTag by - topic_vs_message_tracking_tag[topic_name][stamp]. - - """ - - def __init__(self): - """Initialize data.""" - # {topic => {stamp_str => MessageTrackingTag}} - self.topic_vs_message_tracking_tags = {} - - def add(self, message_tracking_tag): - """Add a MessageTrackingTag.""" - out_topic = message_tracking_tag.out_info.topic - out_stamp = time2str(message_tracking_tag.out_info.stamp) - if out_topic not in self.topic_vs_message_tracking_tags.keys(): - self.topic_vs_message_tracking_tags[out_topic] = {} - infos = self.topic_vs_message_tracking_tags[out_topic] - - if out_stamp not in infos.keys(): - infos[out_stamp] = {} - - infos[out_stamp] = message_tracking_tag - - def erase_until(self, stamp): - """ - Erase added message_tracking_tag where out_stamp < stamp. - - :param stamp: builtin_interfaces.msg.Time - - """ - def time_ge(lhs, rhs): - """ - Compare time. - - Helper function to compare stamps i.e. - self.topic_vs_message_tracking_tags[*].keys(). - - As stamps are string, it is not appropriate to compare as string. - Builtin_msg.msg.Time does not implement `<=>`, we use rclpy.Time, - although clock_type has no meaning. - - """ - lhs_sec, lhs_nano_sec = (int(x) for x in lhs.split('.')) - rhs_sec, rhs_nano_sec = (int(x) for x in rhs.split('.')) - lhs_time = Time(seconds=lhs_sec, nanoseconds=lhs_nano_sec) - rhs_time = Time(seconds=rhs_sec, nanoseconds=rhs_nano_sec) - return lhs_time <= rhs_time - - thres_stamp = time2str(stamp) - erases = {} - for (topic, infos) in self.topic_vs_message_tracking_tags.items(): - for stamp in infos.keys(): - if time_ge(thres_stamp, stamp): - continue - erases.setdefault(topic, []).append(stamp) - - for (topic, stamps) in erases.items(): - for stamp in stamps: - del self.topic_vs_message_tracking_tags[topic][stamp] - - def topics(self): - """Get topic names which has registered MessageTrackingTag.""" - return list(self.topic_vs_message_tracking_tags.keys()) - - def stamps(self, topic): - """Get List[stamps].""" - if topic not in self.topic_vs_message_tracking_tags.keys(): - return [] - - return list(self.topic_vs_message_tracking_tags[topic].keys()) - - def get(self, topic, stamp=None, idx=None): - """ - Get a MessageTrackingTag. - - :param topic: str - :param stamp: str such as 1618559286.884563157 - :return MessageTrackingTag or None - - """ - ret = None - if topic not in self.topic_vs_message_tracking_tags.keys(): - return None - - if stamp is None and idx is None: - return list(self.topic_vs_message_tracking_tags[topic].values())[0] - - infos = self.topic_vs_message_tracking_tags[topic] - - if stamp in infos.keys(): - ret = infos[stamp] - - if idx is not None: - if len(infos.keys()) <= idx: - print('args.idx too large, should be < {len(info.kens())}') - else: - key = sorted(infos.keys())[idx] - ret = infos[key] - - return ret - - def in_topics(self, topic): - """ - Gather input topics by ignoring stamps. - - :param topic: topic name - :return set of topic name - - """ - ret = set() - - if topic not in self.topic_vs_message_tracking_tags.keys(): - return ret - - for message_tracking_tag in self.topic_vs_message_tracking_tags[topic].values(): - for t in message_tracking_tag.in_infos.keys(): - ret.add(t) - return ret - - def all_topics(self): - """Get all topics which are used as input or output.""" - out = set() - - lhs = self.topics() - for t in lhs: - out.add(t) - - rhs = self.in_topics(t) - for t in rhs: - out.add(t) - return out diff --git a/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py b/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py deleted file mode 100755 index 68da39bc..00000000 --- a/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py +++ /dev/null @@ -1,462 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -"""MessageTrackingTag Traverser.""" - -import argparse -from collections import defaultdict, deque -import json -import pickle -import time - -from rclpy.time import Time - -from tilde_vis.data_as_tree import TreeNode -from tilde_vis.message_tracking_tag import time2str - - -def str_stamp2time(str_stamp): - """Convert string time to Time.""" - sec, nanosec = str_stamp.split('.') - return Time(seconds=int(sec), nanoseconds=int(nanosec)) - - -class SolverResult(object): - """SolverResult.""" - - def __init__(self, topic, stamp, dur_ms, - dur_pub_ms, dur_pub_ms_steady, - is_leaf, parent): - """ - Initialize data. - - :param topic: topic [string] - :param stamp: header.stamp [rclpy.Time] - :param dur_ms: duration of header.stamp in ms [double] - :param dur_pub_ms: duration of output.pub_time in ms [double] - :param dur_pub_ms_steady: steady clock ms [double] - :param is_leaf: bool - :param parent: parent topic [string] - - """ - self.topic = topic - self.stamp = stamp - self.dur_ms = dur_ms - self.dur_pub_ms = dur_pub_ms - self.dur_pub_ms_steady = dur_pub_ms_steady - self.is_leaf = is_leaf - self.parent = parent - - -class SolverResultsPrinter(object): - """SolverResultsPrinter.""" - - @classmethod - def as_tree(cls, results): - """ - Construct array of string to print. - - This method returns tree command like expression - from output topic to source topics. - Multiple input topics are shown by indented listing. - - Example: - ------- - topic dur e2e - out_topic: - in_topic1 0.1 0.1 - in_topic1_1 0.2 0.2 - in_topic2 0.3 0.3 - - :param results: SolverResults - :return array of string - - """ - pass - - -class SolverResults(object): - """SolverResults.""" - - def __init__(self): - """Initialize data.""" - self.data = [] # list of Result - - def add(self, *args): - """ - Register Result. - - Parameters - ---------- - args: completely forward. See Result.__init__. - - """ - self.data.append(SolverResult(*args)) - - -class InputSensorStampSolver(object): - """InputSensorStampSolver.""" - - def __init__(self, graph): - """Initialize solver.""" - # {topic: {stamp: {sensor_topic: [stamps]}}} - self.topic_stamp_to_sensor_stamp = {} - self.graph = graph - self.skips = graph.skips - self.empty_results = {} - - def solve(self, message_tracking_tags, tgt_topic, tgt_stamp, - stops=[]): - """ - Traverse from (tgt_topic, tgt_stamp)-message. - - :param message_tracking_tags: MessageTrackingTags - :param tgt_topic: target topic - :param tgt_stamp: target stamp(str) - :param stops: list of stop topics to prevent loop - :return SolverResults - - Calculate topic_stamp_to_sensor_stamp internally. - - """ - graph = self.graph - path_bfs = graph.bfs_rev(tgt_topic) - is_leaf = {t: b for (t, b) in path_bfs} - skips = self.skips - - stamp = tgt_stamp - - # dists[topic][stamp] - dists = defaultdict(lambda: defaultdict(lambda: -1)) - queue = deque() - parentQ = deque() - - start = str_stamp2time(tgt_stamp) - start_message_tracking_tag = message_tracking_tags.get(tgt_topic, tgt_stamp) - start_pub_time = start_message_tracking_tag.out_info.pubsub_stamp - start_pub_time_steady = start_message_tracking_tag.out_info.pubsub_stamp_steady - - dists[tgt_topic][tgt_stamp] = 1 - queue.append((tgt_topic, tgt_stamp, - start_pub_time, start_pub_time_steady)) - parentQ.append('') - - ret = SolverResults() - while len(queue) != 0: - topic, stamp, sub_time, sub_time_steady = queue.popleft() - parent = parentQ.popleft() - - dur = start - str_stamp2time(stamp) - dur_ms = dur.nanoseconds // 10**6 - - dur_pub = Time.from_msg(start_pub_time) - Time.from_msg(sub_time) - dur_pub_ms = dur_pub.nanoseconds // 10**6 - - dur_pub_steady = \ - Time.from_msg(start_pub_time_steady) - \ - Time.from_msg(sub_time_steady) - dur_pub_ms_steady = dur_pub_steady.nanoseconds // 10**6 - - ret.add(topic, stamp, dur_ms, - dur_pub_ms, dur_pub_ms_steady, - is_leaf[topic], parent) - - # NDT-EKF has loop, so stop - if topic in stops: - continue - - # get next edges - next_message_tracking_tag = message_tracking_tags.get(topic, stamp) - if not next_message_tracking_tag: - continue - - for in_infos in next_message_tracking_tag.in_infos.values(): - for in_info in in_infos: - nx_topic = in_info.topic - if nx_topic in skips: - nx_topic = skips[nx_topic] - nx_stamp = time2str(in_info.stamp) - if dists[nx_topic][nx_stamp] > 0: - continue - dists[nx_topic][nx_stamp] = dists[topic][stamp] + 1 - queue.append((nx_topic, nx_stamp, - in_info.pubsub_stamp, - in_info.pubsub_stamp_steady)) - parentQ.append(topic) - - return ret - - def solve2(self, message_tracking_tags, tgt_topic, tgt_stamp, - stops=[]): - """ - Traverse DAG from output to input. - - :param message_tracking_tags: message_tracking_tags - :param tgt_topic: output topic [string] - :param tgt_stamp: output header stamp [string] - :param stops: stop list - :return TreeNode - - Tree structure represents TopicGraph. - This means that even if some MessageTrackingTags loss - in some timing, - returned TreeNode preserve entire graph. - - .name means topic - - .data is MessageTrackingTag of the topic. - [] whn MessageTrackingTag loss - - """ - skips = self.skips - stamp = tgt_stamp - key = tgt_topic + '.'.join(stops) - if key not in self.empty_results: - self.empty_results[key] = self._solve_empty(tgt_topic, stops=stops) - empty_results = self.empty_results[key] - - queue = deque() - - root_results = TreeNode(tgt_topic) - root_results.merge(empty_results) # setup entire graph - - queue.append((tgt_topic, tgt_stamp, root_results)) - while len(queue) != 0: - topic, stamp, current_result = queue.popleft() - - next_message_tracking_tag = message_tracking_tags.get(topic, stamp) - if next_message_tracking_tag is not None: - current_result.add_data(next_message_tracking_tag) - else: - # print(f"message_tracking_tag of {topic} {stamp} not found") - continue - - # NDT-EKF has loop, so stop - if topic in stops: - continue - - for in_infos in next_message_tracking_tag.in_infos.values(): - for in_info in in_infos: - nx_topic = in_info.topic - if nx_topic in skips: - nx_topic = skips[nx_topic] - nx_stamp = time2str(in_info.stamp) - next_result = current_result.get_child(nx_topic) - - queue.append((nx_topic, nx_stamp, - next_result)) - - return root_results - - def append(self, topic, stamp, sensor_topic, sensor_stamp): - """Append results to topic_stamp_to_sensor_stamp.""" - dic = self.topic_stamp_to_sensor_stamp - if topic not in dic.keys(): - dic[topic] = {} - if stamp not in dic[topic].keys(): - dic[topic][stamp] = {} - if sensor_topic not in dic[topic][stamp].keys(): - dic[topic][stamp][sensor_topic] = [] - - dic[topic][stamp][sensor_topic].append(sensor_stamp) - - def _solve_empty(self, tgt_topic, - stops=[]): - """ - Get empty results to know graph. - - :param tgt_topic: output topic [string] - :param stops: stop list - :return TreeNode - - """ - skips = self.skips - graph = self.graph - - queue = deque() - root_results = TreeNode(tgt_topic) - - queue.append((tgt_topic, root_results)) - while len(queue) != 0: - topic, current_result = queue.popleft() - - if topic in stops: - continue - - rev_topics = graph.rev_topics(topic) - for nx_topic in rev_topics: - if nx_topic in skips: - nx_topic = skips[nx_topic] - - next_result = current_result.get_child(nx_topic) - queue.append((nx_topic, next_result)) - - return root_results - - -class TopicGraph(object): - """Construct topic graph by ignoring stamps.""" - - def __init__(self, message_tracking_tags, skips={}): - """ - Initialize graph. - - :param message_tracking_tags: MessageTrackingTags - :param skips: skip topics. {downstream: upstream} - by input-to-output order - ex) {"/sensing/lidar/top/rectified/pointcloud_ex": - "/sensing/lidar/top/mirror_cropped/pointcloud_ex"} - - """ - self.topics = sorted(message_tracking_tags.all_topics()) - self.t2i = {t: i for i, t in enumerate(self.topics)} - self.skips = skips - n = len(self.topics) - - # edges from publisher to subscription - self.topic_edges = [set() for _ in range(n)] - # edges from subscription to publisher - self.rev_edges = [set() for _ in range(n)] - for out_topic in self.topics: - in_topics = message_tracking_tags.in_topics(out_topic) - - out_id = self.t2i[out_topic] - for in_topic in in_topics: - if in_topic in skips.keys(): - in_topic = skips[in_topic] - in_id = self.t2i[in_topic] - self.topic_edges[in_id].add(out_id) - self.rev_edges[out_id].add(in_id) - - def dump(self, fname): - """Dump.""" - out = {} - out['topics'] = self.topics - out['topic2id'] = self.t2i - out['topic_edges'] = [list(edge) for edge in self.topic_edges] - out['rev_edges'] = [list(edge) for edge in self.rev_edges] - - json.dump(out, open(fname, 'wt')) - - def rev_topics(self, topic): - """ - Get input topics of the target topic. - - :param topic: topic name - :return List[Topic] - - """ - out_topic_idx = self.t2i[topic] - return [self.topics[i] for i in self.rev_edges[out_topic_idx]] - - def dfs_rev(self, start_topic): - """ - Do depth first search from the topic. - - :param start_topic: topic name - :return topic names in appearance order - - """ - edges = self.rev_edges - n = len(edges) - seen = [False for _ in range(n)] - sid = self.t2i[start_topic] - - ret = [] - - def dfs(v): - ret.append(self.topics[v]) - seen[v] = True - for nxt in edges[v]: - if seen[nxt]: - continue - dfs(nxt) - dfs(sid) - - return ret - - def bfs_rev(self, start_topic): - """ - Do breadth first search from the topic. - - :param start_topic: topic name - :return topic names in appearance order - - """ - edges = self.rev_edges - n = len(edges) - dist = [-1 for _ in range(n)] - queue = deque() - - sid = self.t2i[start_topic] - dist[sid] = 0 - queue.append(sid) - paths = [] - - while len(queue) != 0: - v = queue.popleft() - paths.append((self.topics[v], len(edges[v]) == 0)) - for nv in edges[v]: - if dist[nv] >= 0: - continue - dist[nv] = dist[v] + 1 - queue.append(nv) - - return paths - - -def run(args): - """Run.""" - pickle_file = args.pickle_file - message_tracking_tags = pickle.load(open(pickle_file, 'rb')) - - tgt_topic = args.topic - tgt_stamp = sorted(message_tracking_tags.stamps(tgt_topic))[args.stamp_index] - - graph = TopicGraph(message_tracking_tags) - - print('dump') - graph.dump('graph.json') - pickle.dump(graph, open('graph.pkl', 'wb'), - protocol=pickle.HIGHEST_PROTOCOL) - - # bfs_path = graph.bfs_rev(tgt_topic) - # print(f"BFS from {tgt_topic}") - # for p, is_leaf in bfs_path: - # print(f" {p} {is_leaf}") - # print("") - - st = time.time() - solver = InputSensorStampSolver(graph) - solver.solve(message_tracking_tags, tgt_topic, tgt_stamp) - et = time.time() - - print(f'solve {(et-st) * 1000} [ms]') - - -def main(): - """Run main.""" - parser = argparse.ArgumentParser() - parser.add_argument('pickle_file') - parser.add_argument('stamp_index', type=int, default=0, - help='header stamp index') - parser.add_argument('topic', - default='/sensing/lidar/no_ground/pointcloud', - nargs='?') - - args = parser.parse_args() - - run(args) - - -if __name__ == '__main__': - main() diff --git a/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py b/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py deleted file mode 100755 index 0addd0b8..00000000 --- a/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py +++ /dev/null @@ -1,99 +0,0 @@ -#!/usr/bin/python3 -# Copyright 2021 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. - -"""Convert rosbag of MessageTrackingTag to pkl file.""" - -import argparse -import pickle - -from rclpy.serialization import deserialize_message -import rosbag2_py -from rosidl_runtime_py.utilities import get_message - -from tilde_vis.message_tracking_tag import MessageTrackingTag, MessageTrackingTags - - -def get_rosbag_options(path, serialization_format='cdr'): - """Get rosbag options.""" - storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') - - converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, - output_serialization_format=serialization_format) - - return storage_options, converter_options - - -def run(args): - """Do main loop.""" - bag_path = args.bag_path - - storage_options, converter_options = get_rosbag_options(bag_path) - - reader = rosbag2_py.SequentialReader() - reader.open(storage_options, converter_options) - - topic_types = reader.get_all_topics_and_types() - # Create a map for quicker lookup - type_map = {topic_types[i].name: topic_types[i].type - for i in range(len(topic_types))} - - # topic => list of record - out_per_topic = MessageTrackingTags() - - skip_topic_vs_count = {} - - cnt = 0 - while reader.has_next(): - if 0 <= args.cnt and args.cnt < cnt: - break - - (topic, data, t) = reader.read_next() - # TODO: need more accurate check - if '/message_tracking_tag' not in topic: - continue - - msg_type = get_message(type_map[topic]) - msg = deserialize_message(data, msg_type) - - message_tracking_tag = MessageTrackingTag.fromMsg(msg) - out_per_topic.add(message_tracking_tag) - - if 0 < cnt and cnt % 1000 == 0: - print(cnt) - cnt += 1 - - pickle.dump(out_per_topic, open('topic_infos.pkl', 'wb'), - protocol=pickle.HIGHEST_PROTOCOL) - - for topic, count in skip_topic_vs_count.items(): - print(f'skipped {topic} {count} times') - - -def main(): - """Run main.""" - parser = argparse.ArgumentParser() - parser.add_argument('bag_path') - parser.add_argument( - '--cnt', type=int, default=-1, - help='number of messages to dump (whole */message_tracking_tag, not per topic)') - args = parser.parse_args() - - run(args) - - -if __name__ == '__main__': - """Main.""" - main() diff --git a/src/tilde_vis/tilde_vis/pathnode_vis.py b/src/tilde_vis/tilde_vis/pathnode_vis.py deleted file mode 100644 index 4bb4fa6f..00000000 --- a/src/tilde_vis/tilde_vis/pathnode_vis.py +++ /dev/null @@ -1,202 +0,0 @@ -#!env python3 -# Copyright 2021 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. - -"""LatencyViewer PoC version. Deprecated.""" - -from collections import defaultdict -from logging import basicConfig, getLogger -import os - -import numpy as np - -import rclpy -from rclpy.node import Node -from rclpy.time import Time - -from tilde_msg.msg import TopicInfo - -basicConfig(level=os.getenv('LOG_LEVEL', 'INFO')) -logger = getLogger(__name__) - -# for subscriber demo -LIDAR_PREPROCESS = [ - '/sensing/lidar/top/self_cropped/pointcloud_ex', - # subscriber is /sensing/lidar/top/velodyne_interpolate_node - # which is not autoware node - # '/sensing/lidar/top/mirror_cropped/pointcloud_ex', - '/sensing/lidar/top/rectified/pointcloud_ex', - '/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/concatenated/pointcloud', - '/sensing/lidar/measurement_range_cropped/pointcloud', -] - -# for publisher demo -LIDAR_PREPROCESS_PUB = [ - '/sensing/lidar/top/self_cropped/pointcloud_ex', - '/sensing/lidar/top/mirror_cropped/pointcloud_ex', - # publisher is /sensing/lidar/top/velodyne_interpolate_node - # which is not autoware node - # '/sensing/lidar/top/rectified/pointcloud_ex', - '/sensing/lidar/top/outlier_filtered/pointcloud', - '/sensing/lidar/concatenated/pointcloud', - '/sensing/lidar/measurement_range_cropped/pointcloud', -] - - -class TopicInfoStatistics(object): - """TopicInfo statistics.""" - - def __init__(self, topics, max_rows=10): - """Initialize statistics data.""" - self.topics = topics - self.t2i = {topic: i for i, topic in enumerate(topics)} - self.seq2time = defaultdict( - lambda: - np.ones(len(self.t2i), dtype=np.float64)) - # (n_messages, n_sub_callbacks), nanoseconds - self.data = - np.ones((max_rows, len(topics)), dtype=np.float) - self.data_idx = 0 - self.max_rows = max_rows - self.num_dump = -1 - - s = '' - for t in topics: - s += f'{t} ' - s = s.rstrip() - s = s.replace(' ', ' -> ') - print(s) - - def register(self, seq, topic, callback_start_time): - """Register result.""" - vec = self.seq2time[seq] - i = self.t2i[topic] - vec[i] = callback_start_time - logger.debug('seq {}: i: {} non zero: {}'.format( - seq, i, np.count_nonzero(vec > 0))) - if (np.count_nonzero(vec > 0) == len(self.t2i) and - self.data_idx < self.max_rows): - self.data[self.data_idx, ...] = vec[...] - del self.seq2time[seq] - self.data_idx += 1 - - # TODO: delete too old seq key (now leaks) - - def is_filled(self): - """Check data.""" - return self.data_idx == self.max_rows - - def dump_and_clear(self, dumps=False): - """Dump and clear results.""" - if dumps: - self.num_dump += 1 - fname = '{}.npy'.format(self.num_dump) - np.save(fname, self.data) - - # TODO: ignore nan(-1) field - data = self.data[self.data.min(axis=1) > 0] - - nano2msec = 1000 * 1000 - data /= nano2msec - - diff = data[:, 1:] - data[:, :-1] - e2e = data[:, -1] - data[:, 0] - - max_time = diff.max(axis=0) - avg_time = diff.mean(axis=0) - min_time = diff.min(axis=0) - max_e2e = e2e.max() - avg_e2e = e2e.mean() - min_e2e = e2e.min() - - def fmt(vec): - s = '' - for v in vec: - s += f'{v:5.1f} ' - return s.rstrip() - - print('max: ' + fmt(max_time)) - print('avg: ' + fmt(avg_time)) - print('min: ' + fmt(min_time)) - print('e2e: ' + fmt([max_e2e, avg_e2e, min_e2e])) - print('') - - self.data[...] = -1.0 - self.data_idx = 0 - - -class PathVisNode(Node): - """Path visualization node.""" - - def __init__(self): - """Initialize.""" - super().__init__('path_vis_node') - self.declare_parameter('topics', LIDAR_PREPROCESS) - self.declare_parameter('window', 10) - self.declare_parameter('dump', False) - self.declare_parameter('watches_pub', False) - - topics = self.get_parameter('topics') \ - .get_parameter_value() \ - .string_array_value - window = self.get_parameter('window') \ - .get_parameter_value() \ - .integer_value - watches_pub = self.get_parameter('watches_pub') \ - .get_parameter_value() \ - .bool_value - info_name = '/info/sub' - if watches_pub: - info_name = '/message_tracking_tag' - topics = LIDAR_PREPROCESS_PUB - - self.statistics = TopicInfoStatistics(topics, window) - self.subs = [] - - logger.debug('info_name: {}'.format(info_name)) - - for topic in topics: - sub = self.create_subscription( - TopicInfo, - topic + info_name, - self.listener_callback, - 1) - - self.subs.append(sub) - - def listener_callback(self, topic_info): - """Store data and print results.""" - seq = topic_info.seq - topic = topic_info.topic_name - start_time = Time.from_msg(topic_info.callback_start).nanoseconds - dump = self.get_parameter('dump').get_parameter_value().bool_value - - logger.debug('{} {}'.format(seq, topic)) - self.statistics.register(seq, topic, start_time) - if self.statistics.is_filled(): - self.statistics.dump_and_clear(dump) - - -def main(args=None): - """Run PathVisNode.""" - rclpy.init(args=args) - node = PathVisNode() - - rclpy.spin(node) - - rclpy.shutdown() - - -if __name__ == '__main__': - """Main.""" - main() diff --git a/src/tilde_vis/tilde_vis/printer.py b/src/tilde_vis/tilde_vis/printer.py deleted file mode 100644 index ff0c5519..00000000 --- a/src/tilde_vis/tilde_vis/printer.py +++ /dev/null @@ -1,215 +0,0 @@ -# Copyright 2021 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. - -"""Printer for stdout w/wo interactions.""" - -import curses -import datetime -import re - - -class Printer(object): - """Print messages to stdout.""" - - def print_(self, lines): - """Print logs.""" - for s in lines: - print(s) - - -class NcursesPrinter(object): - """Print messages with interaction.""" - - MODE_STOP = 0 - MODE_RUN = 1 - MODE_REGEXP = 2 - - UP = curses.KEY_UP - DOWN = curses.KEY_DOWN - PPAGE = curses.KEY_PPAGE - NPAGE = curses.KEY_NPAGE - - def __init__(self, stdscr): - """Initialize.""" - stdscr.scrollok(True) - stdscr.timeout(0) # set non-blocking - self.stdscr = stdscr - - size = stdscr.getmaxyx() - self.y_max = size[0] - self.x_max = size[1] - - # mode line colors - curses.init_pair(1, curses.COLOR_BLACK, curses.COLOR_CYAN) - curses.init_pair(2, curses.COLOR_BLACK, curses.COLOR_WHITE) - self.CYAN = curses.color_pair(1) - self.WHITE = curses.color_pair(2) - - self.mode = self.MODE_RUN - - # showing lines - self.lines = [] - self.start_line = 0 - - # regexp mode - self.regexp_ptn = '' - self.search = None - - self.adjust_keys = ( - self.DOWN, - ord('j'), - self.NPAGE, - ord('d'), - - self.UP, - ord('k'), - self.PPAGE, - ord('u'), - - ord('g'), - ord('G'), - ) - - self.run_keys = ( - ord('q'), - ord('F') - ) - - self.regex_keys = ( - ord('r'), - ord('/') - ) - - def adjust_lines(self, k, lines): - """Move lines by interaction.""" - step = 0 - if k in (self.DOWN, ord('j')): - step = 1 - elif k in (self.NPAGE, ord('d')): - step = 10 - elif k in (self.UP, ord('k')): - step = -1 - elif k in (self.PPAGE, ord('u')): - step = -10 - - self.start_line += step - - if k == ord('g'): - self.start_line = 0 - elif k == ord('G'): - self.start_line = len(lines) - self.y_max + 2 - - if self.start_line < 0: - self.start_line = 0 - elif len(lines) <= self.start_line: - self.start_line = len(lines) - 1 - - def enter_regex_mode(self): - """Change mode.""" - self.mode = self.MODE_REGEXP - - def print_(self, input_lines): - """Print logs.""" - stdscr = self.stdscr - lines = self.lines - - keys = [] - k = stdscr.getch() - while k >= 0: - keys.append(k) - k = stdscr.getch() - - if len(keys) == 0 and self.mode == self.MODE_RUN: - self.lines = input_lines - lines = self.lines - - if len(self.regexp_ptn) > 0: - lines = list(filter( - lambda x: self.search.search(x), - lines)) - - for k in keys: - if self.mode == self.MODE_STOP: - if k in self.run_keys: - self.mode = self.MODE_RUN - self.lines = input_lines - elif k in self.adjust_keys: - self.adjust_lines(k, lines) - elif k in self.regex_keys: - self.enter_regex_mode() - else: - return - elif self.mode == self.MODE_RUN: - if k in self.adjust_keys: - self.adjust_lines(k, lines) - self.mode = self.MODE_STOP - elif k in self.regex_keys: - self.enter_regex_mode() - else: - # update displayed lines - self.lines = input_lines - lines = self.lines - elif self.mode == self.MODE_REGEXP: - if ord(' ') <= k <= ord('~'): - self.regexp_ptn += chr(k) - self.search = re.compile(self.regexp_ptn) - self.start_line = 0 - elif k == curses.KEY_BACKSPACE: - self.regexp_ptn = self.regexp_ptn[:-1] - self.search = re.compile(self.regexp_ptn) - self.start_line = 0 - elif k in (curses.KEY_ENTER, ord('\n')): - self.mode = self.MODE_RUN - else: - raise Exception('unknown mode') - - stdscr.clear() - - self.print_mode() - for i, s in enumerate(lines[self.start_line:]): - if i + 1 == self.y_max - 1: - break - - if s[-1] != '\n': - s += '\n' - lineno = self.start_line + i - stdscr.addstr(i+1, 0, f'{lineno:<3}| ') - stdscr.addstr(i+1, 5, s) - stdscr.refresh() - - def print_mode(self): - """Print mode line.""" - stdscr = self.stdscr - color = self.CYAN - - t = datetime.datetime.now().strftime('%Y/%m/%d %H:%M:%S') - - mode_str = '' - if self.mode == self.MODE_RUN: - mode_str = \ - 'press UP/DOWN/PgUp/PgDown/j/k/d/u/g/G to scroll,' + \ - 'r or "/" to filter' - color = self.WHITE - elif self.mode == self.MODE_STOP: - mode_str = 'scrolling... Press q/F to periodically update' - color = self.CYAN - elif self.mode == self.MODE_REGEXP: - mode_str = 'Regexp: ' + self.regexp_ptn + '_' - color = self.CYAN - - s = mode_str - s += ' ' * (self.x_max - len(mode_str) - len(t)) - s += t - - stdscr.addstr(0, 0, s, color) diff --git a/src/tilde_vis_test/CMakeLists.txt b/src/tilde_vis_test/CMakeLists.txt deleted file mode 100644 index 53329ab0..00000000 --- a/src/tilde_vis_test/CMakeLists.txt +++ /dev/null @@ -1,61 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(tilde_vis_test) - -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -# find dependencies -find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -find_package(tilde REQUIRED) - -add_library(tilde_vis_test SHARED - src/tilde_vis_test.cpp) -ament_target_dependencies(tilde_vis_test - "rclcpp" - "rclcpp_components" - "std_msgs" - "tilde" - "sensor_msgs") - -rclcpp_components_register_node(tilde_vis_test - PLUGIN "tilde_vis_test::Sensor" - EXECUTABLE sensor) -rclcpp_components_register_node(tilde_vis_test - PLUGIN "tilde_vis_test::Relay" - EXECUTABLE relay) -rclcpp_components_register_node(tilde_vis_test - PLUGIN "tilde_vis_test::Filter" - EXECUTABLE filter) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # uncomment the line when a copyright and license is not present in all source files - #set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # uncomment the line when this package is not in a git repo - #set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -install( - TARGETS tilde_vis_test EXPORT tilde_vis_test - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) -install(DIRECTORY - launch - DESTINATION share/${PROJECT_NAME} -) - -ament_package() diff --git a/src/tilde_vis_test/README.md b/src/tilde_vis_test/README.md deleted file mode 100644 index 6c64356e..00000000 --- a/src/tilde_vis_test/README.md +++ /dev/null @@ -1,31 +0,0 @@ -# tilde_vis_test - -## Description - -Supplementary files to test `tilde_vis` package. - -## issues_23_1.launch.py - -See #23. It's OK if the program runs with no error. - -```bash -$ ros2 launch tilde_vis_test issues_23_1.launch.py -[filter-1] [INFO] [1654561227.800326321] [filter]: Filter get message cnt_ = 0 -[filter-1] [INFO] [1654561228.300162503] [filter]: Filter get message cnt_ = 1 -[filter-1] [INFO] [1654561228.800174003] [filter]: Filter get message cnt_ = 2 -[filter-1] [INFO] [1654561229.300167408] [filter]: Filter get message cnt_ = 3 -[filter-1] [INFO] [1654561229.800184144] [filter]: Filter get message cnt_ = 4 -``` - -## issues_23_2.launch.py - -See #23. It's OK if the program runs with no error. -Be aware that FPS is very slow to reproduce bug. - -```bash -$ ros2 launch tilde_vis_test issues_23_2.launch.py - -[relay-1] [INFO] [1654561285.390170828] [relay]: Relay get message cnt_ = 0 -[relay-1] [INFO] [1654561290.390119277] [relay]: Relay get message cnt_ = 1 -[relay-1] [INFO] [1654561295.390336942] [relay]: Relay get message cnt_ = 2 -``` diff --git a/src/tilde_vis_test/launch/issues_23_1.launch.py b/src/tilde_vis_test/launch/issues_23_1.launch.py deleted file mode 100644 index 84918018..00000000 --- a/src/tilde_vis_test/launch/issues_23_1.launch.py +++ /dev/null @@ -1,36 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription - -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - name='filter', - package='tilde_vis_test', - executable='filter', - output='screen'), - Node( - package='tilde_vis_test', - executable='sensor', - output='screen', - parameters=[{ - 'timer_us': 500 * 1000, - }]), - ]) diff --git a/src/tilde_vis_test/launch/issues_23_2.launch.py b/src/tilde_vis_test/launch/issues_23_2.launch.py deleted file mode 100644 index e78adbda..00000000 --- a/src/tilde_vis_test/launch/issues_23_2.launch.py +++ /dev/null @@ -1,35 +0,0 @@ -# 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. - -"""Launch a publisher and a relay.""" - -from launch import LaunchDescription - -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='tilde_vis_test', - executable='relay', - output='screen'), - Node( - package='tilde_vis_test', - executable='sensor', - output='screen', - parameters=[{ - 'timer_us': 5 * 1000 * 1000, - }]), - ]) diff --git a/src/tilde_vis_test/package.xml b/src/tilde_vis_test/package.xml deleted file mode 100644 index 196482c0..00000000 --- a/src/tilde_vis_test/package.xml +++ /dev/null @@ -1,24 +0,0 @@ - - - - tilde_vis_test - 0.0.0 - TODO: Package description - y-okumura - TODO: License declaration - - ament_cmake - - rclcpp - rclcpp_components - sensor_msgs - std_msgs - tilde - - ament_lint_auto - caret_lint_common - - - ament_cmake - - diff --git a/src/tilde_vis_test/src/tilde_vis_test.cpp b/src/tilde_vis_test/src/tilde_vis_test.cpp deleted file mode 100644 index b9e3e0f9..00000000 --- a/src/tilde_vis_test/src/tilde_vis_test.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright 2021 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 "rclcpp/rclcpp.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "tilde/tilde_node.hpp" -#include "tilde/tilde_publisher.hpp" - -#include "sensor_msgs/msg/point_cloud2.hpp" -#include "std_msgs/msg/string.hpp" - -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; // NOLINT - -/** - * Sensor -> TildeNode -> - * PC2 String - */ -namespace tilde_vis_test -{ - -const char in_topic[] = "in"; -const char out_topic[] = "out"; - -class Sensor : public rclcpp::Node -{ -public: - explicit Sensor(const rclcpp::NodeOptions & options) : Node("sensor", options) - { - const std::string TIMER_DUR = "timer_us"; - const int64_t TIMER_DUR_DEFAULT_NS = 100 * 1000; - declare_parameter(TIMER_DUR, TIMER_DUR_DEFAULT_NS); - - auto timer_dur = get_parameter(TIMER_DUR).get_value(); - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - pub_pc_ = this->create_publisher(in_topic, qos); - - auto timer_callback = [this]() -> void { - auto msg = std::make_unique(); - msg->header.stamp = this->now(); - pub_pc_->publish(std::move(msg)); - }; - auto dur = std::chrono::duration(timer_dur); - timer_ = this->create_wall_timer(dur, timer_callback); - } - -private: - rclcpp::Publisher::SharedPtr pub_pc_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -class Relay : public tilde::TildeNode -{ -public: - explicit Relay(const rclcpp::NodeOptions & options) : TildeNode("relay", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - pub_pc_ = this->create_tilde_publisher(out_topic, qos); - - sub_pc_ = this->create_tilde_subscription( - in_topic, qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - (void)msg; - RCLCPP_INFO(this->get_logger(), "Relay get message cnt_ = %d", cnt_); - pub_pc_->publish(std::move(msg)); - cnt_++; - }); - } - -private: - rclcpp::Subscription::SharedPtr sub_pc_; - tilde::TildePublisher::SharedPtr pub_pc_; - int cnt_{0}; -}; - -class Filter : public tilde::TildeNode -{ -public: - explicit Filter(const rclcpp::NodeOptions & options) : TildeNode("filter", options) - { - rclcpp::QoS qos(rclcpp::KeepLast(7)); - - pub_str_ = this->create_tilde_publisher(out_topic, qos); - - sub_pc_ = this->create_tilde_subscription( - in_topic, qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { - (void)msg; - RCLCPP_INFO(this->get_logger(), "Filter get message cnt_ = %d", cnt_); - auto out_msg = std::make_unique(); - out_msg->data = "hello"; - pub_str_->publish(std::move(out_msg)); - cnt_++; - }); - } - -private: - rclcpp::Subscription::SharedPtr sub_pc_; - tilde::TildePublisher::SharedPtr pub_str_; - int cnt_{0}; -}; - -} // namespace tilde_vis_test - -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Sensor) -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Relay) -RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Filter) From a7844b62f8d63ef6141448e68eb9e4bab43cae2d Mon Sep 17 00:00:00 2001 From: hsato-saitama Date: Tue, 28 Mar 2023 15:40:51 +0900 Subject: [PATCH 11/16] README.md Signed-off-by: hsato-saitama --- CPPLINT.cfg | 14 + README.md | 20 + build_depends.repos | 9 + codecov.yaml | 22 + doc/README.md | 148 ++ doc/build.md | 18 + doc/images/tilde_dag.svg | 761 +++++++++++ doc/images/tilde_deadline.svg | 325 +++++ doc/latency_viewer.md | 50 + doc/mechanism.md | 320 +++++ doc/usecase.md | 117 ++ elisp/README.md | 50 + elisp/tilde-view-mode.el | 79 ++ setup.cfg | 15 + src/analysis/README.md | 103 ++ src/analysis/create_bag_for_concat_filter.py | 96 ++ src/analysis/graph_around.py | 89 ++ src/analysis/graph_common.py | 33 + src/analysis/message_tracking_tag.py | 131 ++ src/analysis/parse_message_tracking_tag.py | 84 ++ src/analysis/pubinfo_traverse.py | 206 +++ src/analysis/rosgraph2_impl.py | 589 ++++++++ src/analysis/topic_times.py | 69 + src/analysis/topic_traversal.py | 100 ++ src/tilde/CMakeLists.txt | 158 +++ src/tilde/README.md | 3 + .../include/tilde/message_conversion.hpp | 106 ++ .../tilde/message_conversion_detail.hpp | 50 + src/tilde/include/tilde/stee_node.hpp | 256 ++++ src/tilde/include/tilde/stee_publisher.hpp | 218 +++ .../include/tilde/stee_sources_table.hpp | 90 ++ src/tilde/include/tilde/stee_subscription.hpp | 100 ++ src/tilde/include/tilde/tilde_node.hpp | 261 ++++ src/tilde/include/tilde/tilde_publisher.hpp | 418 ++++++ src/tilde/include/tilde/tp.h | 70 + src/tilde/package.xml | 26 + src/tilde/src/stee_node.cpp | 54 + src/tilde/src/stee_republisher_node_imu.cpp | 87 ++ src/tilde/src/stee_republisher_node_map.cpp | 86 ++ .../src/stee_republisher_node_pointcloud2.cpp | 88 ++ src/tilde/src/stee_sources_table.cpp | 111 ++ src/tilde/src/tilde_node.cpp | 61 + src/tilde/src/tilde_publisher.cpp | 179 +++ src/tilde/src/tp.c | 19 + src/tilde/test/test_stamp_processor.cpp | 153 +++ src/tilde/test/test_stee_node.cpp | 655 +++++++++ src/tilde/test/test_stee_source_table.cpp | 370 +++++ src/tilde/test/test_tilde_node.cpp | 476 +++++++ src/tilde/test/test_tilde_publisher.cpp | 369 +++++ src/tilde_cmake/CMakeLists.txt | 20 + src/tilde_cmake/README.md | 18 + src/tilde_cmake/cmake/tilde_package.cmake | 25 + src/tilde_cmake/package.xml | 18 + src/tilde_cmake/tilde_cmake-extras.cmake | 15 + src/tilde_deadline_detector/CMakeLists.txt | 73 + src/tilde_deadline_detector/README.md | 59 + .../autoware_sensors.yaml | 50 + .../forward_estimator.hpp | 140 ++ .../tilde_deadline_detector_node.hpp | 93 ++ src/tilde_deadline_detector/package.xml | 24 + .../src/forward_estimator.cpp | 296 ++++ .../src/tilde_deadline_detector_node.cpp | 290 ++++ .../test/test_forward_estimator.cpp | 582 ++++++++ .../test_tilde_deadline_detector_node.cpp | 275 ++++ .../CMakeLists.txt | 45 + src/tilde_early_deadline_detector/README.md | 74 + .../autoware_sensors.yaml | 30 + .../default_path.svg | 1 + .../forward_estimator.hpp | 140 ++ .../tilde_early_deadline_detector_node.hpp | 157 +++ src/tilde_early_deadline_detector/package.xml | 25 + .../src/forward_estimator.cpp | 296 ++++ .../tilde_early_deadline_detector_node.cpp | 521 +++++++ src/tilde_message_filters/CMakeLists.txt | 145 ++ src/tilde_message_filters/README.md | 51 + .../tilde_subscriber.hpp | 290 ++++ .../tilde_synchronizer.hpp | 1191 +++++++++++++++++ src/tilde_message_filters/package.xml | 24 + .../src/sample_sync_publisher.cpp | 150 +++ .../src/sample_tilde_subscriber.cpp | 164 +++ .../src/sample_tilde_synchronizer.cpp | 114 ++ .../test/test_from_original_subscriber.cpp | 307 +++++ .../test/test_from_original_synchronizer.cpp | 517 +++++++ .../test/test_tilde_subscriber.cpp | 178 +++ .../test/test_tilde_synchronizer.cpp | 489 +++++++ src/tilde_msg/CMakeLists.txt | 89 ++ src/tilde_msg/msg/DeadlineNotification.msg | 5 + src/tilde_msg/msg/MessageTrackingTag.msg | 4 + src/tilde_msg/msg/PubTopicTimeInfo.msg | 8 + src/tilde_msg/msg/Source.msg | 4 + .../msg/SteeAckermannControlCommand.msg | 2 + .../msg/SteeAckermannLateralCommand.msg | 2 + src/tilde_msg/msg/SteeDetectedObjects.msg | 2 + src/tilde_msg/msg/SteeImu.msg | 2 + src/tilde_msg/msg/SteeLongitudinalCommand.msg | 2 + src/tilde_msg/msg/SteeOccupancyGrid.msg | 2 + src/tilde_msg/msg/SteeOdometry.msg | 2 + src/tilde_msg/msg/SteePath.msg | 2 + src/tilde_msg/msg/SteePathWithLaneId.msg | 2 + src/tilde_msg/msg/SteePointCloud2.msg | 2 + src/tilde_msg/msg/SteePolygonStamped.msg | 2 + src/tilde_msg/msg/SteePoseStamped.msg | 2 + .../msg/SteePoseWithCovarianceStamped.msg | 2 + src/tilde_msg/msg/SteePredictedObjects.msg | 2 + src/tilde_msg/msg/SteeSource.msg | 3 + src/tilde_msg/msg/SteeTrackedObjects.msg | 2 + .../msg/SteeTrafficLightRoiArray.msg | 2 + src/tilde_msg/msg/SteeTrafficSignalArray.msg | 2 + src/tilde_msg/msg/SteeTrajectory.msg | 2 + src/tilde_msg/msg/SteeTwistStamped.msg | 2 + .../msg/SteeTwistWithCovarianceStamped.msg | 2 + src/tilde_msg/msg/SubTopicTimeInfo.msg | 5 + src/tilde_msg/msg/TestTopLevelStamp.msg | 1 + src/tilde_msg/package.xml | 31 + src/tilde_sample/CMakeLists.txt | 80 ++ src/tilde_sample/README.md | 355 +++++ .../multi_publisher_buffered_relay.launch.py | 35 + .../launch/multi_publisher_relay.launch.py | 35 + .../publisher_relay_with_header.launch.py | 31 + .../publisher_relay_without_header.launch.py | 31 + src/tilde_sample/package.xml | 27 + src/tilde_sample/src/goal.cpp | 55 + src/tilde_sample/src/relay_timer.cpp | 135 ++ .../src/relay_timer_with_buffer.cpp | 142 ++ src/tilde_sample/src/sample_publisher.cpp | 139 ++ .../src/sample_stee_publisher.cpp | 86 ++ src/tilde_vis/README.md | 194 +++ src/tilde_vis/package.xml | 20 + src/tilde_vis/resource/tilde_vis | 0 src/tilde_vis/setup.cfg | 4 + src/tilde_vis/setup.py | 30 + src/tilde_vis/test/test_copyright.py | 23 + src/tilde_vis/test/test_data_as_tree.py | 192 +++ src/tilde_vis/test/test_flake8.py | 25 + src/tilde_vis/test/test_latency_viewer.py | 272 ++++ .../test/test_message_tracking_tag.py | 119 ++ .../test_message_tracking_tag_traverse.py | 558 ++++++++ src/tilde_vis/test/test_ncurse_printer.py | 35 + src/tilde_vis/test/test_pep257.py | 23 + src/tilde_vis/tilde_vis/__init__.py | 15 + src/tilde_vis/tilde_vis/caret_vis_tilde.py | 277 ++++ src/tilde_vis/tilde_vis/data_as_tree.py | 128 ++ src/tilde_vis/tilde_vis/latency_viewer.py | 792 +++++++++++ .../tilde_vis/message_tracking_tag.py | 269 ++++ .../message_tracking_tag_traverse.py | 462 +++++++ .../tilde_vis/parse_message_tracking_tag.py | 99 ++ src/tilde_vis/tilde_vis/pathnode_vis.py | 202 +++ src/tilde_vis/tilde_vis/printer.py | 215 +++ src/tilde_vis_test/CMakeLists.txt | 61 + src/tilde_vis_test/README.md | 31 + .../launch/issues_23_1.launch.py | 36 + .../launch/issues_23_2.launch.py | 35 + src/tilde_vis_test/package.xml | 24 + src/tilde_vis_test/src/tilde_vis_test.cpp | 123 ++ 154 files changed, 20404 insertions(+) create mode 100644 CPPLINT.cfg create mode 100644 README.md create mode 100644 build_depends.repos create mode 100644 codecov.yaml create mode 100644 doc/README.md create mode 100644 doc/build.md create mode 100644 doc/images/tilde_dag.svg create mode 100644 doc/images/tilde_deadline.svg create mode 100644 doc/latency_viewer.md create mode 100644 doc/mechanism.md create mode 100644 doc/usecase.md create mode 100644 elisp/README.md create mode 100644 elisp/tilde-view-mode.el create mode 100644 setup.cfg create mode 100644 src/analysis/README.md create mode 100755 src/analysis/create_bag_for_concat_filter.py create mode 100755 src/analysis/graph_around.py create mode 100644 src/analysis/graph_common.py create mode 100644 src/analysis/message_tracking_tag.py create mode 100755 src/analysis/parse_message_tracking_tag.py create mode 100755 src/analysis/pubinfo_traverse.py create mode 100644 src/analysis/rosgraph2_impl.py create mode 100755 src/analysis/topic_times.py create mode 100755 src/analysis/topic_traversal.py create mode 100644 src/tilde/CMakeLists.txt create mode 100644 src/tilde/README.md create mode 100644 src/tilde/include/tilde/message_conversion.hpp create mode 100644 src/tilde/include/tilde/message_conversion_detail.hpp create mode 100644 src/tilde/include/tilde/stee_node.hpp create mode 100644 src/tilde/include/tilde/stee_publisher.hpp create mode 100644 src/tilde/include/tilde/stee_sources_table.hpp create mode 100644 src/tilde/include/tilde/stee_subscription.hpp create mode 100644 src/tilde/include/tilde/tilde_node.hpp create mode 100644 src/tilde/include/tilde/tilde_publisher.hpp create mode 100644 src/tilde/include/tilde/tp.h create mode 100644 src/tilde/package.xml create mode 100644 src/tilde/src/stee_node.cpp create mode 100644 src/tilde/src/stee_republisher_node_imu.cpp create mode 100644 src/tilde/src/stee_republisher_node_map.cpp create mode 100644 src/tilde/src/stee_republisher_node_pointcloud2.cpp create mode 100644 src/tilde/src/stee_sources_table.cpp create mode 100644 src/tilde/src/tilde_node.cpp create mode 100644 src/tilde/src/tilde_publisher.cpp create mode 100644 src/tilde/src/tp.c create mode 100644 src/tilde/test/test_stamp_processor.cpp create mode 100644 src/tilde/test/test_stee_node.cpp create mode 100644 src/tilde/test/test_stee_source_table.cpp create mode 100644 src/tilde/test/test_tilde_node.cpp create mode 100644 src/tilde/test/test_tilde_publisher.cpp create mode 100644 src/tilde_cmake/CMakeLists.txt create mode 100644 src/tilde_cmake/README.md create mode 100644 src/tilde_cmake/cmake/tilde_package.cmake create mode 100644 src/tilde_cmake/package.xml create mode 100644 src/tilde_cmake/tilde_cmake-extras.cmake create mode 100644 src/tilde_deadline_detector/CMakeLists.txt create mode 100644 src/tilde_deadline_detector/README.md create mode 100644 src/tilde_deadline_detector/autoware_sensors.yaml create mode 100644 src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp create mode 100644 src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp create mode 100644 src/tilde_deadline_detector/package.xml create mode 100644 src/tilde_deadline_detector/src/forward_estimator.cpp create mode 100644 src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp create mode 100644 src/tilde_deadline_detector/test/test_forward_estimator.cpp create mode 100644 src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp create mode 100644 src/tilde_early_deadline_detector/CMakeLists.txt create mode 100644 src/tilde_early_deadline_detector/README.md create mode 100644 src/tilde_early_deadline_detector/autoware_sensors.yaml create mode 100644 src/tilde_early_deadline_detector/default_path.svg create mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/forward_estimator.hpp create mode 100644 src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp create mode 100644 src/tilde_early_deadline_detector/package.xml create mode 100644 src/tilde_early_deadline_detector/src/forward_estimator.cpp create mode 100644 src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp create mode 100644 src/tilde_message_filters/CMakeLists.txt create mode 100644 src/tilde_message_filters/README.md create mode 100644 src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp create mode 100644 src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp create mode 100644 src/tilde_message_filters/package.xml create mode 100644 src/tilde_message_filters/src/sample_sync_publisher.cpp create mode 100644 src/tilde_message_filters/src/sample_tilde_subscriber.cpp create mode 100644 src/tilde_message_filters/src/sample_tilde_synchronizer.cpp create mode 100644 src/tilde_message_filters/test/test_from_original_subscriber.cpp create mode 100644 src/tilde_message_filters/test/test_from_original_synchronizer.cpp create mode 100644 src/tilde_message_filters/test/test_tilde_subscriber.cpp create mode 100644 src/tilde_message_filters/test/test_tilde_synchronizer.cpp create mode 100644 src/tilde_msg/CMakeLists.txt create mode 100644 src/tilde_msg/msg/DeadlineNotification.msg create mode 100644 src/tilde_msg/msg/MessageTrackingTag.msg create mode 100644 src/tilde_msg/msg/PubTopicTimeInfo.msg create mode 100644 src/tilde_msg/msg/Source.msg create mode 100644 src/tilde_msg/msg/SteeAckermannControlCommand.msg create mode 100644 src/tilde_msg/msg/SteeAckermannLateralCommand.msg create mode 100644 src/tilde_msg/msg/SteeDetectedObjects.msg create mode 100644 src/tilde_msg/msg/SteeImu.msg create mode 100644 src/tilde_msg/msg/SteeLongitudinalCommand.msg create mode 100644 src/tilde_msg/msg/SteeOccupancyGrid.msg create mode 100644 src/tilde_msg/msg/SteeOdometry.msg create mode 100644 src/tilde_msg/msg/SteePath.msg create mode 100644 src/tilde_msg/msg/SteePathWithLaneId.msg create mode 100644 src/tilde_msg/msg/SteePointCloud2.msg create mode 100644 src/tilde_msg/msg/SteePolygonStamped.msg create mode 100644 src/tilde_msg/msg/SteePoseStamped.msg create mode 100644 src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg create mode 100644 src/tilde_msg/msg/SteePredictedObjects.msg create mode 100644 src/tilde_msg/msg/SteeSource.msg create mode 100644 src/tilde_msg/msg/SteeTrackedObjects.msg create mode 100644 src/tilde_msg/msg/SteeTrafficLightRoiArray.msg create mode 100644 src/tilde_msg/msg/SteeTrafficSignalArray.msg create mode 100644 src/tilde_msg/msg/SteeTrajectory.msg create mode 100644 src/tilde_msg/msg/SteeTwistStamped.msg create mode 100644 src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg create mode 100644 src/tilde_msg/msg/SubTopicTimeInfo.msg create mode 100644 src/tilde_msg/msg/TestTopLevelStamp.msg create mode 100644 src/tilde_msg/package.xml create mode 100644 src/tilde_sample/CMakeLists.txt create mode 100644 src/tilde_sample/README.md create mode 100644 src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py create mode 100644 src/tilde_sample/launch/multi_publisher_relay.launch.py create mode 100644 src/tilde_sample/launch/publisher_relay_with_header.launch.py create mode 100644 src/tilde_sample/launch/publisher_relay_without_header.launch.py create mode 100644 src/tilde_sample/package.xml create mode 100644 src/tilde_sample/src/goal.cpp create mode 100644 src/tilde_sample/src/relay_timer.cpp create mode 100644 src/tilde_sample/src/relay_timer_with_buffer.cpp create mode 100644 src/tilde_sample/src/sample_publisher.cpp create mode 100644 src/tilde_sample/src/sample_stee_publisher.cpp create mode 100644 src/tilde_vis/README.md create mode 100644 src/tilde_vis/package.xml create mode 100644 src/tilde_vis/resource/tilde_vis create mode 100644 src/tilde_vis/setup.cfg create mode 100644 src/tilde_vis/setup.py create mode 100644 src/tilde_vis/test/test_copyright.py create mode 100644 src/tilde_vis/test/test_data_as_tree.py create mode 100644 src/tilde_vis/test/test_flake8.py create mode 100644 src/tilde_vis/test/test_latency_viewer.py create mode 100644 src/tilde_vis/test/test_message_tracking_tag.py create mode 100644 src/tilde_vis/test/test_message_tracking_tag_traverse.py create mode 100644 src/tilde_vis/test/test_ncurse_printer.py create mode 100644 src/tilde_vis/test/test_pep257.py create mode 100644 src/tilde_vis/tilde_vis/__init__.py create mode 100755 src/tilde_vis/tilde_vis/caret_vis_tilde.py create mode 100644 src/tilde_vis/tilde_vis/data_as_tree.py create mode 100644 src/tilde_vis/tilde_vis/latency_viewer.py create mode 100644 src/tilde_vis/tilde_vis/message_tracking_tag.py create mode 100755 src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py create mode 100755 src/tilde_vis/tilde_vis/parse_message_tracking_tag.py create mode 100644 src/tilde_vis/tilde_vis/pathnode_vis.py create mode 100644 src/tilde_vis/tilde_vis/printer.py create mode 100644 src/tilde_vis_test/CMakeLists.txt create mode 100644 src/tilde_vis_test/README.md create mode 100644 src/tilde_vis_test/launch/issues_23_1.launch.py create mode 100644 src/tilde_vis_test/launch/issues_23_2.launch.py create mode 100644 src/tilde_vis_test/package.xml create mode 100644 src/tilde_vis_test/src/tilde_vis_test.cpp diff --git a/CPPLINT.cfg b/CPPLINT.cfg new file mode 100644 index 00000000..ba6bdf08 --- /dev/null +++ b/CPPLINT.cfg @@ -0,0 +1,14 @@ +# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_cpplint/ament_cpplint/main.py#L64-L120 +set noparent +linelength=100 +includeorder=standardcfirst +filter=-build/c++11 # we do allow C++11 +filter=-build/namespaces_literals # we allow using namespace for literals +filter=-runtime/references # we consider passing non-const references to be ok +filter=-whitespace/braces # we wrap open curly braces for namespaces, classes and functions +filter=-whitespace/indent # we don't indent keywords like public, protected and private with one space +filter=-whitespace/parens # we allow closing parenthesis to be on the next line +filter=-whitespace/semicolon # we allow the developer to decide about whitespace after a semicolon +filter=-build/header_guard # we automatically fix the names of header guards using pre-commit +filter=-build/include_order # we use the custom include order +filter=-build/include_subdir # we allow the style of "foo.hpp" diff --git a/README.md b/README.md new file mode 100644 index 00000000..307ede9d --- /dev/null +++ b/README.md @@ -0,0 +1,20 @@ +# TILDE - Tilde Is Latency Data Embedding + +[README(ja)](./doc/README.md) + +## Contents + +| src/ | about | +| ------------ | ---------------------------------------------------- | +| tilde | TILDE main code | +| tilde_sample | samples including talker, relay, listener and so on. | +| tilde_msg | TILDE internal msg | +| tilde_vis | TILDE results visualization | + +## Build + +```bash +. /path/to/ros2_galactic/install/setup.bash +vcs import src < build_depends.repos +colcon build --symlink-install --cmake-args --cmake-args -DCMAKE_BUILD_TYPE=Release +``` diff --git a/build_depends.repos b/build_depends.repos new file mode 100644 index 00000000..4a3bc882 --- /dev/null +++ b/build_depends.repos @@ -0,0 +1,9 @@ +repositories: + autoware_msg/autoware_auto_msgs: + type: git + url: https://github.com/tier4/autoware_auto_msgs.git + version: tier4/main + autoware_common: + type: git + url: https://github.com/tier4/caret_common.git + version: main diff --git a/codecov.yaml b/codecov.yaml new file mode 100644 index 00000000..8ca21967 --- /dev/null +++ b/codecov.yaml @@ -0,0 +1,22 @@ +coverage: + status: + project: + default: + target: auto + patch: + default: + target: auto + +comment: + show_carryforward_flags: true + +flag_management: + default_rules: + carryforward: true + statuses: + - name_prefix: project- + type: project + target: auto + - name_prefix: patch- + type: patch + target: auto diff --git a/doc/README.md b/doc/README.md new file mode 100644 index 00000000..49ea945d --- /dev/null +++ b/doc/README.md @@ -0,0 +1,148 @@ +# TILDE - Tilde Is Latency Data Embedding + +TILDE は複数のノードにまたがるレイテンシ測定やデッドライン検出の為のフレームワークです。 +TILDE はノードから出力されたトピックを遡り、元となった入力トピック(センサー)を特定することができます。 +入力から出力までを辿れる為、レイテンシ計測とデッドライン検出が可能です。 + + + +## Table of Contents + +- [TILDE - Tilde Is Latency Data Embedding](#tilde---tilde-is-latency-data-embedding) + - [Table of Contents](#table-of-contents) + - [TILDE の目標](#tilde-の目標) + - [想定ユースケース](#想定ユースケース) + - [動作原理](#動作原理) + - [API](#api) + - [tilde::TildeNode](#tildetildenode) + - [インストール, 組み込み](#インストール-組み込み) + - [デッドライン検出](#デッドライン検出) + - [周辺ツール](#周辺ツール) + - [リポジトリ一覧](#リポジトリ一覧) + + + +## TILDE の目標 + +ROS2 のトピック通信は、一般に以下の様な有向グラフ(DAG)を構成します。 + +![tilde_dag](./images/tilde_dag.svg) + +「センサーの情報がアクチュエーターにいつ反映されるか」つまり「あるトピックはどのタイミングのセンサーの情報を参照したものか」は難しい問いです。 + +- DAG は必ずしも一本道では無いため、経路ごとに参照されているセンサーの時刻が異なる可能性があります。 + - 例えば PlanningNode は SensorNodeB からの情報を FusionNode 経由で受信する場合と直接受信する場合があります。 + - また PlanningNode は FeedbackNode を通じて SensorNodeC の情報を参照しています。 +- ループが含まれる場合も考える必要があります。 + - FeedbackNode を通じて PlanningNode は潜在的には過去全ての SensorNodeA, SensorNodeB の情報を参照しているとも言えます。 +- DAG からは分からないこともあります。 + - 例えば、ノードによってはメッセージをバッファリングし選択的に入力データを利用している可能性があります。 + +TILDE ではこの様な複雑な問題にも対応できる様、 DAG を解析し、ランタイム時に各トピックがいつのセンサー情報を元に作成されたかを求め、その情報を元にレイテンシ解析やデッドライン検出をすることを目標としています。 + +## 想定ユースケース + +TILDE では以下の様なユースケースを想定ユースケースしています。 + +- オンラインレイテンシ測定 +- デッドライン検出 + - パスの途中終了 + - 入力情報の「賞味期限」検出 + +詳しくは [usecase](./usecase.md) をご覧下さい。 + +## 動作原理 + +TILDE ではメイントピックの publish 時に MessageTrackingTag というメタ情報を `/message_tracking_tag` に publish します。 +MessageTrackingTag は数百バイト程度のメッセージで、メイントピックを構成する入力トピックの情報が記載されます。 +MessageTrackingTag の詳細やオーバーヘッドについては [mechanism](./mechanism.md) をご覧下さい。 + +## API + +TILDE では ROS2 rclcpp と同じ引数で名前少し異なる API 群を用意しています。 +ご自分のアプリケーションに取り込む際は rclcpp::Node を tilde::TildeNode に置換するなど、機械的な置換で利用可能です。 + +- Node + - [tilde::TildeNode](../src/tilde/include/tilde/tilde_node.hpp). 下記も参照。 +- Publisher + - [tilde::Node::create_tilde_publisher()](../src/tilde/include/tilde/tilde_node.hpp) + - [tilde::TildePublisher](../src/tilde/include/tilde/tilde_publisher.hpp) + - [tilde::TildePublisher::publish()](../src/tilde/include/tilde/tilde_publisher.hpp) +- Subscription + - [tilde::Node::create_tilde_subscription()](../src/tilde/include/tilde/tilde_node.hpp) +- MessageTrackingTag explicit API + - [tilde::TildePublisherBase::set_explicit_input_info()](../src/tilde/include/tilde/tilde_publisher.hpp) + - [tilde::TildePublisherBase::set_max_sub_callback_infos_sec()](../src/tilde/include/tilde/tilde_publisher.hpp) +- Deadline detection + - T.B.D. + +### tilde::TildeNode + +rclcpp::Node の子クラスです。 +以下の Parameter を持ちます。 + +- `enable_tilde` + - boolean. + - false の場合 TILDE の機能がオフになる。つまり Subscription callback でのフックや publish 時の MessageTrackingTag 送信が抑制される。 + - 2022/03/02 現在初期化時のみ指定可能。 + +## インストール, 組み込み + +[build](./build.md) + +既存のアプリケーションに TILDE を組み込む際は以下の流れになります。 +※ TODO: TILDE ライブラリ（\*.so）の取込み手順 + +- packages.xml に以下を追加 + +```xml +tilde_cmake +tilde +``` + +- CMakeLists.txt に以下を追加 + +```cmake +find_package(tilde_cmake REQUIRED) +tilde_package() +find_package(tilde REQUIRED) +``` + +- rclcpp::Node, rclcpp::Node::create_publisher, rclcpp::Node::create_subscription に代わり tilde の各 API の利用 + - rclcpp::Node とコンストラクタを tilde::TildeNode に置換 + - create_subscription() を create_tilde_subscription() に置換 + - create_publisher() を create_tilde_publisher() に置換 + - rclcpp::Publisher を tilde::TildePublisher に置換 +- 対象ファイルに include 文の追加 + - `#include "tilde/tilde_node.hpp"` + - `#include "tilde/tilde_publisher.hpp"` +- package.xml に tilde を追加 + - `tilde` +- CMakeLists.txt に tilde を追加 +- メッセージをバッファしているノードがあれば MessageTrackingTag explicit API の呼び出しを追加 + +サンプルプロジェクトは [tilde_sample](../src/tilde_sample) をご覧下さい。 + +- sample_publisher.cpp + - ros2/demos の talker.cpp 相当のプログラムです + - メイントピックの header.stamp の有無が異なる 2 サンプルがあります。 + - `TildeNode`, `create_tilde_publisher` の例です。 +- relay_timer.cpp + - sample_publisher.cpp のトピックを受信・転送するプログラムです。 + - `create_tilde_subscription` の例です。 +- relay_timer_with_buffer.cpp + - relay_timer.cpp で buffer がある例です。 + - relay_timer.cpp に加え explicit API を用いています。 + +## デッドライン検出 + +> **_NOTE:_** 2022/01/21 現在本機能は未実装 + +## 周辺ツール + +- [latency viewer](./latency_viewer.md): オンラインレイテンシ表示ツール + +## リポジトリ一覧 + +- tilde 本体 +- tilde tools ← latency viewer を含む各種 TILDE ツールを 1 レポジトリにまとめる予定 diff --git a/doc/build.md b/doc/build.md new file mode 100644 index 00000000..caffedd4 --- /dev/null +++ b/doc/build.md @@ -0,0 +1,18 @@ +# Tilde のビルド + +ROS2 galactic 環境が構築済みのこと。 + +```bash +. /path/to/ros2/galactic/setup.bash + +git clone git@github.com:tier4/TILDE.git + +rosdep install \ + --from-paths src --ignore-src \ + --rosdistro galactic -y \ + --skip-keys "console_bridge fastcdr fastrtps rti-connext-dds-5.3.1 urdfdom_headers" + +sudo apt-get install liblttng-ust-dev + +colcon build --symlink-install --cmake-args -DCMAKE_BUILD_TYPE=Release +``` diff --git a/doc/images/tilde_dag.svg b/doc/images/tilde_dag.svg new file mode 100644 index 00000000..538b15c8 --- /dev/null +++ b/doc/images/tilde_dag.svg @@ -0,0 +1,761 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + SensorNode + ObstacelCheker + FusionNode + + SensorNodeB + + /sensor/topic/A + + /sensor/fusion + + /sensor/topic/B + + /sensor/topic/C + + /planning/base + + /planning/fback + + /planning/refine + + ControlNode + + /control/cmd + + + + + + + + + + + + + + + + + + + FeedbackNode + + + + + + + + + SensorNodeC + + + + PlanningNode + + + + + diff --git a/doc/images/tilde_deadline.svg b/doc/images/tilde_deadline.svg new file mode 100644 index 00000000..55d05d68 --- /dev/null +++ b/doc/images/tilde_deadline.svg @@ -0,0 +1,325 @@ + + + + + + + + + + + + + + + + + + + + + + + NodeA + + NodeB + + /topicA + + /topicB + + + + TargetNode + + + 10hz + 10hz + + case2 + + case1 + + <= 100 ms + + diff --git a/doc/latency_viewer.md b/doc/latency_viewer.md new file mode 100644 index 00000000..ac133cc8 --- /dev/null +++ b/doc/latency_viewer.md @@ -0,0 +1,50 @@ +# Latency Viewer + +Latency Viewer はあるセンサー入力からのレンテンシの表示ツールです。 +MessageTrackingTag を利用して DAG を遡ることで、注目しているトピックからセンサーまで全てのパスにおけるレイテンシを確認できます。 +vmstat や top の様に定期的にレイテンシ情報が更新されます。 + +起動方法やオプションについては [ビシュアライズツールの README](../src/tilde_vis/README.md) をご覧下さい。 + +## 出力例 + +trajectory_follower を表示した結果は以下の通りです。 + +トピック・header stamp・レイテンシが 2 種類が表示されます。 + +- トピック名 + - 前のスペースの数でパスが表示されます +- header stamp + - メイントピックの header stamp です +- レイテンシ + - ROS time ベースと steady time の 2 種類が表示されています。 + - レイテンシはある行のトピックが publish されてから `/control/trajectory_follower/lateral/control_cmd` が publish されるまでの経過時間です + +```text +/control/trajectory_follower/lateral/control_cmd 1618559274.549348133 0 0 + /localization/twist 1618559274.544330488 5 3 + /planning/scenario_planning/trajectory 1618559274.479350019 15 12 + (snip) + /sensing/lidar/concatenated/pointcloud 1618559273.951109000 289 288 + /sensing/lidar/left/outlier_filtered/pointcloud 1618559274.168087000 305 302 + /sensing/lidar/left/mirror_cropped/pointcloud_ex 1618559274.168087000 305 304 + /sensing/lidar/left/self_cropped/pointcloud_ex 1618559274.168087000 305 304 + /sensing/lidar/left/pointcloud_raw_ex* NA NA NA + /sensing/lidar/right/outlier_filtered/pointcloud 1618559274.170810000 300 301 + /sensing/lidar/right/mirror_cropped/pointcloud_ex 1618559274.170810000 305 302 + /sensing/lidar/right/self_cropped/pointcloud_ex 1618559274.170810000 305 303 + /sensing/lidar/right/pointcloud_raw_ex* NA NA NA + /sensing/lidar/top/outlier_filtered/pointcloud 1618559273.951109000 334 333 + /sensing/lidar/top/mirror_cropped/pointcloud_ex 1618559273.951109000 410 407 + /sensing/lidar/top/self_cropped/pointcloud_ex 1618559273.951109000 435 433 + /sensing/lidar/top/pointcloud_raw_ex* NA NA NA + /vehicle/status/twist* NA NA NA +``` + +以下のことが読みとれます。 + +- `trajectory_follower` は `/localization/twist` と `/planning/scenario_planning/trajectory` を入力とする + - それぞれのレイテンシは ROS time (今回はシミュレーション時間) で 5 ms, 15 ms、 steady time で 3 ms, 12 ms. +- `/planning/scenario_planning/trajectory` に至るまでに concat filter (`/sensing/lidar/concatenated/pointcloud`) がある + - 最終的に `/sensing/lidar/left/self_cropped/pointcloud_ex` などに辿りつく. + - `/sensing/lidar/left/pointcloud_raw_ex` は終端(センサー)だが、TILDE 非対応の為、レイテンシ等は NA になっている diff --git a/doc/mechanism.md b/doc/mechanism.md new file mode 100644 index 00000000..07928034 --- /dev/null +++ b/doc/mechanism.md @@ -0,0 +1,320 @@ +# TILDE の動作原理 + +TILDE は、ユーザプログラムがメイントピックを publish するのに併せて MessageTrackingTag というトピックを `/message_tracking_tag` に publish します。 +MessageTrackingTag は数百バイト程度のメッセージで、メイントピックを構成する入力トピックの情報が記載されます。 + +ここで **メイントピック** とはアプリケーションが本来やりとりするメッセージです。 +TILDE が MessageTrackingTag という付加的なトピックをやりとりする為、アプリケーション本来のメッセージをメイントピックと呼称しています。 + + + +## Table of Contents + +- [TILDE の動作原理](#tilde-の動作原理) + - [Table of Contents](#table-of-contents) + - [MessageTrackingTag](#messagetrackingtag) + - [例](#例) + - [DAG と動作概要](#dag-と動作概要) + - [stamp](#stamp) + - [NodeC の MessageTrackingTag](#nodec-の-messagetrackingtag) + - [MessageTrackingTag の作成メカニズム](#messagetrackingtag-の作成メカニズム) + - [概要](#概要) + - [class](#class) + - [create_tilde_publisher](#create_tilde_publisher) + - [create_tilde_subscription](#create_tilde_subscription) + - [publish](#publish) + - [Explicit API](#explicit-api) + - [オーバーヘッド](#オーバーヘッド) + + + +まず MessageTrackingTag についてデータ構造と例を記述します。 +次に TILDE が MessageTrackingTag を作る仕組みを記述します。 +最後にオーバーヘッドについて記載します。 + +## MessageTrackingTag + +MessageTrackingTag はメイントピックの publish 時と同時に送信されるメタ情報です。 +`<メイントピック名>/message_tracking_tag` に送信されます。 + +メッセージ定義は以下の通りです。 +※ TODO: ファイル名やデータ構造はリファクタ予定 + +[MessageTrackingTag.msg](../src/tilde_msg/msg/MessageTrackingTag.msg) + +- Header: + - header + - シーケンス番号 + - 送信者情報(node 名や publisher ID) +- `output_info`: 出力トピックに関する情報 + - トピック名: メイントピックのトピック名 + - header stamp: メイントピックの header stamp + - 送信時刻: メイントピックの送信時刻 +- `input_infos`: 入力トピックに関する情報。下記を入力トピック分。 + - トピック名: メイントピックのトピック名 + - header stamp: メイントピックの header stamp + - 受信時刻: メイントピックの受信時刻 + +トピック名やノード名の長さにもよりますがデータサイズは以下の通りです。送信周波数はメイントピックと同じです。 + +- Header + output_info: 約 80 バイト + トピック名やノード名分のバイト数 +- input_infos: (入力トピック数 \* 約 40 バイト) + トピック名のバイト数 + +## 例 + +### DAG と動作概要 + +NodeA, NodeB, NodeC からなる以下の DAG を考えます(丸はノード、四角はトピック)。 + +```mermaid +graph LR + NodeA((NodeA)) --t=2,stamp=1--> topic_a + NodeB((NodeB)) --t=4,stamp=3--> topic_b + topic_a --> NodeC((NodeC)) + topic_b --> NodeC + NodeC --t=6,stamp=5--> topic_c +``` + +- 動作概要 + - NodeA は t=2 に、NodeB は t=4 に publish します。 + - NodeC は t=6 に起床し、これらの情報を元に自身の計算をして topic_c を送信します。 +- 補足 + - 矢印中の stamp は次に示す通り ros2 std_msgs::msg::Header の stamp フィールドです。 + - メッセージ識別に利用します。 + +### stamp + +各 Node では ros2 の sensor_msgs やアプリケーション固有のメッセージ型を送信します。 +例えば `sensor_msgs/msg/PointCloud2` の場合、以下の様なメッセージになります(説明の為フィールドは省略しています)。 + +```mermaid +classDiagram + direction LR + Header <.. PointCloud2 + class PointCloud2 { + +header: Header + +height + +width + +data: uint8[] + } + class Header { + +stamp: Time + } +``` + +ROS2 のセンサーやナーゲーションで用いられるメッセージでは標準的に header フィールドが付与されています。 +header フィールドには stamp フィールドがあり、ユーザ定義のタイムスタンプを記入します。 +TILDE では **header フィールドの stamp を利用** によりメッセージを識別します。 + +### NodeC の MessageTrackingTag + +TILDE はメインメッセージの publish をフックして MessageTrackingTag を送信します。 +NodeC の出力する MessageTrackingTag は以下の様な物になります(フィールドは一部省略しています)。 + +```mermaid +classDiagram + direction LR + OutputInfo <.. MessageTrackingTag_NodeC + InputInfo_TopicA <.. MessageTrackingTag_NodeC + InputInfo_TopicB <.. MessageTrackingTag_NodeC + class MessageTrackingTag_NodeC { + +seq: 0 + +Node: NodeC + +output_info + +input_infos[0]: InputInfo_TopicA + +input_infos[1]: InputInfo_TOpicB + } + class OutputInfo { + +topic: "topic_c" + +send_time: 6 + +stamp: 5 + } + class InputInfo_TopicA { + +topic: "topic_a" + +stamp: 1 + +recv_time: 2 + } + class InputInfo_TopicB { + +topic: "topic_b" + +stamp: 3 + +recv_time: 4 + } +``` + +この様に MessageTrackingTag には **Node 単位で、出力と入力の紐付け情報** が記載されます。 +より具体的には **送信したメインメッセージと、自身が subscription している入力トピックのメッセージを header stamp で紐付け** ます。 +上記の様に、デフォルトではメッセージの紐付けは各入力トピックについて **メインメッセージ送信前に受信した最新のメッセージ** になっています。 explicit API により明示的に紐付け情報を設定することが可能です。 + +## MessageTrackingTag の作成メカニズム + +MessageTrackingTag はメイントピックの publish 時に同時に送信されます。 +また `input_infos` では入力トピックとの紐付け情報が設定されます。 + +これらを行なうためには subscription 時に `input_infos` 用の情報を保持したり、 MessageTrackingTag のメッセージを作成・送信する必要がありますが、TILDE が publish や subscription callback をフックしてこれらの処理を実行する為、アプリケーションでこれらを意識する必要はありません。 + +### 概要 + +以下で UML 風の図を用いて TILDE の動作概要を記述します。先に簡単に言葉でまとめます。 + +- アプリケーションから見た TILDE + - MessageTrackingTag を作成するのに必要な情報の蓄積や MessageTrackingTag の送信は TILDE が行なう。 + - その為、基本的にはアプリケーションで TILDE や MessageTrackingTag のことを考える必要はない。 + - ただし内部でバッファリングしている場合、メッセージを正しくトラッキングするには input_info を明示的に登録する必要がある +- MessageTrackingTag の作成 + - TILDE のカスタム create_publisher によりカスタムの Publisher である TildePublisher が作成される。 + - TildePublisher は入力情報に関するデータを持っている。 + - メイントピックの publish をフックし、メイントピックを送信すると同時に入力情報データから MessageTrackingTag を作成して MessageTrackingTag を送信する。 +- input infos の紐付け + - TILE のカスタム create_subscription により subscription コールバックがフックされる。 + - フック中の処理で TildePublisher に対して入力トピックや header stamp などの情報を登録する。 + +### class + +TILDE では以下のクラス・API を提供します。 + +- rclcpp::Node や rclcpp::Publisher に相当する TildeNode や TIldePublisher +- `create_publisher` や `create_subscription` の TILDE カスタム版 +- いずれもクラス名・関数名が異なることを除けば ROS2 のものと同じシグニチャです。よって機械的変換で組み込むことができます。 + +```mermaid +classDiagram + Node <|-- TildeNode + Publisher <.. TildePublisher + TildeNode <|-- UserNode + TildePublisher <.. UserNode + Subscription~T~ <.. UserNode + + class Node{ + +create_publisher(topic) + +create_subscription(topic, cb) + } + class TildeNode{ + +create_tilde_publisher(topic) + +create_tilde_subscription(topic, cb) + } + class Publisher~T~ { + +publish(msg) + } + class TildePublisher { + +publish(msg) + +main_pub_: Publisher + +message_tracking_tag_pub_: Publisher + -input_info + } + class Subscription~T~ { + } + class UserNode { + +subscription_callback(T msg) + -pub_: TildePublisher + } +``` + +### create_tilde_publisher + +`create_tilde_publisher(topic, ...)` によりメインメッセージと MessageTrackingTag 用の publisher が作成されます。 +MessageTrackingTag 用のトピック名はメイントピック名に `/message_tracking_tag` という接尾語がついたものです。 + +```mermaid +sequenceDiagram + UserNode-->>TildeNode: create_tilde_publisher(topic, ...) + activate TildeNode + TildeNode-->TildeNode: tilde_pub_ = new TildePublisher + TildeNode-->topic: tilde_pub_.main_pub_ = create_publisher(topic, ...) + note over topic: topic created + TildeNode-->topic/message_tracking_tag: tilde_pub.message_tracking_tag_pub_ = create_publisher(topic + "/message_tracking_tag", ...) + note over topic/message_tracking_tag: topic created + TildeNode-->>UserNode: tilde_pub + deactivate TildeNode +``` + +`create_tilde_publisher` の返り値は rclcpp::Publisher ではなく TildePublisher です。 + +### create_tilde_subscription + +`create_tilde_subscription(topic, qos, cb)` により Subscription が作成されます。 +TILDE では subscription callback をフックする為、ユーザ指定のコールバック関数 cb を TILDE 用のコールバック関数でラップした新たなコールバック関数を登録します。 + +疑似コードで記載すると以下の様になります。 + +```cpp +void create_tilde_subscription(topic, qos, cb) { + auto tilde_cb = [this, topic, cb](T msg) { + // MessageTrackingTag 用に入力情報の紐付け + auto sub_time = now(); + this->tilde_pub.set_input_info( + topic, + sub_time, + msg.header.stamp); + + // cb の呼び出し + cb(msg); + }; + + return this->create_subscription(topic ,qos, tilde_cb); +} +``` + +subscription 時の動作は以下の通りです。 +subscription 時に TildePublisher の input_info 情報を登録します。 + +```mermaid +sequenceDiagram + topic-->>Executor: msg + Executor-->>+UserNode: tilde_callback(msg) + UserNode-->>UserNode: register input_info + activate UserNode + UserNode-->>UserNode: cb(msg) = user defined callback + deactivate UserNode + UserNode-->-Executor: ret +``` + +### publish + +TildePublisher は登録済みの `input_info` を参照して MessageTrackingTag を作成します。 +MessageTrackingTag とメインメッセージを送信します。 + +```mermaid +sequenceDiagram + UserNode-->>TildePublisher: pub.publish(msg) + activate TildePublisher + TildePublisher-->TildePublisher: message_tracking_tag = get_message_tracking_tag() + + TildePublisher-->>topic/message_tracking_tag: message_tracking_tag_pub_.publish(message_tracking_tag) + TildePublisher-->>topic: main_pub_.publish(msg); + deactivate TildePublisher +``` + +## Explicit API + +[NodeC の MessageTrackingTag](#nodec-の-messagetrackingtag) では「メインメッセージ送信前に受信した最新のメッセージ」に紐付けられると記述しました。 +受信メッセージを内部でバッファして選択的に利用している、あるいは入力トピックが複数ありそれぞれバッファリングしている等、入力トピックと出力トピックが明示的に紐付かないノードでは explicit API を使って明示的に紐付け情報を設定することが可能です。 + +下図は 4 入力、 1 出力のノードの例です。 +それぞれの入力はバッファされ publish 時に選択的に利用されます。 +この図では `/left` は L1-L3 の 3 世代あり、 L2 が使われています。 + +```text + 入力をバッファ callback の中でバッファの中から + 利用するデータを選択 + buffer +- callback-+ +/left --> L1 L2 L3 -> | L2 | + | | +/right --> R1 R2 -> | R1 | --> /concatenate + | | +/top --> T1 T2 T3 -> | T2 | + | | +/twist --> W1 W2 W3 -> | W2 W3 | + +-----------+ + ^ + TILDE では「最も最近受信した stamp」しか覚えていない為、正確な MessageTrackingTag を作成できない +``` + +Explicit API では「`/concatenate` を作成するのに `/left` の L2、`/right` の R1 (以下略)を使った」という指定ができます。 + +## オーバーヘッド + +※ TODO: 大体以下を記載する。 + +- TILDE 有無時のオーバヘッド + - autoware か demo システムを対象に TILDE 適用有無時のベンチマーク結果を記載する + - CPU 利用率やネットワーク帯域など ← リソース分析で検討しているプロセスリソースモニタリングツールが使える? diff --git a/doc/usecase.md b/doc/usecase.md new file mode 100644 index 00000000..2e561094 --- /dev/null +++ b/doc/usecase.md @@ -0,0 +1,117 @@ +# TILDE の想定ユースケース + +TILDE では以下の様なユースケースを想定ユースケースしています。 + +- オンライン測定 +- デッドライン検出 + - パスの途中終了 + - 入力情報の「賞味期限」検出 + + + +## Table of Contents + +- [TILDE の想定ユースケース](#tilde-の想定ユースケース) + - [Table of Contents](#table-of-contents) + - [TILDE で分かること](#tilde-で分かること) + - [オンラインレイテンシ計測](#オンラインレイテンシ計測) + - [オンライン性](#オンライン性) + - [オフライン性ツール CARET との比較](#オフライン性ツール-caret-との比較) + - [デッドライン検出](#デッドライン検出) + + + +## TILDE で分かること + +ROS2 のトピック通信は、一般に以下の様な有向グラフ(DAG)を構成します。 + +![tilde_dag](./images/tilde_dag.svg) + +TILDE では、各ノードでメインのトピックを publish する際に MessageTrackingTag という「メイントピックを構成する入力トピックの情報(トピック)」を同時に publish します。 +メッセージの特定の為、メインのトピックは ROS2 の [std_msgs/msg/Header](https://github.com/ros2/common_interfaces/blob/master/std_msgs/msg/Header.msg) フィールドを持ち stamp フィールドに適切な値を設定している必要があります。 + +以下の図の様なケースを考えます。 +stamp はメイントピックの Header stamp で、説明の為単位は秒とします。 + +```mermaid +graph LR + /sensor/topic/A --stamp=4--> FusionNode((FusionNode)) + /sensor/topic/B --stamp=6--> FusionNode + FusionNode --stamp=8-->/sensor/fusion + /sensor/fusion --stamp=8--> PlanningNode((PlanningNode)) + /sensor/topic/B --stamp=3--> PlanningNode + PlanningNode --stamp=10-->/planning/base +``` + +FusionNode は以下の様な MessageTrackingTag を送信します。 + +- **MessageTrackingTag(FusionNode)**: stamp=8 の `/sensor/fusion` は以下のトピックを参照した + - t=5 に受信した stamp=4 の `/sensor/topic/A` + - t=7 に受信した stamp=6 の `/sensor/topic/B` + - 他、トピックの送信時刻などの付属情報 + +PlanningNode も同様の MessageTrackingTag を送信します。 + +- **MessageTrackingTag(PlanningNode)**: stamp=10 の `/planning/base` は以下のトピックを参照した + - t=8 に受信した stamp=8 の `/sensor/fusion` + - t=4 に受信した stamp=3 の `/sensor/topic/B` + +これらの情報から、stamp=10 の `/planning/base` は以下のセンサー情報を元に算出されたと分かります。 + +- **推論**: stamp=10 の `/planning/base` は以下のトピックを参照している + - `/sensor/topic/A`: stamp=4 のもの(`/sensor/fusion` 経由) + - `/sensor/topic/B`: stamp=6 のもの(`/sensor/fusion` 経由) と stamp=3 のもの(直接受信) + +同様の推論を繰り返すことで「stamp=X の `/control/cmd` はいつのセンサー情報を用いたか?」という問いに答えることができます。 +MessageTrackingTag を元に、DAG を逆向きに遡ってデータの紐付けを行うことからこの様な推論を **MessageTrackingTag の探索** と呼称します。 + +## オンラインレイテンシ計測 + +上記の推論から stamp=10 の `/planning/base` は各センサーの最も古い値として以下を参照していることが分かります。 + +- `/sensor/topic/A`: stamp=4 のもの(`/sensor/fusion` 経由) +- `/sensor/topic/B`: stamp=3 のもの(直接受信) + +「センサーの stamp = センサーの計測時刻」と仮定するとこのデータを作るまで **センサー A 取得から 6 秒, センサー B からは 7 秒かかった** と言えます。 + +他のノードでも同様の推論が可能です。 + +また、 MessageTrackingTag の探索を途中で止めることでノード間にかかった時間も分かります。 +例えば 「/sensor/fusion が送信されてから /control/cmd が送信されるまで X 秒かかった」などの推論ができます。 + +この様にして TILDE を用いてレンテンシを計測することが可能です。 + +### オンライン性 + +MessageTrackingTag は topic で送信される為、計測対象のシステムを動かしながら MessageTrackingTag を解析することが可能です。 +latency viewer では top や vmstat の様にシステムを動かしながらレンテンシを見ることができます。 +TILDE ではこの様にシステムを動かしながらメトリククを計算できる性質を **オンライン** と呼称しています。 + +### オフライン性ツール CARET との比較 + +[CARET](https://tier4.github.io/CARET_doc/) は LTTng のトレースポイントを用いて計算する為、システムを動かした後に性能を評価します。この様に事後に解析することを、**オフライン** と呼称しています。 + +レンテンシ計測の観点では TILDE と CARET は似ていますが、以下の様に特徴付けられます。 + +- TILDE はオンラインにレイテンシを計測できるが取得できるレイテンシは荒い +- CARET はオフライン計測だが、細かいレイテンシを取得できる + +## デッドライン検出 + +> **_NOTE:_** (2022/01/21 現在本機能は未実装) + +以下の様な DAG を考えます。この DAG は所定の時間内(例えば 100 ms 以内)に処理を追える必要があるとします。 +NodeA、NodeB はタイマーで動作すると考えます。 + +![tilde_deadline](./images/tilde_deadline.svg) + +上記の通り TILDE により TargetNode は topicA の stamp や送信時間を取得できる為、「TargetNode の処理開始時点で topicA 送信から 80 ms 経過している」等と推論できます。 + +その他 TILDE の情報を用いることで以下の様な問題を検出することができます。 + +- (1) 直接的な入力トピックの遅延 (case1) + - NodeB やネットワークの遅延により TargetNode が topicB を想定した周期で受信できない場合があります。 + - TargetNode 実行時に「最後に topicB を受信した」を取得できる為、この様な問題を検出できます。 +- (2) 間接的な入力トピックの遅延 (case2) + - NodeB は正しい周波数で動作しているが NodeA が停止・あるいは topicA の遅延などで、NodeB が参照している情報が古い可能性があります。 + - TILDE では MessageTrackingTag を辿ることで、センサーや途中のトピックの諸元を調べることができるため、この様な問題を検出できます。 diff --git a/elisp/README.md b/elisp/README.md new file mode 100644 index 00000000..aa7dea55 --- /dev/null +++ b/elisp/README.md @@ -0,0 +1,50 @@ +# TILDE view mode + +## About + +Emacs major mode for viewing latency viewer stdout log. +You can see stdout log just like ncurses window. + +## Usage + +First, run tilde latency viewer with `--batch` mode. +Redirect stdout to file. + +Open the log file, and `M-x tilde-view-mode`. +Of course, you need read tilde-view-mode.el in advance +by putting load path, or `M-x eval-buffer` or other fancy way. + +## Key Bindings + +The latency viewer log file has space-index structure. +In the non batch mode, latency viewer shows lines between `/target/topic`. +Let's call these lines a BLOCK. + +```text +stamp ... +/target/topic + /depth1/child/topic1 + /depth2/child/topic1 + /depth2/child/topic2 + /depth1/child/topic2 + /depth2/child/topic3 +stamp ... + (snip) +``` + +You can select "wide" and "narrow" view. + +### wide view mode + +In the wide view, we can see full text. +Additionally, we can fold sub trees by hitting "Tab" or "o". +Type "a" to show all lines. + +### narrow view mode + +Move cursor inside BLOCK, and hit "space" to narrow region. +Hit "n" or "p" to move BLOCKs. +Hit "w" to widen. + +We can use the narrow view mode, just like latency viewer non-batch mode. +We can also move the block with the region folded. diff --git a/elisp/tilde-view-mode.el b/elisp/tilde-view-mode.el new file mode 100644 index 00000000..270a7d56 --- /dev/null +++ b/elisp/tilde-view-mode.el @@ -0,0 +1,79 @@ +(defun tilde-narrow-block () + (interactive) + (save-excursion + (forward-char) + (let* ((bgn (re-search-backward "^/" nil t)) + (end (re-search-forward "^/" nil t 2)) + ) + (if end + (progn + (message (int-to-string end)) + (narrow-to-region bgn (- end 1)) + ) + (narrow-to-region bgn (point-max)) + )))) + +(defun tilde-next-block () + (interactive) + (forward-char) + (re-search-forward "^/" nil 1) + (backward-char)) + +(defun tilde-prev-block () + (interactive) + (re-search-backward "^/" nil 1)) + +(defun tilde-next-block-with-narrow () + (interactive) + (if (not (buffer-narrowed-p)) + (progn + (tilde-next-block)) + (widen) + (tilde-next-block) + (tilde-narrow-block) + )) + +(defun tilde-prev-block-with-narrow () + (interactive) + (if (not (buffer-narrowed-p)) + (progn + (tilde-prev-block)) + (widen) + (tilde-prev-block) + (tilde-narrow-block) + )) + +(defun tilde-previous-line () + (interactive) + (previous-line) + (move-beginning-of-line 1)) + +(defun tilde-next-line () + (interactive) + (next-line) + (move-beginning-of-line 1)) + +(defun tilde-view-mode () + (interactive) + (kill-all-local-variables) + (setq major-mode 'tilde-view-mode + mode-name "TILDE view mode") + (outline-minor-mode) + (setq outline-regexp " *") + + (setq tilde-view-mode-map (make-keymap)) + (suppress-keymap tilde-view-mode-map) + (define-key tilde-view-mode-map (kbd "TAB") 'outline-toggle-children) + (define-key tilde-view-mode-map "a" 'outline-show-all) + (define-key tilde-view-mode-map "o" 'outline-hide-sublevels) + + (define-key tilde-view-mode-map "j" 'tilde-next-line) + (define-key tilde-view-mode-map "k" 'tilde-previous-line) + (define-key tilde-view-mode-map "n" 'tilde-next-block-with-narrow) + (define-key tilde-view-mode-map "p" 'tilde-prev-block-with-narrow) + + (define-key tilde-view-mode-map "w" 'widen) + (define-key tilde-view-mode-map (kbd "SPC") 'tilde-narrow-block) + + (use-local-map tilde-view-mode-map) + ) diff --git a/setup.cfg b/setup.cfg new file mode 100644 index 00000000..5214751c --- /dev/null +++ b/setup.cfg @@ -0,0 +1,15 @@ +[flake8] +# Modified from https://github.com/ament/ament_lint/blob/ebd524bb9973d5ec1dc48a670ce54f958a5a0243/ament_flake8/ament_flake8/configuration/ament_flake8.ini +extend-ignore = B902,C816,D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404,I202,CNL100,E203,E501,Q000 +import-order-style = pep8 +max-line-length = 100 +show-source = true +statistics = true + +[isort] +profile=black +line_length=100 +force_sort_within_sections=true +force_single_line=true +reverse_relative=true +known_third_party=launch diff --git a/src/analysis/README.md b/src/analysis/README.md new file mode 100644 index 00000000..7dd18c84 --- /dev/null +++ b/src/analysis/README.md @@ -0,0 +1,103 @@ +# old scripts for analysis + +## Description + +ros2 の bag や graph を操作するためのユーティリティ群。 +オプションは `-h` でヘルプを参照のこと。 + +## topic_times.py + +bag file 内のトピックの送信時刻を出力する。 +時刻は header field の timestamp を参照する。無い場合はその旨出力して終了する。 +シミュレータを動かし全トピックを計測の上、送信時刻を確認するなど。 + +```bash +$ ./topic_times.py + +$ ./topic_times.py lsim_all_topics/lsim_all_topics_0.db3 /sensing/lidar/top/rectified/pointcloud +1618559266.752622000: +1618559266.850460000: +1618559266.950311000: +1618559267.506740000: +1618559267.150548000: +1618559267.250492000: +1618559267.350317000: +1618559267.450254000: +1618559267.550121000: +1618559267.650272000: +1618559267.750288000: +``` + +## topic_traversal.py + +topic や node の from, to を指定して最短経路を取得する。 +`rqt_graph` がバックエンドの為、実機やシミュレータなど対象システムが動作している状態で実行すること。 + +```bash +$ ./topic_traversal.py + +$ ./topic_traversal.py /sensing/lidar/top/pointcloud_raw_ex /sensing/lidar/top/rectified/pointcloud +Graph refresh: done, updated[True] +ROS stats update took 1.032935380935669s + +args: /sensing/lidar/top/pointcloud_raw_ex -> /sensing/lidar/top/rectified/pointcloud +graph: /sensing/lidar/top/pointcloud_raw_ex| -> /sensing/lidar/top/rectified/pointcloud| + +Found + ' /sensing/lidar/top/pointcloud_raw_ex|' + '/sensing/lidar/top/crop_box_filter_self|' + ' /sensing/lidar/top/self_cropped/pointcloud_ex|' + '/sensing/lidar/top/crop_box_filter_mirror|' + ' /sensing/lidar/top/mirror_cropped/pointcloud_ex|' + '/sensing/lidar/top/velodyne_interpolate_node|' + ' /sensing/lidar/top/rectified/pointcloud|' +``` + +Found のうち、半角スペース始まりは topic、そうでないものは Node の様子(不確かか書き方なのは内部で使っている `rqt_graph` が加工したもののため)。 + +## graph_around + +ある地点からグラフを BFS してノードやトピックを出力する。 + +```bash +$ ./graph_around.py /localization/pose_twist_fusion_filter/ekf_localizer --depth 2 + +depth: 0 +now: '/localization/pose_twist_fusion_filter/ekf_localizer|' + +depth: 1 +now: ' /tf|' +now: ' /localization/pose_twist_fusion_filter/pose|' +now: ' /localization/pose_twist_fusion_filter/pose_with_covariance|' +now: ' /localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias|' +now: ' /localization/pose_twist_fusion_filter/twist|' + +depth: 2 +now: '/perception/object_recognition/prediction/transform_listener_impl_55931a9941c0|' +now: '/sensing/lidar/pointcloud_preprocessor/transform_listener_impl_5588e2e7d940|' +now: '/sensing/lidar/pointcloud_preprocessor/transform_listener_impl_5588e2f57090|' +(snip) +``` + +## parse_message_tracking_tag.py + +PugInfo が保存された bag ファイルを message_tracking_tag.py のデータ書式に変換し +pkl 形式で保存する。 +結果は `${CWD}/topic_infos.pkl` に保存される。 + +```bash +parse_message_tracking_tag.py +``` + +## message_tracking_tag_traverse.py + +parse_message_tracking_tag.py で作成された pkl ファイルを元に E2E latency を算出する。 +第三引数が終点となるトピック名。 +第二引数は何番目に送信されたトピック(正確には Message_Tracking_Tag)を対象とするか。あまり若い数字だと初期化中の為辿り付けないセンサーが発生することがある。 + +```bash +./message_tracking_tag_traverse.py \ + /topic_infos.pkl \ + 1000 \ + /localization/pose_twist_fusion_filter/twist_with_covariance +``` diff --git a/src/analysis/create_bag_for_concat_filter.py b/src/analysis/create_bag_for_concat_filter.py new file mode 100755 index 00000000..f00eeb68 --- /dev/null +++ b/src/analysis/create_bag_for_concat_filter.py @@ -0,0 +1,96 @@ +#!/usr/bin/python3 + +import argparse +import os +import shutil + +import rosbag2_py +from rosidl_runtime_py.utilities import get_message +from rclpy.serialization import deserialize_message + +serialization_format = 'cdr' + + +def main(args): + in_bag_path = args.in_bag + out_bag_path = args.out_dir + + if os.path.exists(out_bag_path): + shutil.rmtree(out_bag_path) + + in_storage_options = rosbag2_py.StorageOptions(uri=in_bag_path, storage_id='sqlite3') + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + reader = rosbag2_py.SequentialReader() + reader.open(in_storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + topics = [ + "/sensing/lidar/left/outlier_filtered/pointcloud", + "/sensing/lidar/right/outlier_filtered/pointcloud", + "/sensing/lidar/top/outlier_filtered/pointcloud", + "/vehicle/status/twist", + "/clock" + ] + + # topics to save. + # design: + # + # We save all of twist and clock. + topics_to_save = { + "/sensing/lidar/left/outlier_filtered/pointcloud": [ + 500, 505 + ], + "/sensing/lidar/right/outlier_filtered/pointcloud": [ + 500, 504, 505 + ], + "/sensing/lidar/top/outlier_filtered/pointcloud": [ + 500, + ] + } + + storage_filter = rosbag2_py.StorageFilter(topics=topics) + reader.set_filter(storage_filter) + + out_storage_options = rosbag2_py.StorageOptions(out_bag_path, "sqlite3") + writer = rosbag2_py.SequentialWriter() + writer.open(out_storage_options, converter_options) + + for meta in reader.get_all_topics_and_types(): + writer.create_topic(meta) + + counts = {k: 0 for k in topics} + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + counts[topic] += 1 + + if "clock" in topic: + writer.write(topic, data, t) + elif "twist" in topic: + writer.write(topic, data, t) + elif counts[topic] in topics_to_save[topic]: + print(f"{topic}: {msg.header.stamp}") + writer.write(topic, data, t) + + if counts["/sensing/lidar/left/outlier_filtered/pointcloud"] == 1000: + break + + del writer + rosbag2_py.Reindexer().reindex(out_storage_options) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("in_bag", help="input bagfile") + parser.add_argument("out_dir", help="output directory") + args = parser.parse_args() + + main(args) diff --git a/src/analysis/graph_around.py b/src/analysis/graph_around.py new file mode 100755 index 00000000..ab86ddba --- /dev/null +++ b/src/analysis/graph_around.py @@ -0,0 +1,89 @@ +#!/usr/bin/env python3 +# list around the node + +import argparse +from collections import deque + +import rclpy +from rosgraph2_impl import Edge + +from graph_common import find_key, get_graph + +EXCLUDE_TOPICS = [ + "/parameter_events", + "/rosout", + "/clock", + ] + + +def main(args): + frm = args.frm + max_depth = args.depth + least_edges = args.least_edges + + rclpy.init() + graph = get_graph(least_edges) + + key = find_key(graph.nt_edges.edges_by_end, frm) + if not key: + print("{} not found".format(frm)) + return + print("key: '{}'".format(key)) + + seen = set() + current_item = deque([key]) + next_item = deque([]) + depth = 0 + + edges = graph.nt_edges.edges_by_start + print("depth: {}".format(depth)) + while len(current_item) != 0 and depth <= max_depth: + now = current_item.popleft() + print("now: '{}'".format(now)) + + for nxt in edges[now]: + if isinstance(nxt, Edge): + # print("nxt.start: {}".format(nxt.start)) + nxt = nxt.end + nxt = nxt.strip() + if nxt in EXCLUDE_TOPICS: + continue + nxt = find_key(edges, nxt) + if nxt is None: + continue + if nxt in seen: + continue + seen.add(nxt) + next_item.append(nxt) + + # for n in edges[nxt]: + # if isinstance(n, Edge): + # # print("nxt.start: {}".format(nxt.start)) + # n = n.end + # n = n.strip() + # n = find_key(edges, n) + # if n in EXCLUDE_TOPICS: + # continue + # if n is None: + # continue + # if n in seen: + # continue + # next_item.append(n) + + if len(current_item) == 0: + depth += 1 + current_item = next_item + next_item = deque([]) + print("") + if depth <= max_depth: + print("depth: {}".format(depth)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("frm") + parser.add_argument("--depth", type=int, default=2) + parser.add_argument("--least-edges", type=int, default=200, + help="the least number of edges to wait for graph.update") + args = parser.parse_args() + main(args) diff --git a/src/analysis/graph_common.py b/src/analysis/graph_common.py new file mode 100644 index 00000000..81d17bfc --- /dev/null +++ b/src/analysis/graph_common.py @@ -0,0 +1,33 @@ +import rclpy +from rosgraph2_impl import Graph + + +def get_graph(least_num, prints_edge=False): + node = rclpy.create_node("topic_traversal_node") + graph = Graph(node) + graph.update() + + cnt = 0 + while cnt < 10 and len(graph.nt_edges.edges_by_end) <= least_num: + cnt += 1 + graph = Graph(node) + graph.update() + + if len(graph.nt_edges.edges_by_end) <= least_num: + raise Exception("not enough edges") + + print("found {} keys".format(len(graph.nt_edges.edges_by_end.keys()))) + + if prints_edge: + for k in graph.nt_edges.edges_by_end.keys(): + print(" '{}'".format(k)) + + return graph + + +def find_key(edges, name): + for key in edges: + if name in key and 0 <= len(key) - len(name) <= 2: + # print("find_key: {} -> {}".format(name, key)) + return key + return None diff --git a/src/analysis/message_tracking_tag.py b/src/analysis/message_tracking_tag.py new file mode 100644 index 00000000..e79286dc --- /dev/null +++ b/src/analysis/message_tracking_tag.py @@ -0,0 +1,131 @@ +def time2str(t): + return f"{t.sec}.{t.nanosec:09d}" + + +class TopicInfo(object): + def __init__(self, topic, has_stamp, stamp): + self.topic = topic + self.has_stamp = has_stamp + self.stamp = stamp + + def __str__(self): + stamp_s = time2str(self.stamp) if self.has_stamp else "NA" + return f"TopicInfo(topic={self.topic}, stamp={stamp_s})" + + +class Message_Tracking_Tag(object): + """ + Message_Tracking_Tag. + + - out_info = TopicInfo + - in_infos = {topic_name => list of TopicInfo} + """ + + def __init__(self, out_topic, out_stamp): + self.out_info = TopicInfo(out_topic, True, out_stamp) + # topic name vs TopicInfo + self.in_infos = {} + + def add_input_info(self, in_topic, has_stamp, stamp): + self.in_infos.setdefault(in_topic, []).append(TopicInfo(in_topic, has_stamp, stamp)) + + def __str__(self): + s = "Message_Tracking_Tag: \n" + s += f" out_info={self.out_info}\n" + for _, ti in self.in_infos.items(): + s += f" in_infos={ti}\n" + return s + + @property + def out_topic(self): + return self.out_info.topic + + +class Message_Tracking_Tags(object): + """ + Topic vs Message_Tracking_Tag. + + We have double-key dictionary internally, i.e. + we can get Message_Tracking_Tag by topic_vs_message_tracking_tag[topic_name][stamp]. + """ + + def __init__(self): + self.topic_vs_message_tracking_tags = {} + + def add(self, message_tracking_tag): + out_topic = message_tracking_tag.out_info.topic + out_stamp = time2str(message_tracking_tag.out_info.stamp) + if out_topic not in self.topic_vs_message_tracking_tags.keys(): + self.topic_vs_message_tracking_tags[out_topic] = {} + infos = self.topic_vs_message_tracking_tags[out_topic] + + if out_stamp not in infos.keys(): + infos[out_stamp] = {} + + infos[out_stamp] = message_tracking_tag + + def topics(self): + return list(self.topic_vs_message_tracking_tags.keys()) + + def stamps(self, topic): + """Return List[stamps].""" + if topic not in self.topic_vs_message_tracking_tags.keys(): + return [] + + return list(self.topic_vs_message_tracking_tags[topic].keys()) + + def get(self, topic, stamp=None, idx=None): + """ + Get corresponding Message_Tracking_Tag. + + topic: str + stamp: str + """ + ret = None + if topic not in self.topic_vs_message_tracking_tags.keys(): + return None + + if stamp is None and idx is None: + return list(self.topic_vs_message_tracking_tags[topic].values())[0] + + infos = self.topic_vs_message_tracking_tags[topic] + + if stamp in infos.keys(): + ret = infos[stamp] + + if idx is not None: + if len(infos.keys()) <= idx: + print("args.idx too large, should be < {len(info.kens())}") + else: + key = sorted(infos.keys())[idx] + ret = infos[key] + + return ret + + def in_topics(self, topic): + """ + Gather input topics by ignoring stamps. + + return set of topic name + """ + ret = set() + + if topic not in self.topic_vs_message_tracking_tags.keys(): + return ret + + for message_tracking_tag in self.topic_vs_message_tracking_tags[topic].values(): + for t in message_tracking_tag.in_infos.keys(): + ret.add(t) + return ret + + def all_topics(self): + out = set() + + lhs = self.topics() + for t in lhs: + out.add(t) + + rhs = self.in_topics(t) + for t in rhs: + out.add(t) + return out diff --git a/src/analysis/parse_message_tracking_tag.py b/src/analysis/parse_message_tracking_tag.py new file mode 100755 index 00000000..6ff1c757 --- /dev/null +++ b/src/analysis/parse_message_tracking_tag.py @@ -0,0 +1,84 @@ +#!/usr/bin/python3 + +import pickle +import argparse + +import rosbag2_py +from rosidl_runtime_py.utilities import get_message +from rclpy.serialization import deserialize_message + +from message_tracking_tag import Message_Tracking_Tag, Message_Tracking_Tags + + +def get_rosbag_options(path, serialization_format='cdr'): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + return storage_options, converter_options + + +def main(args): + bag_path = args.bag_path + + storage_options, converter_options = get_rosbag_options(bag_path) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + cnt = 0 + + # topic => list of record + out_per_topic = Message_Tracking_Tags() + + skip_topic_vs_count = {} + + while reader.has_next() and cnt <= args.cnt: + (topic, data, t) = reader.read_next() + # TODO: need more accurate check + if "/message_tracking_tag" not in topic: + continue + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + out_topic = msg.output_info.topic_name + out_stamp = msg.output_info.header_stamp + + message_tracking_tag = Message_Tracking_Tag(out_topic, out_stamp) + + for input_info in msg.input_infos: + in_topic = input_info.topic_name + in_has_stamp = input_info.has_header_stamp + if not in_has_stamp: + if in_topic not in skip_topic_vs_count.keys(): + print(f"skip {in_topic} as no header.stamp, out_topic = {out_topic}") + skip_topic_vs_count[in_topic] = 1 + else: + skip_topic_vs_count[in_topic] += 1 + continue + in_stamp = input_info.header_stamp + + message_tracking_tag.add_input_info(in_topic, in_has_stamp, in_stamp) + + out_per_topic.add(message_tracking_tag) + + pickle.dump(out_per_topic, open("topic_infos.pkl", "wb"), protocol=pickle.HIGHEST_PROTOCOL) + + for topic, count in skip_topic_vs_count.items(): + print(f"skipped {topic} {count} times") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("bag_path") + parser.add_argument("--cnt", type=int, default=10) + args = parser.parse_args() + + main(args) diff --git a/src/analysis/pubinfo_traverse.py b/src/analysis/pubinfo_traverse.py new file mode 100755 index 00000000..7d5c8ac7 --- /dev/null +++ b/src/analysis/pubinfo_traverse.py @@ -0,0 +1,206 @@ +#!/usr/bin/python3 + +import pickle +import argparse +from collections import deque, defaultdict +import time + +from rclpy.time import Time + +from message_tracking_tag import time2str + + +def str_stamp2time(timestamp: str): + sec, nanosec = timestamp.split(".") + return Time(seconds=int(sec), nanoseconds=int(nanosec)) + + +class InputSensorStampSolver(object): + def __init__(self): + # {topic: {stamp: {sensor_topic: [stamps]}}} + self.topic_stamp_to_sensor_stamp = {} + + def solve(self, message_tracking_tags, tgt_topic, tgt_stamp): + """ + Solve. + + topic: target topic + stamp: target stamp(str) + + return list of {sensor: oldest stamp} + + Calculate topic_stamp_to_sensor_stamp internally. + """ + graph = TopicGraph(message_tracking_tags) + path_bfs = graph.bfs_rev(tgt_topic) + is_leaf = {t: b for (t, b) in path_bfs} + + stamp = tgt_stamp + + # dists[topic][stamp] + dists = defaultdict(lambda: defaultdict(lambda: -1)) + queue = deque() + parentQ = deque() + + dists[tgt_topic][tgt_stamp] = 1 + queue.append((tgt_topic, tgt_stamp)) + parentQ.append("") + + st = time.time() + start = None + while len(queue) != 0: + topic, stamp = queue.popleft() + parent = parentQ.popleft() + is_leaf_s = "looks_sensor" if is_leaf[topic] else "" + + if start is None: + start = str_stamp2time(stamp) + dur = start - str_stamp2time(stamp) + dur_ms = dur.nanoseconds // 10**6 + + print(f"{topic:80} {stamp:>20} {dur_ms:4} ms {is_leaf_s} {parent}") + + # NDT-EKF has loop, so skip + if topic == "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias": + continue + + # get next edges + next_message_tracking_tag = message_tracking_tags.get(topic, stamp) + if not next_message_tracking_tag: + continue + + for in_infos in next_message_tracking_tag.in_infos.values(): + for in_info in in_infos: + nx_topic = in_info.topic + nx_stamp = time2str(in_info.stamp) + if dists[nx_topic][nx_stamp] > 0: + continue + dists[nx_topic][nx_stamp] = dists[topic][stamp] + 1 + queue.append((nx_topic, nx_stamp)) + parentQ.append(topic) + + et = time.time() + print(f"solve internal: {(et-st)*1000} [ms]") + + def append(self, topic, stamp, sensor_topic, sensor_stamp): + dic = self.topic_stamp_to_sensor_stamp + if topic not in dic.keys(): + dic[topic] = {} + if stamp not in dic[topic].keys(): + dic[topic][stamp] = {} + if sensor_topic not in dic[topic][stamp].keys(): + dic[topic][stamp][sensor_topic] = [] + + dic[topic][stamp][sensor_topic].append(sensor_stamp) + + +class TopicGraph(object): + """Construct topic graph by ignoring stamps.""" + + def __init__(self, message_tracking_tags): + self.topics = sorted(message_tracking_tags.all_topics()) + self.t2i = {t: i for i, t in enumerate(self.topics)} + n = len(self.topics) + + # from sub -> pub + self.topic_edges = [set() for _ in range(n)] + # from out -> in + self.rev_edges = [set() for _ in range(n)] + for out_topic in self.topics: + in_topics = message_tracking_tags.in_topics(out_topic) + + out_id = self.t2i[out_topic] + for in_topic in in_topics: + in_id = self.t2i[in_topic] + self.topic_edges[in_id].add(out_id) + self.rev_edges[out_id].add(in_id) + + def rev_topics(self, topic): + """ + Get input topics. + + return List[Topic] + """ + input_topics = self.t2i[topic] + return [self.topics[i] for i in input_topics.rev_edges] + + def dfs_rev(self, start_topic): + """ + Traverse topic graph reversely from start_topic. + + return topic names in appearance order + """ + edges = self.rev_edges + n = len(edges) + seen = [False for _ in range(n)] + sid = self.t2i[start_topic] + + ret = [] + + def dfs(v): + ret.append(self.topics[v]) + seen[v] = True + for nxt in edges[v]: + if seen[nxt]: + continue + dfs(nxt) + dfs(sid) + + return ret + + def bfs_rev(self, start_topic): + """Return list of (topic, is_leaf).""" + edges = self.rev_edges + n = len(edges) + dist = [-1 for _ in range(n)] + queue = deque() + + sid = self.t2i[start_topic] + dist[sid] = 0 + queue.append(sid) + paths = [] + + while len(queue) != 0: + v = queue.popleft() + paths.append((self.topics[v], len(edges[v]) == 0)) + for nv in edges[v]: + if dist[nv] >= 0: + continue + dist[nv] = dist[v] + 1 + queue.append(nv) + + return paths + + +def main(args): + pickle_file = args.pickle_file + message_tracking_tags = pickle.load(open(pickle_file, "rb")) + + tgt_topic = args.topic + tgt_stamp = sorted(message_tracking_tags.stamps(tgt_topic))[args.stamp_index] + + # graph = TopicGraph(message_tracking_tags) + # bfs_path = graph.bfs_rev(tgt_topic) + # print(f"BFS from {tgt_topic}") + # for p, is_leaf in bfs_path: + # print(f" {p} {is_leaf}") + # print("") + + st = time.time() + solver = InputSensorStampSolver() + solver.solve(message_tracking_tags, tgt_topic, tgt_stamp) + et = time.time() + + print(f"solve {(et-st) * 1000} [ms]") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("pickle_file") + parser.add_argument("stamp_index", type=int, default=0, + help="header stamp index") + parser.add_argument("topic", default="/sensing/lidar/no_ground/pointcloud", nargs="?") + + args = parser.parse_args() + + main(args) diff --git a/src/analysis/rosgraph2_impl.py b/src/analysis/rosgraph2_impl.py new file mode 100644 index 00000000..78b43da8 --- /dev/null +++ b/src/analysis/rosgraph2_impl.py @@ -0,0 +1,589 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2008, Willow Garage, Inc. +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# * Redistributions in binary form must reproduce the above +# copyright notice, this list of conditions and the following +# disclaimer in the documentation and/or other materials provided +# with the distribution. +# * Neither the name of Willow Garage, Inc. nor the names of its +# contributors may be used to endorse or promote products derived +# from this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +# Revision $Id$ +# Adopted from the ROS1 ros_comm repository +# https://github.com/ros/ros_comm/blob/5de058ad1bbf3a525ae3f5288a76e59ec0dd62d0/tools/rosgraph/src/rosgraph/impl/graph.py + +# flake8: noqa + +from __future__ import print_function + +""" +Data structures and library for representing ROS Computation Graph state. +""" + +import time +import itertools +import random + +from collections import defaultdict + +from python_qt_binding.QtCore import qDebug, qWarning +from rclpy.qos import qos_check_compatible +from rclpy.qos import QoSCompatibility + + +_ROS_NAME = '/rosviz' + + +def topic_node(topic): + """ + Write me. + + In order to prevent topic/node name aliasing, we have to remap + topic node names. Currently we just prepend a space, which is + an illegal ROS name and thus not aliased. + @return str: topic mapped to a graph node name. + """ + return ' ' + topic + + +def node_topic(node): + """ + Write me. + + Inverse of topic_node + @return str: undo topic_node() operation + """ + return node[1:] + + +class BadNode(object): + """ + Write me. + + Data structure for storing info about a 'bad' node + """ + + # no connectivity + DEAD = 0 + # intermittent connectivity + WONKY = 1 + + def __init__(self, name, node_type, reason): + """ + Write me. + + @param type: DEAD | WONKY + @type type: int + """ + self.name = name + self.reason = reason + self.type = node_type + + +class EdgeList(object): + """ + Write me. + + Data structure for storing Edge instances + """ + + __slots__ = ['edges_by_start', 'edges_by_end'] + + def __init__(self): + # in order to make it easy to purge edges, we double-index them + self.edges_by_start = {} + self.edges_by_end = {} + + def __iter__(self): + return itertools.chain(*[v for v in self.edges_by_start.values()]) # NOLINT + + def has(self, edge): + return edge in self + + def __contains__(self, edge): + """ + Write me. + + @return: True if edge is in edge list + @rtype: bool + """ + key = edge.key + return key in self.edges_by_start and edge in self.edges_by_start[key] + + def add(self, edge): + """ + Add an edge to our internal representation. not multi-thread safe. + + @param edge: edge to add + @type edge: Edge + """ + # see note in __init__ + def update_map(edge_map, key, edge): + if key in edge_map: + edges = edge_map[key] + if edge not in edges: + edges.append(edge) + return True + else: + return False + else: + edge_map[key] = [edge] + return True + + updated = update_map(self.edges_by_start, edge.key, edge) + updated = update_map(self.edges_by_end, edge.r_key, edge) or updated + return updated + + def add_edges(self, start, dest, direction, label='', qos=None): + """ + Write me. + + Create Edge instances for args and add resulting edges to edge + list. Convenience method to avoid repetitive logging, etc... + @param edge_list: data structure to add edge to + @type edge_list: EdgeList + @param start: name of start node. If None, warning will be logged and add fails + @type start: str + @param dest: name of start node. If None, warning will be logged and add fails + @type dest: str + @param direction: direction string (i/o/b) + @type direction: str + @return: True if update occurred + @rtype: bool + """ + # the warnings should generally be temporary, occurring of the + # master/node information becomes stale while we are still + # doing an update + updated = False + if not start: + qWarning("cannot add edge: cannot map start [%s] to a node name", start) + elif not dest: + qWarning("cannot add edge: cannot map dest [%s] to a node name", dest) + else: + for args in edge_args(start, dest, direction, label, qos): + updated = self.add(Edge(*args)) or updated + return updated + + def delete_all(self, node): + """ + Delete all edges that start or end at node. + + @param node: name of node + @type node: str + """ + def matching(map_, pref): + return [map_[k] for k in map.keys() if k.startswith(pref)] + + pref = node+"|" + edge_lists = matching(self.edges_by_start, pref) + matching(self.edges_by_end, pref) + for el in edge_lists: + for e in el: + self.delete(e) + + def delete(self, edge): + # see note in __init__ + def update_map(map, key, edge): # NOLINT + if key in map: + edges = map[key] + if edge in edges: + edges.remove(edge) + return True + update_map(self.edges_by_start, edge.key, edge) + update_map(self.edges_by_end, edge.r_key, edge) + + +class Edge(object): + """Data structure for representing ROS node graph edge.""" + + __slots__ = ['start', 'end', 'label', 'key', 'r_key', 'qos'] + + def __init__(self, start, end, label='', qos=None): + self.start = start + self.end = end + self.label = label + self.qos = qos + self.key = "%s|%s" % (self.start, self.label) + # reverse key, indexed from end + self.r_key = "%s|%s" % (self.end, self.label) + + def __ne__(self, other): + return self.start != other.start or self.end != other.end + + def __str__(self): + return "%s->%s" % (self.start, self.end) + + def __eq__(self, other): + return self.start == other.start and self.end == other.end and \ + self.label == other.label and self.qos == other.qos + + +def edge_args(start, dest, direction, label, qos): + """ + Compute argument ordering for Edge constructor based on direction flag. + + @param direction str: i, o, or b (in/out/bi_dir) relative to start + @param start str: name of starting node + @param start dest: name of destination node + """ + edge_args = [] + if direction in ['o', 'b']: + edge_args.append((start, dest, label, qos)) + if direction in ['i', 'b']: + edge_args.append((dest, start, label, qos)) + return edge_args + + +class Graph(object): + """ + Utility class for polling ROS statistics from running ROS graph. + + Not multi-thread-safe + """ + + def __init__(self, node, node_ns='/', topic_ns='/'): + self._node = node + self.node_ns = node_ns or '/' + self.topic_ns = topic_ns or '/' + + # ROS nodes + self.nn_nodes = set([]) # NOLINT + # ROS topic nodes + self.nt_nodes = set([]) # NOLINT + + # ROS nodes that aren't responding quickly + self.bad_nodes = {} + import threading + self.bad_nodes_lock = threading.Lock() + + # ROS services + self.srvs = set([]) + # ROS node->node transport connections + self.nn_edges = EdgeList() + # ROS node->topic connections + self.nt_edges = EdgeList() + # ROS all node->topic connections, including empty + self.nt_all_edges = EdgeList() + + # node names to URI map + self.node_uri_map = {} # { node_name_str : uri_str } + # reverse map URIs to node names + self.uri_node_map = {} # { uri_str : node_name_str } + + # time we last updated the graph + self.last_graph_refresh = 0 + self.last_node_refresh = {} + + self.graph_stale = 5.0 + # time we last communicated with node + # seconds until node data is considered stale + self.node_stale = 5.0 # seconds + + # dictionary with qos incompatibilities found + # {topic_name: {publisher_node_name: [subscription_node_name, ...], ...}, ...} + self.topic_with_qos_incompatibility = defaultdict(lambda: defaultdict(list)) + + def set_node_stale(self, stale_secs): + """ + Write me. + + @param stale_secs: seconds that data is considered fresh + @type stale_secs: double + """ + self.node_stale = stale_secs + + def _graph_refresh(self): + """ + Write me. + + @return: True if nodes information was updated + @rtype: bool + """ + updated = False + publishers = defaultdict(list) + subscriptions = defaultdict(list) + servers = defaultdict(list) + + publisher_topic_names = set() + subscriber_topic_names = set() + + for name, namespace in self._node.get_node_names_and_namespaces(): + node_name = namespace + name if namespace.endswith('/') else namespace + '/' + name + + for topic_name, topic_type in \ + self._node.get_publisher_names_and_types_by_node(name, namespace): + publishers[topic_name].append(node_name) + publisher_topic_names.add(topic_name) + + for topic_name, topic_type in \ + self._node.get_subscriber_names_and_types_by_node(name, namespace): + subscriptions[topic_name].append(node_name) + subscriber_topic_names.add(topic_name) + + for service_name, service_type in \ + self._node.get_service_names_and_types_by_node(name, namespace): + servers[service_name].append(node_name) + + publisher_qos_lists = defaultdict(lambda: defaultdict(list)) + for topic_name in publisher_topic_names: + for topic_endpoint_info in self._node.get_publishers_info_by_topic(topic_name): + node_name = topic_endpoint_info.node_namespace + if not node_name.endswith('/'): + node_name += '/' + node_name += topic_endpoint_info.node_name + publisher_qos_lists[topic_name][node_name].append(topic_endpoint_info.qos_profile) + subscriber_qos_lists = defaultdict(lambda: defaultdict(list)) + for topic_name in subscriber_topic_names: + for topic_endpoint_info in self._node.get_subscriptions_info_by_topic(topic_name): + node_name = topic_endpoint_info.node_namespace + if not node_name.endswith('/'): + node_name += '/' + node_name += topic_endpoint_info.node_name + subscriber_qos_lists[topic_name][node_name].append(topic_endpoint_info.qos_profile) + + for topic, node_qos_profiles_dict in publisher_qos_lists.items(): + for pub_node, pub_qos_list in node_qos_profiles_dict.items(): + for sub_node, sub_qos_list in subscriber_qos_lists[topic].items(): + if any( + qos_check_compatible(pub_qos, sub_qos)[0] == QoSCompatibility.ERROR + for pub_qos in pub_qos_list + for sub_qos in sub_qos_list + ): + self.topic_with_qos_incompatibility[topic][pub_node].append(sub_node) + + pubs = list(publishers.items()) + subs = list(subscriptions.items()) + srvs = list(servers.items()) + + nodes = [] + nt_all_edges = EdgeList() + nt_nodes = self.nt_nodes + for state, direction in ((pubs, 'o'), (subs, 'i')): + for topic, l in state: + if topic.startswith(self.topic_ns): + nodes.extend([n for n in l if n.startswith(self.node_ns)]) + nt_nodes.add(topic_node(topic)) + for node in l: + if direction == 'o': + qos_list = publisher_qos_lists[topic][node] + elif direction == 'i': + qos_list = subscriber_qos_lists[topic][node] + else: + qos_list = {None} + for qos in qos_list: + updated |= nt_all_edges.add_edges( + node, topic_node(topic), direction, qos=qos) + self.nt_nodes = nt_nodes + self.nt_all_edges = nt_all_edges + self.nt_edges = nt_all_edges + + nn_edges = EdgeList() + for topic, pub_nodes in publishers.items(): + for pub_node, sub_node in itertools.product(pub_nodes, subscriptions.get(topic, [])): + updated = nn_edges.add_edges(pub_node, sub_node, 'o', topic) or updated + self.nn_edges = nn_edges + + nodes = set(nodes) + + srvs = set([s for s, _ in srvs]) + purge = None + if nodes ^ self.nn_nodes: + purge = self.nn_nodes - nodes + self.nn_nodes = nodes + updated = True + if srvs ^ self.srvs: + self.srvs = srvs + updated = True + + if purge: + qDebug("following nodes and related edges will be purged: %s", ','.join(purge)) + for p in purge: + self.nn_edges.delete_all(p) + self.nt_edges.delete_all(p) + self.nt_all_edges.delete_all(p) + + qDebug("Graph refresh: done, updated[%s]" % updated) + return updated + + def _mark_bad_node(self, node, reason): + try: + # bad nodes are updated in a separate thread, so lock + self.bad_nodes_lock.acquire() + if node in self.bad_nodes: + self.bad_nodes[node].type = BadNode.DEAD + else: + self.bad_nodes[node] = (BadNode(node, BadNode.DEAD, reason)) + finally: + self.bad_nodes_lock.release() + + def _unmark_bad_node(self, node, reason): + """Promotes bad node to 'wonky' status.""" + try: + # bad nodes are updated in a separate thread, so lock + self.bad_nodes_lock.acquire() + bad = self.bad_nodes[node] + bad.type = BadNode.WONKY + finally: + self.bad_nodes_lock.release() + + def _node_refresh_bus_info(self, node, bad_node=False): + """ + Retrieve bus info from the node and update nodes and edges as appropriate. + + @param node: node name + @type node: str + @param api: XML-RPC proxy + @type api: ServerProxy + @param bad_node: If True, node has connectivity issues and + should be treated differently + @type bad_node: bool + """ + if bad_node: + self._mark_bad_node(node, 'Not found during discovery') + return True + + def _node_refresh(self, node, bad_node=False): + """ + Contact node for stats/connectivity information. + + @param node: name of node to contact + @type node: str + @param bad_node: if True, node has connectivity issues + @type bad_node: bool + @return: True if node was successfully contacted + @rtype bool + """ + # TODO: I'd like for master to provide this information in + # getSystemState() instead to prevent the extra connection per node + updated = False + uri = self._node_uri_refresh(node) + + bad_node = (uri is None) + updated = self._node_refresh_bus_info(node, bad_node) + return updated + + def _node_uri_refresh(self, node): + current_nodes = { + namespace + ('' if namespace.endswith('/') else '/') + name + for name, namespace in self._node.get_node_names_and_namespaces()} + if node not in current_nodes: + qWarning('Node "{}" does not exist'.format(node)) + return None + return node + + def bad_update(self): + """ + Write me. + + Update loop for nodes with bad connectivity. We box them separately + so that we can maintain the good performance of the normal update loop. + Once a node is on the bad list it stays there. + """ + last_node_refresh = self.last_node_refresh + + # nodes left to check + try: + self.bad_nodes_lock.acquire() + # make copy due to multithreading + update_queue = self.bad_nodes.values()[:] + finally: + self.bad_nodes_lock.release() + + # return value. True if new data differs from old + updated = False + # number of nodes we checked + num_nodes = 0 + + start_time = time.time() + while update_queue: + # figure out the next node to contact + next = update_queue.pop() + # rate limit talking to any particular node + if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): + updated = self._node_refresh(next.name, True) or updated + # include in random offset (max 1/5th normal update interval) + # to help spread out updates + last_node_refresh[next] = time.time() + (random.random() * self.node_stale / 5.0) + num_nodes += 1 + + # small yield to keep from torquing the processor + time.sleep(0.01) + end_time = time.time() + qDebug("ROS stats (bad nodes) update took %ss" % (end_time - start_time)) + return updated + + def update(self): + """ + Write me. + + Update all the stats. This method may take awhile to complete as it will + communicate with all nodes + master. + """ + last_node_refresh = self.last_node_refresh + + # nodes left to check + update_queue = None + # True if there are still more stats to fetch this cycle + work_to_do = True + # return value. True if new data differs from old + updated = False + # number of nodes we checked + num_nodes = 0 + + start_time = time.time() + while work_to_do: + + # each time through the loop try to talk to either the master + # or a node. stop when we have talked to everybody. + + # get a new node list from the master + if time.time() > (self.last_graph_refresh + self.graph_stale): + updated = self._graph_refresh() + self.last_graph_refresh = time.time() + # contact the nodes for stats + else: + # initialize update_queue based on most current nodes list + if update_queue is None: + update_queue = list(self.nn_nodes) + # no nodes left to contact + elif not update_queue: + work_to_do = False + # contact next node + else: + # figure out the next node to contact + next = update_queue.pop() + # rate limit talking to any particular node + if time.time() > (last_node_refresh.get(next, 0.0) + self.node_stale): + updated = self._node_refresh(next) or updated + # include in random offset (max 1/5th normal update interval) + # to help spread out updates + last_node_refresh[next] = \ + time.time() + (random.random() * self.node_stale / 5.0) + num_nodes += 1 + + # small yield to keep from torquing the processor + time.sleep(0.01) + end_time = time.time() + qDebug("ROS stats update took %ss" % (end_time - start_time)) + return updated diff --git a/src/analysis/topic_times.py b/src/analysis/topic_times.py new file mode 100755 index 00000000..818df908 --- /dev/null +++ b/src/analysis/topic_times.py @@ -0,0 +1,69 @@ +#!/usr/bin/python3 + +import argparse + +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message + + +def get_rosbag_options(path, serialization_format='cdr'): + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + return storage_options, converter_options + + +def main(args): + bag_path = args.bag_path + topic = args.topic + + storage_options, converter_options = get_rosbag_options(bag_path) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + storage_filter = rosbag2_py.StorageFilter(topics=[topic]) + reader.set_filter(storage_filter) + + print("topic: {}".format(topic)) + cnt = 0 + + while reader.has_next() and cnt <= args.cnt: + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + cnt += 1 + + if args.types_only: + print(msg_type) + continue + + if not hasattr(msg, "header"): + print("{} does not have header".format(msg_type)) + print(msg) + continue + + print("{}.{}: frame_id: {} msg_type: {}".format( + msg.header.stamp.sec, + msg.header.stamp.nanosec, + msg.header.frame_id, + msg_type)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("bag_path") + parser.add_argument("topic") + parser.add_argument("--types-only", action="store_true") + parser.add_argument("--cnt", type=int, default=10) + args = parser.parse_args() + + main(args) diff --git a/src/analysis/topic_traversal.py b/src/analysis/topic_traversal.py new file mode 100755 index 00000000..73ee5104 --- /dev/null +++ b/src/analysis/topic_traversal.py @@ -0,0 +1,100 @@ +#!/usr/bin/env python3 +# traversal topic +# given path: topic1 -> node1 -> topic2 -> node2 -> topic3 -> node3 -> topic4 +# traverse topic2 to topic4, then "topic2 -> node2 -> topic3 -> node3 -> topic4" printed + +import argparse + +import rclpy +from rosgraph2_impl import Edge, Graph + +EXCLUDE_TOPICS = [ + "/parameter_events", + "/rosout", + "/clock", + ] + + +def main(args): + rclpy.init() + node = rclpy.create_node("topic_traversal_node") + graph = Graph(node) + graph.update() + + cnt = 0 + while cnt < 10 and len(graph.nt_edges.edges_by_end) <= len(EXCLUDE_TOPICS): + cnt += 1 + graph = Graph(node) + graph.update() + + if len(graph.nt_edges.edges_by_end) <= len(EXCLUDE_TOPICS): + print("not enough edges") + return + + print("found {} keys".format(len(graph.nt_edges.edges_by_end.keys()))) + for k in graph.nt_edges.edges_by_end.keys(): + print(" '{}'".format(k)) + + def find_key(name): + for key in graph.nt_edges.edges_by_end: + if name in key and 0 <= len(key) - len(name) <= 2: + # print("find_key: {} -> {}".format(name, key)) + return key + return None + + name_from = find_key(args.frm) + name_to = find_key(args.to) + + print("") + print("args: {} -> {}".format(args.frm, args.to)) + print("graph: {} -> {}".format(name_from, name_to)) + print("") + + seen = set() + path = [] + + def traverse(now): + # print("path: {}, traverse {}".format(path, now)) + if now is None: + return False + if now in seen: # basically, assume now is not in seen + return False + + seen.add(now) + path.append(now) + if now == name_from: + return True + + for nxt in graph.nt_edges.edges_by_end[now]: + if isinstance(nxt, Edge): + # print("nxt.start: {}".format(nxt.start)) + nxt = nxt.start + nxt = nxt.strip() + if nxt in EXCLUDE_TOPICS: + continue + # print("nxt: '{}'".format(nxt)) + nxt = find_key(nxt) + if nxt in seen: + continue + # print("nxt: {}".format(nxt)) + if traverse(nxt): + return True + path.pop() + + return False + + if traverse(name_to): + print("Found") + path = path[::-1] + # path = path[::2] + + for i in path: + print(" '{}'".format(i)) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser() + parser.add_argument("frm") + parser.add_argument("to") + args = parser.parse_args() + main(args) diff --git a/src/tilde/CMakeLists.txt b/src/tilde/CMakeLists.txt new file mode 100644 index 00000000..2516a7f8 --- /dev/null +++ b/src/tilde/CMakeLists.txt @@ -0,0 +1,158 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(tilde_cmake REQUIRED) +find_package(tilde_msg REQUIRED) +find_package(rmw REQUIRED) +find_package(LTTngUST REQUIRED) + +tilde_package() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) + ament_lint_auto_find_test_dependencies() + + ament_add_gtest(test_tilde_publisher + test/test_tilde_publisher.cpp) + target_include_directories(test_tilde_publisher + PUBLIC + $ + $) + target_link_libraries(test_tilde_publisher + ${PROJECT_NAME}) + ament_target_dependencies(test_tilde_publisher + "rclcpp" + "tilde_msg") + + ament_add_gtest(test_tilde_node + test/test_tilde_node.cpp) + target_include_directories(test_tilde_node + PUBLIC + $ + $) + target_link_libraries(test_tilde_node + ${PROJECT_NAME}) + ament_target_dependencies(test_tilde_node + "rclcpp" + "tilde_msg" + "sensor_msgs" + "std_msgs") + + ament_add_gtest(test_stamp_processor + test/test_stamp_processor.cpp) + target_include_directories(test_stamp_processor + PUBLIC + $ + $) + target_link_libraries(test_stamp_processor + ${PROJECT_NAME}) + ament_target_dependencies(test_stamp_processor + "rclcpp" + "sensor_msgs" + "std_msgs") + + ament_add_gtest(test_stee_node + test/test_stee_node.cpp) + target_include_directories(test_stee_node + PUBLIC + $ + $) + target_link_libraries(test_stee_node + ${PROJECT_NAME}) + ament_target_dependencies(test_stee_node + "rclcpp" + "tilde_msg" + "sensor_msgs") + + ament_add_gtest(test_stee_source_table + test/test_stee_source_table.cpp) + target_include_directories(test_stee_source_table + PUBLIC + $ + $) + target_link_libraries(test_stee_source_table + ${PROJECT_NAME}) + ament_target_dependencies(test_stee_source_table + "rclcpp" + "tilde_msg" + "sensor_msgs") + +endif() + +include_directories(include) +ament_export_include_directories(include) +install( + DIRECTORY include/ + DESTINATION include) + +add_library(${PROJECT_NAME} SHARED + "src/tilde_node.cpp" + "src/tilde_publisher.cpp" + "src/stee_sources_table.cpp" + "src/stee_node.cpp" + "src/tp.c" +) +target_link_libraries(${PROJECT_NAME} ${LTTNGUST_LIBRARIES}) +ament_target_dependencies(${PROJECT_NAME} + "rclcpp" + "tilde_msg" + LTTngUST +) +target_compile_definitions(${PROJECT_NAME} + PRIVATE "TILDE_BUILDING_LIBRARY") +ament_export_targets(${PROJECT_NAME}) +ament_export_libraries(${PROJECT_NAME}) +ament_export_dependencies(rclcpp tilde_msg) + +add_library(stee_republisher_node SHARED + src/stee_republisher_node_pointcloud2.cpp + src/stee_republisher_node_imu.cpp + src/stee_republisher_node_map.cpp) +ament_target_dependencies(stee_republisher_node + "rclcpp_components") +target_link_libraries(stee_republisher_node + ${PROJECT_NAME}) +rclcpp_components_register_node(stee_republisher_node + PLUGIN "tilde::SteeRepublisherNode" + EXECUTABLE stee_republisher_node_pointcloud2_exe) +rclcpp_components_register_node(stee_republisher_node + PLUGIN "tilde::SteeRepublisherNodeImu" + EXECUTABLE stee_republisher_node_imu_exe) +rclcpp_components_register_node(stee_republisher_node + PLUGIN "tilde::SteeRepublisherNodeMap" + EXECUTABLE stee_republisher_node_map_exe) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +install( + TARGETS stee_republisher_node + stee_republisher_node_pointcloud2_exe + stee_republisher_node_imu_exe + stee_republisher_node_map_exe + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES DESTINATION include +) + +ament_package() diff --git a/src/tilde/README.md b/src/tilde/README.md new file mode 100644 index 00000000..5bc9d981 --- /dev/null +++ b/src/tilde/README.md @@ -0,0 +1,3 @@ +# Tilde + +see [doc](../../doc/README.md) diff --git a/src/tilde/include/tilde/message_conversion.hpp b/src/tilde/include/tilde/message_conversion.hpp new file mode 100644 index 00000000..75b4dabe --- /dev/null +++ b/src/tilde/include/tilde/message_conversion.hpp @@ -0,0 +1,106 @@ +// Copyright 2021 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__MESSAGE_CONVERSION_HPP_ +#define TILDE__MESSAGE_CONVERSION_HPP_ + +#include "tilde/message_conversion_detail.hpp" + +#include + +// sensing_msgs +#include "tilde_msg/msg/stee_imu.hpp" +#include "tilde_msg/msg/stee_point_cloud2.hpp" + +// geometry_msgs +#include "tilde_msg/msg/stee_polygon_stamped.hpp" +#include "tilde_msg/msg/stee_pose_stamped.hpp" +#include "tilde_msg/msg/stee_pose_with_covariance_stamped.hpp" +#include "tilde_msg/msg/stee_twist_stamped.hpp" +#include "tilde_msg/msg/stee_twist_with_covariance_stamped.hpp" + +// nav_msgs +#include "tilde_msg/msg/stee_occupancy_grid.hpp" +#include "tilde_msg/msg/stee_odometry.hpp" + +// autoware_auto_perception_msgs +#include "tilde_msg/msg/stee_detected_objects.hpp" +#include "tilde_msg/msg/stee_predicted_objects.hpp" +#include "tilde_msg/msg/stee_tracked_objects.hpp" +#include "tilde_msg/msg/stee_traffic_light_roi_array.hpp" +#include "tilde_msg/msg/stee_traffic_signal_array.hpp" + +// autoware_auto_planning_msgs +#include "tilde_msg/msg/stee_path.hpp" +#include "tilde_msg/msg/stee_path_with_lane_id.hpp" +#include "tilde_msg/msg/stee_trajectory.hpp" + +// autoware_auto_control_msgs +#include "tilde_msg/msg/stee_ackermann_control_command.hpp" +#include "tilde_msg/msg/stee_ackermann_lateral_command.hpp" +#include "tilde_msg/msg/stee_longitudinal_command.hpp" + +namespace tilde +{ +// define your type +using TypeTable = std::tuple< + Pair, + Pair, + + Pair, + Pair, + Pair< + geometry_msgs::msg::PoseWithCovarianceStamped, tilde_msg::msg::SteePoseWithCovarianceStamped>, + Pair, + Pair< + geometry_msgs::msg::TwistWithCovarianceStamped, tilde_msg::msg::SteeTwistWithCovarianceStamped>, + + Pair, + Pair, + + Pair, + Pair, + Pair, + Pair< + autoware_auto_perception_msgs::msg::TrafficLightRoiArray, + tilde_msg::msg::SteeTrafficLightRoiArray>, + Pair< + autoware_auto_perception_msgs::msg::TrafficSignalArray, tilde_msg::msg::SteeTrafficSignalArray>, + + Pair, + Pair, + Pair, + + Pair< + autoware_auto_control_msgs::msg::AckermannControlCommand, + tilde_msg::msg::SteeAckermannControlCommand>, + Pair< + autoware_auto_control_msgs::msg::AckermannLateralCommand, + tilde_msg::msg::SteeAckermannLateralCommand>, + Pair< + autoware_auto_control_msgs::msg::LongitudinalCommand, + tilde_msg::msg::SteeLongitudinalCommand> >; + +template +struct _ConvertedType +{ + using type = typename Get::type; +}; + +template +using ConvertedMessageType = typename _ConvertedType::type; + +} // namespace tilde + +#endif // TILDE__MESSAGE_CONVERSION_HPP_ diff --git a/src/tilde/include/tilde/message_conversion_detail.hpp b/src/tilde/include/tilde/message_conversion_detail.hpp new file mode 100644 index 00000000..171c6df4 --- /dev/null +++ b/src/tilde/include/tilde/message_conversion_detail.hpp @@ -0,0 +1,50 @@ +// Copyright 2021 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__MESSAGE_CONVERSION_DETAIL_HPP_ +#define TILDE__MESSAGE_CONVERSION_DETAIL_HPP_ + +#include + +namespace tilde +{ + +template +struct Pair +{ + using Key = K; + using Value = V; +}; + +template +struct Get; + +template +struct Get> +{ + using type = typename std::conditional< + std::is_same::value, typename Head::Value, std::nullptr_t>::type; +}; + +template +struct Get> +{ + using type = typename std::conditional< + std::is_same::value, typename Head::Value, + typename Get>::type>::type; +}; + +} // namespace tilde + +#endif // TILDE__MESSAGE_CONVERSION_DETAIL_HPP_ diff --git a/src/tilde/include/tilde/stee_node.hpp b/src/tilde/include/tilde/stee_node.hpp new file mode 100644 index 00000000..28c6b9d1 --- /dev/null +++ b/src/tilde/include/tilde/stee_node.hpp @@ -0,0 +1,256 @@ +// Copyright 2021 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__STEE_NODE_HPP_ +#define TILDE__STEE_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tilde/message_conversion.hpp" +#include "tilde/stee_publisher.hpp" +#include "tilde/stee_sources_table.hpp" +#include "tilde/stee_subscription.hpp" +#include "tilde_msg/msg/stee_source.hpp" + +#include +#include +#include +#include +#include + +namespace tilde +{ +template +inline constexpr bool always_false_v = false; + +class SteeNode : public rclcpp::Node +{ +public: + RCLCPP_SMART_PTR_DEFINITIONS(SteeNode) + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit SteeNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit SteeNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + RCLCPP_PUBLIC + virtual ~SteeNode(); + + template < + typename MessageT, typename ConvertedMessageT = ConvertedMessageType, + typename CallbackT, typename AllocatorT = std::allocator, + typename MessageDeleter = std::default_delete, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename CallbackArgT = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename SubscriptionT = rclcpp::Subscription, + typename ConvertedSubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy, + typename ConvertedMessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy, +#if ROS_DISTRO_GALACTIC + typename SteeSubscriptionT = SteeSubscription< + MessageT, ConvertedMessageT, AllocatorT, MessageMemoryStrategyT, + ConvertedMessageMemoryStrategyT> +#else + typename SteeSubscriptionT = SteeSubscription< + MessageT, ConvertedMessageT, AllocatorT, typename rclcpp::TypeAdapter::custom_type, + typename rclcpp::TypeAdapter::ros_message_type, MessageMemoryStrategyT, + ConvertedMessageMemoryStrategyT> +#endif + > + std::shared_ptr create_stee_subscription( + const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = + rclcpp::SubscriptionOptionsWithAllocator(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strategy = + (MessageMemoryStrategyT::create_default())) + { + std::shared_ptr sub{nullptr}; + std::shared_ptr converted_sub{nullptr}; + auto stee_sub = std::make_shared(); + + if (enable_stee_) { + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(this); + auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); + auto converted_topic_name = resolved_topic_name + "/stee"; + + auto new_callback = [this, resolved_topic_name, + callback](std::unique_ptr converted_msg) -> void { + using ConstRef = const MessageT &; + using UniquePtr = std::unique_ptr; + using SharedConstPtr = std::shared_ptr; + using ConstRefSharedConstPtr = const std::shared_ptr &; + using SharedPtr = std::shared_ptr; + + auto not_stop_topic = stop_topics_.find(resolved_topic_name) == stop_topics_.end(); + + // We added NOLINT because + // google/cpplint cannot handle `if constexpr` well. + // https://github.com/cpplint/cpplint/pull/136 is not applied. + if constexpr (std::is_same_v) { // NOLINT + if (not_stop_topic) { + set_source_table(resolved_topic_name, &converted_msg); + } + callback(converted_msg->body); + } else if constexpr (std::is_same_v) { // NOLINT + if (not_stop_topic) { + set_source_table(resolved_topic_name, converted_msg.get()); + } + auto msg = std::make_unique(std::move(converted_msg->body)); + callback(std::move(msg)); + } else if constexpr (std::is_same_v) { // NOLINT + if (not_stop_topic) { + set_source_table(resolved_topic_name, converted_msg.get()); + } + auto msg = std::make_shared(std::move(converted_msg->body)); + callback(msg); + } else if constexpr (std::is_same_v) { // NOLINT + if (not_stop_topic) { + set_source_table(resolved_topic_name, converted_msg.get()); + } + const auto msg = std::make_shared(std::move(converted_msg->body)); + callback(msg); + } else if constexpr (std::is_same_v) { // NOLINT + if (not_stop_topic) { + set_source_table(resolved_topic_name, converted_msg.get()); + } + auto msg = std::make_shared(std::move(converted_msg->body)); + callback(msg); + } else { + static_assert(always_false_v, "non-exhaustive visitor"); + } + }; + + // TODO(y-okumura-isp): how to prepare converted_msg_mem_strategy + converted_sub = + create_subscription(converted_topic_name, qos, new_callback, options); + + if (converted_sub->get_publisher_count() == 0) { + RCLCPP_WARN(this->get_logger(), "no SteePublisher: '%s'", converted_topic_name.c_str()); + } + + stee_sub->set_converted_sub(converted_sub); + } else { + sub = create_subscription(topic_name, qos, callback, options, msg_mem_strategy); + stee_sub->set_sub(sub); + } + + return stee_sub; + } + + template < + typename MessageT, typename ConvertedMessageT = ConvertedMessageType, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename ConvertedPublisherT = rclcpp::Publisher, + typename SteePublisherT = SteePublisher> + std::shared_ptr create_stee_publisher( + const std::string & topic_name, const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = + rclcpp::PublisherOptionsWithAllocator()) + { + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(this); + auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); + + auto pub = + create_publisher(resolved_topic_name, qos, options); + auto converted_pub = create_publisher( + resolved_topic_name + "/stee", qos, options); + auto stee_pub = std::make_shared( + source_table_, pub, converted_pub, get_fully_qualified_name(), this->get_clock(), + steady_clock_); + return stee_pub; + } + + template < + typename MessageT, typename ConvertedMessageT = ConvertedMessageType, + typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename ConvertedPublisherT = rclcpp::Publisher, + typename SteePublisherT = SteePublisher> + std::shared_ptr create_stee_republisher( + const std::string & topic_name, const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = + rclcpp::PublisherOptionsWithAllocator()) + { + auto converted_pub = create_publisher( + topic_name + "/stee", qos, options); + auto stee_pub = std::make_shared( + source_table_, nullptr, converted_pub, get_fully_qualified_name(), this->get_clock(), + steady_clock_); + return stee_pub; + } + +private: + void init(); + std::shared_ptr steady_clock_; + + std::shared_ptr source_table_; + + /// stop topics for preventing loop topic structure + /** + * set this by ROS2 parameter. + * key: stee_stop_topics + * value: string[] + * + * TODO(y-okumura-isp): read this parameter dynamically. + * We can set it only by startup option for now. + */ + std::set stop_topics_; + + /// Enable STEE or not + /** + * We can set this parameter by "enable_stee" only at initialization. + * This is because if we change "enable/disable" dynamically, + * we need to dynamically change the message type (original type or STEE type). + * We cannot do this without re-create the publisher/subscription pair. + */ + bool enable_stee_{}; + + template > + void set_source_table(const std::string & topic, const ConvertedMessageT * msg) + { + auto stamp = Process::get_timestamp_from_const(&(msg->body)); + + if (!stamp) { + return; + } + + if (msg->sources.size() > 0) { + source_table_->set(topic, *stamp, msg->sources); + } else { + std::vector sources; + tilde_msg::msg::SteeSource source_msg; + source_msg.topic = topic; + source_msg.stamp = *stamp; + source_msg.first_subscription_steady_time = steady_clock_->now(); + sources.emplace_back(std::move(source_msg)); + source_table_->set(topic, *stamp, sources); + } + } +}; + +} // namespace tilde + +#endif // TILDE__STEE_NODE_HPP_ diff --git a/src/tilde/include/tilde/stee_publisher.hpp b/src/tilde/include/tilde/stee_publisher.hpp new file mode 100644 index 00000000..cee533a3 --- /dev/null +++ b/src/tilde/include/tilde/stee_publisher.hpp @@ -0,0 +1,218 @@ +// Copyright 2021 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__STEE_PUBLISHER_HPP_ +#define TILDE__STEE_PUBLISHER_HPP_ + +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher.hpp" +#include "tilde/message_conversion.hpp" +#include "tilde/stee_sources_table.hpp" +#include "tilde/tilde_publisher.hpp" +#include "tilde_msg/msg/stee_source.hpp" + +#include +#include +#include +#include +#include + +namespace tilde +{ +template < + typename MessageT, typename ConvertedMessageT = ConvertedMessageType, + typename AllocatorT = std::allocator> +class SteePublisher +{ +private: + using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + using MessageDeleter = rclcpp::allocator::Deleter; + using PublisherT = rclcpp::Publisher; + using ConvertedPublisherT = rclcpp::Publisher; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(SteePublisher) + + /// Default constructor + SteePublisher( + std::shared_ptr source_table, std::shared_ptr pub, + std::shared_ptr converted_pub, const std::string & node_fqn, + std::shared_ptr clock, std::shared_ptr steady_clock) + : source_table_(source_table), + pub_(pub), + converted_pub_(converted_pub), + node_fqn_(node_fqn), + clock_(clock), + steady_clock_(steady_clock) + { + } + + void publish(std::unique_ptr msg) + { + auto converted_msg = std::make_unique(); + // TODO(y-okumura-isp): Can we avoid copy? + converted_msg->body = *msg; + set_sources(converted_msg.get()); + + converted_pub_->publish(std::move(converted_msg)); + + if (pub_) { + pub_->publish(std::move(msg)); + } + } + + void publish(const MessageT & msg) + { + ConvertedMessageT converted_msg; + // TODO(y-okumura-isp): Can we avoid copy? + converted_msg.body = msg; + set_sources(&converted_msg); + + converted_pub_->publish(converted_msg); + + if (pub_) { + pub_->publish(msg); + } + } + + /** + * publish() variant + * We can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(const rcl_serialized_message_t & serialized_msg) + { + std::cout << "publish serialized message (not supported)" << std::endl; + // publish_info(get_timestamp(clock_->now(), msg.get())); + if (pub_) { + pub_->publish(serialized_msg); + } + } + + /** + * publish() variant + * We can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(const rclcpp::SerializedMessage & serialized_msg) + { + std::cout << "publish SerializedMessage (not supported)" << std::endl; + if (pub_) { + pub_->publish(serialized_msg); + } + } + + /** + * publish() variant + * We can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(rclcpp::LoanedMessage && loaned_msg) + { + std::cout << "publish LoanedMessage (not supported)" << std::endl; + if (pub_) { + pub_->publish(loaned_msg); + } + } + + size_t get_subscription_count() const + { + size_t ret = 0; + if (pub_) { + ret += pub_->get_subscription_count(); + } + + return ret + converted_pub_->get_subscription_count(); + } + + size_t get_intra_process_subscription_count() const + { + size_t ret = 0; + if (pub_) { + ret = pub_->get_intra_process_subscription_count(); + } + return ret + converted_pub_->get_intra_process_subscription_count(); + } + + RCLCPP_PUBLIC + const char * get_topic_name() const + { + if (pub_) { + return pub_->get_topic_name(); + } + + return converted_pub_->get_topic_name(); + } + + /// Explicit API + /** + * \param[in] sub_topic topic FQN + * \param[in] stamp header.stamp of the used message + */ + RCLCPP_PUBLIC + void add_explicit_input_info(const std::string & sub_topic, const rclcpp::Time & stamp) + { + assert(stamp.get_clock_type() == RCL_ROS_TIME); + is_explicit_ = true; + explicit_info_[sub_topic].insert(stamp); + } + +private: + std::shared_ptr source_table_; + std::shared_ptr pub_; + std::shared_ptr converted_pub_; + const std::string node_fqn_; + std::shared_ptr clock_; + std::shared_ptr steady_clock_; + + bool is_explicit_{false}; + // explicit input data + std::map> explicit_info_; + + void set_sources(ConvertedMessageT * converted_msg) + { + std::set found; + + if (!is_explicit_) { + auto topic_sources = source_table_->get_latest_sources(); + for (auto & topic_source : topic_sources) { + for (auto & source : topic_source.second) { + if (found.find(source) != found.end()) { + continue; + } + found.insert(source); + converted_msg->sources.emplace_back(std::move(source)); + } + } + } else { + for (const auto & topic_stamps : explicit_info_) { + const auto & topic = topic_stamps.first; + for (const auto & stamp : topic_stamps.second) { + auto sources = source_table_->get_sources(topic, stamp); + for (auto & source : sources) { + if (found.find(source) != found.end()) { + continue; + } + found.insert(source); + converted_msg->sources.emplace_back(std::move(source)); + } + } + } + explicit_info_.clear(); + } + } +}; + +} // namespace tilde + +#endif // TILDE__STEE_PUBLISHER_HPP_ diff --git a/src/tilde/include/tilde/stee_sources_table.hpp b/src/tilde/include/tilde/stee_sources_table.hpp new file mode 100644 index 00000000..cb806485 --- /dev/null +++ b/src/tilde/include/tilde/stee_sources_table.hpp @@ -0,0 +1,90 @@ +// Copyright 2021 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__STEE_SOURCES_TABLE_HPP_ +#define TILDE__STEE_SOURCES_TABLE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tilde_msg/msg/stee_source.hpp" + +#include +#include +#include +#include + +namespace tilde +{ + +struct SteeSourceCmp +{ + bool operator()( + const tilde_msg::msg::SteeSource & lhs, const tilde_msg::msg::SteeSource & rhs) const; +}; + +class SteeSourcesTable +{ +public: + // resolved topic name + using TopicName = std::string; + // header stamp + using Stamp = rclcpp::Time; + using SourcesMsg = std::vector; + using TopicSources = std::map; + // main data structure + using Sources = std::map>; + // implicit relation + using Latest = std::map; + + /// Constructor + SteeSourcesTable( + size_t default_max_stamps_per_topic, + std::map max_stamps_per_topic = std::map()); + + /// Set input sources. + /** + * \param[in] topic resolved topic name + * \param[in] stamp message stamp + * \@aram[in] sources_msg stee sources in the message + */ + void set(const TopicName & topic, const Stamp & stamp, const SourcesMsg & sources_msg); + + /// Get the latest sources. + /** + * \param[in] topic resolved topic name + * \return empty if no topic + */ + TopicSources get_latest_sources() const; + + /// Get sources of the specific message + /** + * \param[in] topic resolved topic name + * \param[in] stamp message stamp + * \return empty if not found + */ + SourcesMsg get_sources(const TopicName & topic, const Stamp & stamp) const; + +private: + /// default maximum number of stamps to Sources + size_t default_max_stamps_per_topic_; + /// maximum number of stamps to Sources + std::map max_stamps_per_topic_; + /// Sources + Sources sources_; + /// Implicit relation + Latest latest_; +}; + +} // namespace tilde + +#endif // TILDE__STEE_SOURCES_TABLE_HPP_ diff --git a/src/tilde/include/tilde/stee_subscription.hpp b/src/tilde/include/tilde/stee_subscription.hpp new file mode 100644 index 00000000..fbf47a6e --- /dev/null +++ b/src/tilde/include/tilde/stee_subscription.hpp @@ -0,0 +1,100 @@ +// Copyright 2021 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__STEE_SUBSCRIPTION_HPP_ +#define TILDE__STEE_SUBSCRIPTION_HPP_ + +#include "rclcpp/subscription.hpp" +#include "tilde/message_conversion.hpp" + +#include +#include +#include + +namespace tilde +{ + +#if ROS_DISTRO_GALACTIC +template < + typename CallbackMessageT, + typename ConvertedCallbackMessageT = ConvertedMessageType, + typename AllocatorT = std::allocator, + typename MessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy, + typename ConvertedMessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy > +#else +template < + typename MessageT, typename ConvertedMessageT = ConvertedMessageType, + typename AllocatorT = std::allocator, + typename SubscribedT = typename rclcpp::TypeAdapter::custom_type, + typename ROSMessageT = typename rclcpp::TypeAdapter::ros_message_type, + typename MessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy, + typename ConvertedMessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy > +#endif +class SteeSubscription +{ +private: +#if ROS_DISTRO_GALACTIC + using SubscriptionT = rclcpp::Subscription; + using ConvertedSubscriptionT = + rclcpp::Subscription; +#else + using SubscriptionT = + rclcpp::Subscription; + using ConvertedSubscriptionT = rclcpp::Subscription< + ConvertedMessageT, AllocatorT, typename rclcpp::TypeAdapter::custom_type, + typename rclcpp::TypeAdapter::ros_message_type, + ConvertedMessageMemoryStrategyT>; +#endif + +public: + RCLCPP_SMART_PTR_DEFINITIONS(SteeSubscription) + + /// Constructor + /** + * Hold only one of sub or converted_sub. + */ + SteeSubscription() {} + + void set_sub(std::shared_ptr sub) + { + assert(converted_sub_ == nullptr); + sub_ = sub; + topic_fqn_ = sub->get_topic_name(); + } + + void set_converted_sub(std::shared_ptr converted_sub) + { + assert(sub_ == nullptr); + converted_sub_ = converted_sub; + std::string extended_name = converted_sub_->get_topic_name(); + std::string affix = "/stee"; + assert(affix.length() < extended_name.length()); + topic_fqn_ = extended_name.substr(0, extended_name.length() - affix.length()); + } + + [[nodiscard]] RCLCPP_PUBLIC const char * get_topic_name() const { return topic_fqn_.c_str(); } + +private: + std::shared_ptr sub_; + std::shared_ptr converted_sub_; + std::string topic_fqn_; +}; + +} // namespace tilde + +#endif // TILDE__STEE_SUBSCRIPTION_HPP_ diff --git a/src/tilde/include/tilde/tilde_node.hpp b/src/tilde/include/tilde/tilde_node.hpp new file mode 100644 index 00000000..87d72330 --- /dev/null +++ b/src/tilde/include/tilde/tilde_node.hpp @@ -0,0 +1,261 @@ +// Copyright 2021 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__TILDE_NODE_HPP_ +#define TILDE__TILDE_NODE_HPP_ + +#include "rcl_interfaces/msg/set_parameters_result.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/message_info.hpp" +#include "rclcpp/node.hpp" +#include "rclcpp/node_interfaces/get_node_topics_interface.hpp" +#include "rclcpp/visibility_control.hpp" +#include "rmw/types.h" +#include "tilde/tp.h" +#include "tilde_msg/msg/message_tracking_tag.hpp" +#include "tilde_publisher.hpp" + +#include +#include +#include +#include +#include + +#define TILDE_NODE_GET_PTR(MessageT, msg, out) \ + using S = std::decay_t; \ + using ConstRef = const MessageT &; \ + using UniquePtr = std::unique_ptr; \ + using SharedConstPtr = std::shared_ptr; \ + using ConstRefSharedConstPtr = const std::shared_ptr &; \ + using SharedPtr = std::shared_ptr; \ + \ + if constexpr (std::is_same_v) { \ + (out) = &(msg); \ + } else if constexpr (std::is_same_v) { \ + (out) = (msg).get(); \ + } else if constexpr (std::is_same_v) { \ + (out) = (msg).get(); \ + } else if constexpr (std::is_same_v) { \ + (out) = (msg).get(); \ + } else if constexpr (std::is_same_v) { \ + (out) = (msg).get(); \ + } else { \ + static_assert(always_false_v, "non-exhaustive visitor!"); \ + } + +namespace tilde +{ +template +inline constexpr bool always_false_v = false; + +/// Use TildeNode instead of rclcpp::Node and its create_* methods +class TildeNode : public rclcpp::Node +{ + typedef std::map> PublisherMap; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(TildeNode) + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit TildeNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit TildeNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + RCLCPP_PUBLIC + virtual ~TildeNode() = default; + + /// create custom subscription + template < + typename MessageT, typename CallbackT, typename AllocatorT = std::allocator, + typename MessageDeleter = std::default_delete, + typename CallbackMessageT = + typename rclcpp::subscription_traits::has_message_type::type, + typename CallbackArgT = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename SubscriptionT = rclcpp::Subscription, + typename MessageMemoryStrategyT = + rclcpp::message_memory_strategy::MessageMemoryStrategy> + std::shared_ptr create_tilde_subscription( + const std::string & topic_name, const rclcpp::QoS & qos, CallbackT && callback, + const rclcpp::SubscriptionOptionsWithAllocator & options = + rclcpp::SubscriptionOptionsWithAllocator(), + typename MessageMemoryStrategyT::SharedPtr msg_mem_strategy = + (MessageMemoryStrategyT::create_default())) + { + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(this); + auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); + + auto callback_addr = &callback; + + tracepoint( + TRACEPOINT_PROVIDER, tilde_subscription_init, callback_addr, get_fully_qualified_name(), + resolved_topic_name.c_str()); + + auto main_topic_callback = [this, resolved_topic_name, callback, + callback_addr](CallbackArgT msg) -> void { + if (this->enable_tilde_) { + auto subscription_time = this->now(); + auto subscription_time_steady = this->steady_clock_->now(); + + tracepoint( + TRACEPOINT_PROVIDER, tilde_subscribe, callback_addr, + subscription_time_steady.nanoseconds()); + + const MessageT * p_msg; + TILDE_NODE_GET_PTR(MessageT, msg, p_msg); + register_message_as_input( + p_msg, resolved_topic_name, subscription_time, subscription_time_steady); + } + // finally, call original function + callback(std::forward(msg)); + }; + + return create_subscription( + topic_name, qos, main_topic_callback, options, msg_mem_strategy); + } + + template < + typename MessageT, typename AllocatorT = std::allocator, + typename PublisherT = rclcpp::Publisher, + typename TildePublisherT = TildePublisher> + std::shared_ptr create_tilde_publisher( + const std::string & topic_name, const rclcpp::QoS & qos, + const rclcpp::PublisherOptionsWithAllocator & options = + rclcpp::PublisherOptionsWithAllocator()) + { + auto pub = create_publisher(topic_name, qos, options); + auto info_topic = std::string(pub->get_topic_name()) + "/message_tracking_tag"; + auto info_pub = + create_publisher(info_topic, rclcpp::QoS(1), options); + + auto tilde_pub = std::make_shared( + info_pub, pub, get_fully_qualified_name(), this->get_clock(), steady_clock_, + this->enable_tilde_); + tilde_pubs_[info_topic] = tilde_pub; + + tracepoint( + TRACEPOINT_PROVIDER, tilde_publisher_init, tilde_pub.get(), get_fully_qualified_name(), + pub->get_topic_name()); + + return tilde_pub; + } + + /// register message as input + /** + * Register message to the both implicit and explicit data store. + * Canonically, this is called when subscription gets a message. + * + * \param p_msg[in] message raw pointer, can be freed after the function call + * \param resolved_topic_name[in] topic FQN + * \param subscription_time subscription time on ROS_TIME + * \param subscription_time subscription time on steady clock + */ + template > + void register_message_as_input( + const MessageT * p_msg, const std::string & resolved_topic_name, + const rclcpp::Time & subscription_time, const rclcpp::Time & subscription_time_steady) + { + // prepare InputInfo + + auto header_stamp = Process::get_timestamp_from_const(p_msg); + + auto input_info = std::make_shared(); + + input_info->sub_time = subscription_time; + input_info->sub_time_steady = subscription_time_steady; + if (header_stamp) { + input_info->has_header_stamp = true; + input_info->header_stamp = *header_stamp; + } + + // TODO(y-okumura-isp): consider race condition in multi threaded executor. + // i.e. subA comes when subB callback which uses topicA is running + for (auto & [topic, tp] : tilde_pubs_) { + tp->set_implicit_input_info(resolved_topic_name, input_info); + if (input_info->has_header_stamp) { + tp->set_explicit_subscription_time(resolved_topic_name, input_info); + } + } + } + + /// automatically find subscription_time and subscription_time_steady + /** + * Fill subscription_time and subscription_time_steady from internal data. + * This if for TILDE framework internal use, + * so users are expected to use explicit API. + * + * \param p_msg[in] + * \param resolved_topic_name[in] + * \param subscription_time[out] + * \param subscription_time_steady[out] + * \return true if subscription_time found else false + * \sa register_message_as_input + */ + template > + bool find_subscription_time( + const MessageT * p_msg, const std::string & resolved_topic_name, + rclcpp::Time & subscription_time, rclcpp::Time & subscription_time_steady) + { + // get header stamp + auto header_stamp = Process::get_timestamp_from_const(p_msg); + + InputInfo input_info; + + if (header_stamp) { + input_info.has_header_stamp = true; + input_info.header_stamp = *header_stamp; + } + + bool found = false; + if (header_stamp && tilde_pubs_.size() > 0) { + auto pub = tilde_pubs_.begin()->second; + found = pub->get_input_info(resolved_topic_name, *header_stamp, input_info); + } + + if (found) { + subscription_time = input_info.sub_time; + subscription_time_steady = input_info.sub_time_steady; + } + + return found; + } + + rclcpp::Time get_steady_time() { return steady_clock_->now(); } + +private: + /// topic vs publishers + PublisherMap tilde_pubs_; + /// node clock may be simulation time + std::shared_ptr steady_clock_; + + /// whether to enable tilde + bool enable_tilde_; + + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + + void init(); +}; + +} // namespace tilde + +#undef TILDE_NODE_GET_PTR + +#endif // TILDE__TILDE_NODE_HPP_ diff --git a/src/tilde/include/tilde/tilde_publisher.hpp b/src/tilde/include/tilde/tilde_publisher.hpp new file mode 100644 index 00000000..1da633e3 --- /dev/null +++ b/src/tilde/include/tilde/tilde_publisher.hpp @@ -0,0 +1,418 @@ +// Copyright 2021 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__TILDE_PUBLISHER_HPP_ +#define TILDE__TILDE_PUBLISHER_HPP_ + +#include "rclcpp/clock.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/publisher.hpp" +#include "tilde/tp.h" +#include "tilde_msg/msg/message_tracking_tag.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#define TILDE_S_TO_NS(seconds) ((seconds) * (1000LL * 1000LL * 1000LL)) + +namespace tilde +{ + +/// Internal class to hold input message information +class InputInfo +{ +public: + rclcpp::Time sub_time; + rclcpp::Time sub_time_steady; + bool has_header_stamp; + rclcpp::Time header_stamp; + + InputInfo() : has_header_stamp(false) {} + + bool operator==(const InputInfo & rhs) const; +}; + +/// detect header field, not found case +template +struct HasHeader : public std::false_type +{ +}; + +/// detect header field, found case +template +struct HasHeader : std::true_type +{ +}; + +/// has top level stamp +template +struct HasStamp : public std::false_type +{ +}; + +template +struct HasStamp : public std::true_type +{ +}; + +/// has top level stamp and no header +template +struct HasStampWithoutHeader : public std::false_type +{ +}; + +template +struct HasStampWithoutHeader< + M, typename std::enable_if>, HasStamp>>::type> +: public std::true_type +{ +}; + +/// SFINAE to get header.stamp, not found case +template +struct Process +{ + /// stamp getter for non-const pointer without header field + /** + * \param[in] m message + */ + static std::optional get_timestamp(M * m) + { + (void)m; + return {}; + } + + /// stamp getter for const pointer without header field + /** + * Return dummy stamp as M has no header field. + * + * \param[in] m message + */ + static std::optional get_timestamp_from_const(const M * m) + { + (void)m; + return {}; + } +}; + +/// SFINAEEs to get header.stamp, found case +template +struct Process::value>::type> +{ + /// stamp getter for non-const pointer with header field + /** + * Return header.stamp + * + * \param[in] m message + */ + static std::optional get_timestamp(M * m) { return m->header.stamp; } + + /// stamp getter for const pointer with header field + /** + * Return header.stamp + * + * \param[in] m message + */ + static std::optional get_timestamp_from_const(const M * m) + { + return m->header.stamp; + } +}; + +/// SFINAE to get top level stamp, found case +template +struct Process::value>::type> +{ + /// stamp getter for non-const pointer with header field + /** + * Return header.stamp + * + * \param[in] m message + */ + static std::optional get_timestamp(M * m) { return m->stamp; } + + /// stamp getter for const pointer with header field + /** + * Return header.stamp + * + * \param[in] m message + */ + static std::optional get_timestamp_from_const(const M * m) { return m->stamp; } +}; + +template +auto get_timestamp(rclcpp::Time t, T * a) -> decltype(rclcpp::Time(a->header.stamp), t) +{ + // std::cout << "get header timestamp" << std::endl; + rclcpp::Time ret(a->header.stamp); + return ret; +} + +rclcpp::Time get_timestamp(rclcpp::Time t, ...); + +class TildePublisherBase +{ +public: + using InfoMsg = tilde_msg::msg::MessageTrackingTag; + + /// Constructor + /** + * \param[in] clock for RCL_ROS_TIME + * \param[in] clock for RCL_STEADY_TIME + * \param[in] node_fqn a node name + * \param[in] enable enable TILDE or not + */ + explicit TildePublisherBase( + std::shared_ptr clock, std::shared_ptr steady_clock, + std::string node_fqn, bool enable = true); + + /// Set implicit input info + /** + * This is a TILDE internal API for connecting input and output. + * + * \param[in] sub_topic Subscribed topic name + * \param[in] p InputInfo to set + */ + void set_implicit_input_info( + const std::string & sub_topic, const std::shared_ptr p); + + /// Explicit API helper + /** + * It's a TILDE internal API for connecting input and output by holding + * "sub_topic + stamp" vs InputInfo. + * + * \param[in] sub_topic Subscribed topic name + * \param[in] p InputInfo + */ + void set_explicit_subscription_time( + const std::string & sub_topic, const std::shared_ptr p); + + /// Explicit API + /** + * Declare input messages explicitly. + * Specify all messages you use before publishing the message. + * + * TildePublisher gets corresponding InputInfo from topic name + stamp. + * If not found, input_info.header_stamp in MessageTrackingTag is filled + * but sub_time and sub_time_steady are zero cleared. + * + * \param[in] sub_topic used topic + * \param[in] stamp header.stamp of the used message + */ + void add_explicit_input_info(const std::string & sub_topic, const rclcpp::Time & stamp); + + /// Fill input info field of the argument message + /** + * It's a TILDE internal API. + * Fill intput info field according to implicit and explicit info. + * Explicit info is cleared after calling this. + * + * \param[out] info_msg Target message + */ + void fill_input_info(tilde_msg::msg::MessageTrackingTag & info_msg); + + /// Set how long to hold InputInfo + /** + * TildePublisher holds InputInfo of every message just a seconds to + * fill in sub_time and sub_time_steady fields of MessageTrackingTag + * for explicit API. + * You can adjust how many seconds to hold InputInfo. + * The default is 2 seconds. + * + * \param[in] sec how many seconds to hold InputInfo + */ + void set_max_sub_callback_infos_sec(size_t sec); + + /// Get input info + /** + * \param[in] topic fully qualified topic name + * \param[in] header_stamp target header.stamp + * \param[out] info output data + * + * \return true if found else false + */ + bool get_input_info( + const std::string & topic, const rclcpp::Time & header_stamp, InputInfo & info); + + /// Enable or disable TILDE + /** + * \param[in] enable boolean + */ + void set_enable(bool enable); + + void print_input_infos(); + +protected: + std::shared_ptr clock_; + std::shared_ptr steady_clock_; + + const std::string node_fqn_; + int64_t seq_; + + std::map sub_topics_; + + bool enable_; + +private: + // parent node subscription topic vs InputInfo + std::map> input_infos_; + + // explicit InputInfo + // If this is set, FW creates MessageTrackingTag only by this info + bool is_explicit_; + std::map> explicit_input_infos_; + // topic, header stamp vs sub callback time + std::map>> + explicit_sub_time_infos_; + + // how many seconds to preserve explicit_sub_callback_infos per topic + size_t MAX_SUB_CALLBACK_INFOS_SEC_; +}; + +/// rclcpp::Publisher TILDE version +template > +class TildePublisher : public TildePublisherBase +{ +private: + using MessageAllocatorTraits = rclcpp::allocator::AllocRebind; + using MessageAllocator = typename MessageAllocatorTraits::allocator_type; + using MessageDeleter = rclcpp::allocator::Deleter; + using PublisherT = rclcpp::Publisher; + using MessageTrackingTagPublisher = rclcpp::Publisher; + +public: + RCLCPP_SMART_PTR_DEFINITIONS(TildePublisher) + + /// Default constructor + TildePublisher( + std::shared_ptr info_pub, std::shared_ptr pub, + const std::string & node_fqn, const std::shared_ptr & clock, + const std::shared_ptr & steady_clock, bool enable) + : TildePublisherBase(clock, steady_clock, node_fqn, enable), info_pub_(info_pub), pub_(pub) + { + } + + void publish(std::unique_ptr msg) + { + if (enable_) { + auto stamp = Process::get_timestamp(msg.get()); + publish_info(stamp); + } + pub_->publish(std::move(msg)); + } + + void publish(const MessageT & msg) + { + if (enable_) { + auto stamp = Process::get_timestamp_from_const(&msg); + publish_info(stamp); + } + pub_->publish(msg); + } + + /** + * publish() variant + * can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(const rcl_serialized_message_t & serialized_msg) + { + std::cout << "publish serialized message (not supported)" << std::endl; + // publish_info(get_timestamp(clock_->now(), msg.get())); + pub_->publish(serialized_msg); + } + + /** + * publish() variant + * can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(const rclcpp::SerializedMessage & serialized_msg) + { + std::cout << "publish SerializedMessage (not supported)" << std::endl; + pub_->publish(serialized_msg); + } + + /** + * publish() variant + * can send a main message but cannot send the corresponding MessageTrackingTag + */ + void publish(rclcpp::LoanedMessage && loaned_msg) + { + std::cout << "publish LoanedMessage (not supported)" << std::endl; + pub_->publish(loaned_msg); + } + + // TODO(y-okumura-isp) get_allocator + + size_t get_subscription_count() const { return pub_->get_subscription_count(); } + + size_t get_intra_process_subscription_count() const + { + return pub_->get_intra_process_subscription_count(); + } + + RCLCPP_PUBLIC + const char * get_topic_name() const { return pub_->get_topic_name(); } + +private: + std::shared_ptr info_pub_; + std::shared_ptr pub_; + const std::string node_fqn_; + + /// Publish MessageTrackingTag + /** + * \param has_header_stamp whether main message has header.stamp + * \param t header stamp + */ + void publish_info(const std::optional & t) + { + bool has_header_stamp = t ? true : false; + + auto msg = std::make_unique(); + msg->header.stamp = clock_->now(); + // msg->header.frame_id // Nothing todo + + msg->output_info.topic_name = pub_->get_topic_name(); + msg->output_info.node_fqn = node_fqn_; + msg->output_info.seq = seq_; + seq_++; + msg->output_info.pub_time = clock_->now(); + msg->output_info.pub_time_steady = steady_clock_->now(); + msg->output_info.has_header_stamp = has_header_stamp; + if (has_header_stamp) { + msg->output_info.header_stamp = *t; + } + + fill_input_info(*msg); + + for (auto & input_info : msg->input_infos) { + auto pub_time = + TILDE_S_TO_NS(msg->output_info.pub_time.sec) + msg->output_info.pub_time.nanosec; + auto sub_time_steady = + TILDE_S_TO_NS(input_info.sub_time_steady.sec) + input_info.sub_time_steady.nanosec; + auto sub = &sub_topics_[input_info.topic_name]; + tracepoint(TRACEPOINT_PROVIDER, tilde_publish, this, pub_time, sub, sub_time_steady); + } + + info_pub_->publish(std::move(msg)); + } +}; + +} // namespace tilde + +#endif // TILDE__TILDE_PUBLISHER_HPP_ diff --git a/src/tilde/include/tilde/tp.h b/src/tilde/include/tilde/tp.h new file mode 100644 index 00000000..5f0295cf --- /dev/null +++ b/src/tilde/include/tilde/tp.h @@ -0,0 +1,70 @@ +// Copyright 2021 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. + +// Provide fake header guard for cpplint +#undef TILDE__TP_H_ +#ifndef TILDE__TP_H_ +#define TILDE__TP_H_ + +#undef TRACEPOINT_PROVIDER +#define TRACEPOINT_PROVIDER ros2_caret + +#undef TRACEPOINT_INCLUDE +#define TRACEPOINT_INCLUDE "tilde/tp.h" + +#if !defined(_TP_H) || defined(TRACEPOINT_HEADER_MULTI_READ) +#define _TP_H + +#include + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, tilde_subscription_init, + TP_ARGS( + const void *, subscription_arg, const char *, node_name_arg, const char *, topic_name_arg), + TP_FIELDS(ctf_integer_hex(const void *, subscription, subscription_arg) + ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, tilde_subscribe, + TP_ARGS(const void *, subscription_arg, const uint64_t, tilde_message_id_arg), + TP_FIELDS(ctf_integer_hex(const void *, subscription, subscription_arg) + ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg))) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, tilde_publisher_init, + TP_ARGS(const void *, publisher_arg, const char *, node_name_arg, const char *, topic_name_arg), + TP_FIELDS(ctf_integer_hex(const void *, publisher, publisher_arg) + ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, tilde_subscribe_added, + TP_ARGS( + const void *, subscription_id_arg, const char *, node_name_arg, const char *, topic_name_arg), + TP_FIELDS(ctf_integer_hex(const void *, subscription_id, subscription_id_arg) + ctf_string(node_name, node_name_arg) ctf_string(topic_name, topic_name_arg))) + +TRACEPOINT_EVENT( + TRACEPOINT_PROVIDER, tilde_publish, + TP_ARGS( + const void *, publisher_arg, const uint64_t, tilde_publish_timestamp_arg, const void *, + subscription_id_arg, const uint64_t, tilde_message_id_arg), + TP_FIELDS(ctf_integer_hex(const void *, publisher, publisher_arg) + ctf_integer(const uint64_t, tilde_publish_timestamp, tilde_publish_timestamp_arg) + ctf_integer_hex(const void *, subscription_id, subscription_id_arg) + ctf_integer(const uint64_t, tilde_message_id, tilde_message_id_arg))) +#endif /* _TP_H */ + +#include + +#endif // TILDE__TP_H_ diff --git a/src/tilde/package.xml b/src/tilde/package.xml new file mode 100644 index 00000000..c82d02fd --- /dev/null +++ b/src/tilde/package.xml @@ -0,0 +1,26 @@ + + + + tilde + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + liblttng-ust-dev + rclcpp + rclcpp_components + rmw + tilde_cmake + tilde_msg + + ament_lint_auto + caret_lint_common + sensor_msgs + + + ament_cmake + + diff --git a/src/tilde/src/stee_node.cpp b/src/tilde/src/stee_node.cpp new file mode 100644 index 00000000..138901eb --- /dev/null +++ b/src/tilde/src/stee_node.cpp @@ -0,0 +1,54 @@ +// Copyright 2021 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/stee_node.hpp" + +#include + +using tilde::SteeNode; + +SteeNode::SteeNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + init(); +} + +SteeNode::SteeNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options) +: Node(node_name, namespace_, options) +{ + init(); +} + +SteeNode::~SteeNode() {} + +void SteeNode::init() +{ + steady_clock_.reset(new rclcpp::Clock(RCL_STEADY_TIME)); + // TODO(y-okumura-isp): set appropriate max stamps + source_table_.reset(new SteeSourcesTable(100)); + + declare_parameter("enable_stee", true); + get_parameter("enable_stee", enable_stee_); + + auto stop_topics_vec = declare_parameter>( + "stee_stop_topics", + std::vector{ + "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias", + }); + for (const auto & t : stop_topics_vec) { + stop_topics_.insert(t); + } +} diff --git a/src/tilde/src/stee_republisher_node_imu.cpp b/src/tilde/src/stee_republisher_node_imu.cpp new file mode 100644 index 00000000..77a68064 --- /dev/null +++ b/src/tilde/src/stee_republisher_node_imu.cpp @@ -0,0 +1,87 @@ +// Copyright 2021 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/stee_node.hpp" + +using sensor_msgs::msg::Imu; +using tilde_msg::msg::SteeImu; + +namespace tilde +{ + +/// Stee Republisher +// TODO(y-okumura-isp): support other message types than Imu +class SteeRepublisherNodeImu : public SteeNode +{ +public: + explicit SteeRepublisherNodeImu(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode("stee_republisher_node", options) + { + init(); + } + + explicit SteeRepublisherNodeImu( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, options) + { + init(); + } + + explicit SteeRepublisherNodeImu( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, namespace_, options) + { + init(); + } + + virtual ~SteeRepublisherNodeImu() {} + +private: + std::vector original_input_topics_; + std::string original_output_topic_; + + // subscriptions for original input topics + std::vector::SharedPtr> input_stee_subscriptions_; + + // subscription + publisher for republish + rclcpp::Subscription::SharedPtr output_subscription_; + SteePublisher::SharedPtr output_republisher_; + + void init() + { + original_input_topics_ = + declare_parameter>("input_topics", std::vector{}); + original_output_topic_ = declare_parameter("output_topic", ""); + + for (const auto & input_topic : original_input_topics_) { + auto sub = create_stee_subscription( + input_topic, + rclcpp::QoS(1).best_effort(), // TODO(y-okumura-isp): parameterize QoS + [](Imu::UniquePtr msg) { (void)msg; }); + input_stee_subscriptions_.push_back(sub); + } + + output_republisher_ = create_stee_republisher(original_output_topic_, rclcpp::QoS(1)); + output_subscription_ = create_subscription( + original_output_topic_, rclcpp::QoS(1).best_effort(), [this](Imu::UniquePtr msg) { + output_republisher_->publish(std::move(msg)); + }); // TODO(y-okumura-isp): parameterize QoS + } +}; + +} // namespace tilde + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNodeImu) diff --git a/src/tilde/src/stee_republisher_node_map.cpp b/src/tilde/src/stee_republisher_node_map.cpp new file mode 100644 index 00000000..469493d3 --- /dev/null +++ b/src/tilde/src/stee_republisher_node_map.cpp @@ -0,0 +1,86 @@ +// Copyright 2021 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/stee_node.hpp" + +using sensor_msgs::msg::PointCloud2; +using tilde_msg::msg::SteePointCloud2; + +namespace tilde +{ + +/// Stee Republisher +// TODO(y-okumura-isp): support other message types than PointCloud2 +class SteeRepublisherNodeMap : public SteeNode +{ +public: + explicit SteeRepublisherNodeMap(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode("stee_republisher_node", options) + { + init(); + } + + explicit SteeRepublisherNodeMap( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, options) + { + init(); + } + + explicit SteeRepublisherNodeMap( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, namespace_, options) + { + init(); + } + + virtual ~SteeRepublisherNodeMap() {} + +private: + std::vector original_input_topics_; + std::string original_output_topic_; + + // subscriptions for original input topics + std::vector::SharedPtr> input_stee_subscriptions_; + + // subscription + publisher for republish + rclcpp::Subscription::SharedPtr output_subscription_; + SteePublisher::SharedPtr output_republisher_; + + void init() + { + original_input_topics_ = + declare_parameter>("input_topics", std::vector{}); + original_output_topic_ = declare_parameter("output_topic", ""); + + for (const auto & input_topic : original_input_topics_) { + auto sub = create_stee_subscription( + input_topic, rclcpp::QoS(1), [](PointCloud2::UniquePtr msg) { (void)msg; }); + input_stee_subscriptions_.push_back(sub); + } + + output_republisher_ = create_stee_republisher( + original_output_topic_, rclcpp::QoS(1).transient_local()); + output_subscription_ = create_subscription( + original_output_topic_, rclcpp::QoS(1).transient_local(), [this](PointCloud2::UniquePtr msg) { + output_republisher_->publish(std::move(msg)); + }); // TODO(y-okumura-isp): parameterize QoS + } +}; + +} // namespace tilde + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNodeMap) diff --git a/src/tilde/src/stee_republisher_node_pointcloud2.cpp b/src/tilde/src/stee_republisher_node_pointcloud2.cpp new file mode 100644 index 00000000..466b3c5f --- /dev/null +++ b/src/tilde/src/stee_republisher_node_pointcloud2.cpp @@ -0,0 +1,88 @@ +// Copyright 2021 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/stee_node.hpp" + +using sensor_msgs::msg::PointCloud2; +using tilde_msg::msg::SteePointCloud2; + +namespace tilde +{ + +/// Stee Republisher +// TODO(y-okumura-isp): support other message types than PointCloud2 +class SteeRepublisherNode : public SteeNode +{ +public: + explicit SteeRepublisherNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode("stee_republisher_node", options) + { + init(); + } + + explicit SteeRepublisherNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, options) + { + init(); + } + + explicit SteeRepublisherNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()) + : SteeNode(node_name, namespace_, options) + { + init(); + } + + virtual ~SteeRepublisherNode() {} + +private: + std::vector original_input_topics_; + std::string original_output_topic_; + + // subscriptions for original input topics + std::vector::SharedPtr> point_cloud2_stee_subscriptions_; + + // subscription + publisher for republish + rclcpp::Subscription::SharedPtr point_cloud2_subscription_; + SteePublisher::SharedPtr point_cloud2_republisher_; + + void init() + { + original_input_topics_ = + declare_parameter>("input_topics", std::vector{}); + original_output_topic_ = declare_parameter("output_topic", ""); + + for (const auto & input_topic : original_input_topics_) { + auto sub = create_stee_subscription( + input_topic, + rclcpp::QoS(1).best_effort(), // TODO(y-okumura-isp): parameterize QoS + [](PointCloud2::UniquePtr msg) { (void)msg; }); + point_cloud2_stee_subscriptions_.push_back(sub); + } + + point_cloud2_republisher_ = + create_stee_republisher(original_output_topic_, rclcpp::QoS(1).best_effort()); + point_cloud2_subscription_ = create_subscription( + original_output_topic_, rclcpp::QoS(1).best_effort(), [this](PointCloud2::UniquePtr msg) { + point_cloud2_republisher_->publish(std::move(msg)); + }); // TODO(y-okumura-isp): parameterize QoS + } +}; + +} // namespace tilde + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(tilde::SteeRepublisherNode) diff --git a/src/tilde/src/stee_sources_table.cpp b/src/tilde/src/stee_sources_table.cpp new file mode 100644 index 00000000..e3863b69 --- /dev/null +++ b/src/tilde/src/stee_sources_table.cpp @@ -0,0 +1,111 @@ +// Copyright 2021 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/stee_sources_table.hpp" + +#include +#include +#include + +using tilde::SteeSourceCmp; +using tilde::SteeSourcesTable; + +SteeSourcesTable::SteeSourcesTable( + size_t default_max_stamps_per_topic, + std::map max_stamps_per_topic) +: default_max_stamps_per_topic_(default_max_stamps_per_topic), + max_stamps_per_topic_(std::move(max_stamps_per_topic)) +{ +} + +bool SteeSourceCmp::operator()( + const tilde_msg::msg::SteeSource & lhs, const tilde_msg::msg::SteeSource & rhs) const +{ + return ( + lhs.topic < rhs.topic || lhs.stamp.sec < rhs.stamp.sec || + lhs.stamp.nanosec < rhs.stamp.nanosec || + lhs.first_subscription_steady_time.sec < rhs.first_subscription_steady_time.sec || + lhs.first_subscription_steady_time.nanosec < rhs.first_subscription_steady_time.nanosec); +} + +void SteeSourcesTable::set( + const SteeSourcesTable::TopicName & topic, const SteeSourcesTable::Stamp & stamp, + const SteeSourcesTable::SourcesMsg & sources_msg) +{ + assert(stamp.get_clock_type() == RCL_ROS_TIME); + + std::set found; + for (const auto & source : sources_msg) { + if (found.find(source) != found.end()) { + continue; + } + found.insert(source); + sources_[topic][stamp].push_back(source); + } + latest_[topic] = stamp; + + auto max_stamps = default_max_stamps_per_topic_; + auto max_it = max_stamps_per_topic_.find(topic); + if (max_it != max_stamps_per_topic_.end()) { + max_stamps = max_it->second; + } + + auto & stamp_vs_sources = sources_[topic]; + while (stamp_vs_sources.size() > max_stamps) { + stamp_vs_sources.erase(stamp_vs_sources.begin()); + } +} + +SteeSourcesTable::TopicSources SteeSourcesTable::get_latest_sources() const +{ + SteeSourcesTable::TopicSources ret; + + for (const auto & latest_it : latest_) { + const auto & topic = latest_it.first; + const auto & stamp = latest_it.second; + + auto sources_it = sources_.find(topic); + if (sources_it == sources_.end()) { + continue; + } + + const auto & stamp_sources = sources_it->second; + auto stamp_sources_it = stamp_sources.find(stamp); + if (stamp_sources_it != stamp_sources.end()) { + const auto & sources = stamp_sources_it->second; + ret[topic] = sources; + } + } + + return ret; +} + +SteeSourcesTable::SourcesMsg SteeSourcesTable::get_sources( + const SteeSourcesTable::TopicName & topic, const SteeSourcesTable::Stamp & stamp) const +{ + assert(stamp.get_clock_type() == RCL_ROS_TIME); + SteeSourcesTable::SourcesMsg empty; + + auto it = sources_.find(topic); + if (it == sources_.end()) { + return empty; + } + + auto it_stamp_vs_sources = it->second.find(stamp); + if (it_stamp_vs_sources == it->second.end()) { + return empty; + } + + return it_stamp_vs_sources->second; +} diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp new file mode 100644 index 00000000..a14de72d --- /dev/null +++ b/src/tilde/src/tilde_node.cpp @@ -0,0 +1,61 @@ +// Copyright 2021 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/tilde_node.hpp" + +#include +#include + +using tilde::TildeNode; + +TildeNode::TildeNode(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + init(); +} + +TildeNode::TildeNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options) +: Node(node_name, namespace_, options) +{ + init(); +} + +void TildeNode::init() +{ + steady_clock_.reset(new rclcpp::Clock(RCL_STEADY_TIME)); + 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) { + auto result = rcl_interfaces::msg::SetParametersResult(); + + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == "enable_tilde") { + enable_tilde_ = parameter.as_bool(); + for (auto & [topic, pub] : tilde_pubs_) { + (void)topic; + pub->set_enable(enable_tilde_); + } + } + } + + return result; + }); +} diff --git a/src/tilde/src/tilde_publisher.cpp b/src/tilde/src/tilde_publisher.cpp new file mode 100644 index 00000000..9c7fcb47 --- /dev/null +++ b/src/tilde/src/tilde_publisher.cpp @@ -0,0 +1,179 @@ +// Copyright 2021 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/tilde_publisher.hpp" + +#include "tilde_msg/msg/sub_topic_time_info.hpp" + +#include +#include + +using tilde::InputInfo; +using tilde::TildePublisherBase; + +bool InputInfo::operator==(const InputInfo & rhs) const +{ + return sub_time == rhs.sub_time && sub_time_steady == rhs.sub_time_steady && + has_header_stamp == rhs.has_header_stamp && header_stamp == rhs.header_stamp; +} + +rclcpp::Time tilde::get_timestamp(rclcpp::Time t, ...) +{ + std::cout << "get rclcpp::Time t" << std::endl; + return t; +} + +TildePublisherBase::TildePublisherBase( + std::shared_ptr clock, std::shared_ptr steady_clock, + std::string node_fqn, bool enable) +: clock_(std::move(clock)), + steady_clock_(std::move(steady_clock)), + node_fqn_(std::move(node_fqn)), + seq_(0), + enable_(enable), + is_explicit_(false), + MAX_SUB_CALLBACK_INFOS_SEC_(2) +{ +} + +void TildePublisherBase::set_implicit_input_info( + const std::string & sub_topic, const std::shared_ptr p) +{ + input_infos_[sub_topic] = p; + if (sub_topics_.count(sub_topic) == 0) { + sub_topics_[sub_topic] = sub_topic; + tracepoint( + TRACEPOINT_PROVIDER, tilde_subscribe_added, &sub_topics_[sub_topic], node_fqn_.c_str(), + sub_topic.c_str()); + } +} + +void TildePublisherBase::add_explicit_input_info( + const std::string & sub_topic, const rclcpp::Time & stamp) +{ + InputInfo info; + + if (!is_explicit_) { + is_explicit_ = true; + } + + // find sub callback time info and fill info + auto it = explicit_sub_time_infos_.find(sub_topic); + if (it != explicit_sub_time_infos_.end() && it->second.find(stamp) != it->second.end()) { + info.sub_time = it->second[stamp]->sub_time; + info.sub_time_steady = it->second[stamp]->sub_time_steady; + } + + info.has_header_stamp = true; + info.header_stamp = stamp; + explicit_input_infos_[sub_topic].push_back(info); + + if (sub_topics_.count(sub_topic) == 0) { + sub_topics_[sub_topic] = sub_topic; + tracepoint( + TRACEPOINT_PROVIDER, tilde_subscribe_added, &sub_topics_[sub_topic], node_fqn_.c_str(), + sub_topic.c_str()); + } +} + +void TildePublisherBase::fill_input_info(tilde_msg::msg::MessageTrackingTag & info_msg) +{ + info_msg.input_infos.clear(); + + if (!is_explicit_) { + info_msg.input_infos.resize(input_infos_.size()); + + size_t i = 0; + for (const auto & [topic, input_info] : input_infos_) { + info_msg.input_infos[i].topic_name = topic; + info_msg.input_infos[i].sub_time = input_info->sub_time; + info_msg.input_infos[i].sub_time_steady = input_info->sub_time_steady; + info_msg.input_infos[i].has_header_stamp = input_info->has_header_stamp; + info_msg.input_infos[i].header_stamp = input_info->header_stamp; + i++; + } + } else { + for (const auto & [topic, input_infos] : explicit_input_infos_) { + for (const auto & input_info : input_infos) { + tilde_msg::msg::SubTopicTimeInfo info; + info.topic_name = topic; + info.sub_time = input_info.sub_time; + info.sub_time_steady = input_info.sub_time_steady; + info.has_header_stamp = input_info.has_header_stamp; + info.header_stamp = input_info.header_stamp; + info_msg.input_infos.push_back(info); + } + } + explicit_input_infos_.clear(); + } +} + +void TildePublisherBase::set_explicit_subscription_time( + const std::string & sub_topic, const std::shared_ptr p) +{ + auto & header_stamp2sub_time = explicit_sub_time_infos_[sub_topic]; + header_stamp2sub_time[p->header_stamp] = p; + + // cleanup old infos + rclcpp::Duration dur(MAX_SUB_CALLBACK_INFOS_SEC_, 0); + auto thres = clock_->now() - dur; + + for (auto it = header_stamp2sub_time.begin(); it != header_stamp2sub_time.end();) { + if (it->first < thres) { + it = header_stamp2sub_time.erase(it); + } else { + break; + } + } +} + +void TildePublisherBase::set_max_sub_callback_infos_sec(size_t sec) +{ + MAX_SUB_CALLBACK_INFOS_SEC_ = sec; +} + +bool TildePublisherBase::get_input_info( + const std::string & topic, const rclcpp::Time & header_stamp, InputInfo & info) +{ + auto hs2ii_it = explicit_sub_time_infos_.find(topic); + if (hs2ii_it == explicit_sub_time_infos_.end()) { + return false; + } + + auto header_stamp2input_info = hs2ii_it->second; + auto it = header_stamp2input_info.find(header_stamp); + if (it == header_stamp2input_info.end()) { + return false; + } + + info = *(it->second); + return true; +} + +void TildePublisherBase::set_enable(bool enable) { enable_ = enable; } + +void TildePublisherBase::print_input_infos() +{ + auto time2str = [](const rclcpp::Time & rt) -> std::string { + builtin_interfaces::msg::Time t = rt; + return std::string("sec: ") + std::to_string(t.sec) + " nsec: " + std::to_string(t.nanosec); + }; + + std::cout << "print_input_infos\n"; + for (auto & [topic, pinfo] : input_infos_) { + std::cout << " " << topic << "\n"; + std::cout << " sub_time: " << time2str(pinfo->sub_time) + << "\n stamp: " << time2str(pinfo->header_stamp) << std::endl; + } +} diff --git a/src/tilde/src/tp.c b/src/tilde/src/tp.c new file mode 100644 index 00000000..d7d3e7b1 --- /dev/null +++ b/src/tilde/src/tp.c @@ -0,0 +1,19 @@ +// Copyright 2021 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. + +#define TRACEPOINT_CREATE_PROBES + +#define TRACEPOINT_DEFINE + +#include "tilde/tp.h" diff --git a/src/tilde/test/test_stamp_processor.cpp b/src/tilde/test/test_stamp_processor.cpp new file mode 100644 index 00000000..a70c15e8 --- /dev/null +++ b/src/tilde/test/test_stamp_processor.cpp @@ -0,0 +1,153 @@ +// Copyright 2021 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 "builtin_interfaces/msg/time.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include + +#include +#include +#include + +using sensor_msgs::msg::PointCloud2; +using std_msgs::msg::String; +using tilde::Process; + +class TestStampProcessor : public ::testing::Test +{ +}; + +struct MsgWithTopLevelStamp +{ + builtin_interfaces::msg::Time stamp; +}; + +struct MsgWithHeaderAndTopLevelStamp +{ + std_msgs::msg::Header header; + builtin_interfaces::msg::Time stamp; +}; + +TEST_F(TestStampProcessor, pointer_with_header_stamp) +{ + PointCloud2 msg; + + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + msg.header.stamp = expect; + auto stamp = Process::get_timestamp(&msg); + + EXPECT_TRUE(stamp ? true : false); + EXPECT_EQ(*stamp, expect); +} + +TEST_F(TestStampProcessor, pointer_without_header_stamp) +{ + String msg; + auto stamp = Process::get_timestamp(&msg); + + EXPECT_FALSE((stamp ? true : false)); +} + +TEST_F(TestStampProcessor, const_pointer_with_header_stamp) +{ + PointCloud2 _msg; + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + _msg.header.stamp = expect; + + const PointCloud2 msg(_msg); + auto stamp = Process::get_timestamp_from_const(&msg); + + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); +} + +TEST_F(TestStampProcessor, const_pointer_without_header_stamp) +{ + const String msg; + auto stamp = Process::get_timestamp_from_const(&msg); + + EXPECT_FALSE((stamp ? true : false)); +} + +TEST_F(TestStampProcessor, pointer_with_top_level_stamp) +{ + MsgWithTopLevelStamp msg; + + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + msg.stamp = expect; + auto stamp = Process::get_timestamp(&msg); + + EXPECT_FALSE(tilde::HasHeader::value); + EXPECT_TRUE(tilde::HasStamp::value); + EXPECT_TRUE(tilde::HasStampWithoutHeader::value); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); +} + +TEST_F(TestStampProcessor, const_pointer_with_top_level_stamp) +{ + MsgWithTopLevelStamp _msg; + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + _msg.stamp = expect; + + const MsgWithTopLevelStamp msg(_msg); + auto stamp = Process::get_timestamp_from_const(&msg); + + EXPECT_FALSE(tilde::HasHeader::value); + EXPECT_TRUE(tilde::HasStamp::value); + EXPECT_TRUE(tilde::HasStampWithoutHeader::value); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); +} + +TEST_F(TestStampProcessor, pointer_with_header_and_top_level_stamp) +{ + MsgWithHeaderAndTopLevelStamp msg; + + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + rclcpp::Time unexpected(3, 4, RCL_ROS_TIME); + msg.header.stamp = expect; + msg.stamp = unexpected; + auto stamp = Process::get_timestamp(&msg); + + EXPECT_TRUE(tilde::HasHeader::value); + EXPECT_TRUE(tilde::HasStamp::value); + EXPECT_FALSE(tilde::HasStampWithoutHeader::value); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); +} + +TEST_F(TestStampProcessor, const_pointer_with_header_and_top_level_stamp) +{ + MsgWithHeaderAndTopLevelStamp _msg; + + rclcpp::Time expect(1, 2, RCL_ROS_TIME); + rclcpp::Time unexpected(3, 4, RCL_ROS_TIME); + _msg.header.stamp = expect; + _msg.stamp = unexpected; + + const MsgWithHeaderAndTopLevelStamp msg(_msg); + auto stamp = Process::get_timestamp_from_const(&msg); + + EXPECT_TRUE(tilde::HasHeader::value); + EXPECT_TRUE(tilde::HasStamp::value); + EXPECT_FALSE(tilde::HasStampWithoutHeader::value); + EXPECT_TRUE((stamp ? true : false)); + EXPECT_EQ(*stamp, expect); +} diff --git a/src/tilde/test/test_stee_node.cpp b/src/tilde/test/test_stee_node.cpp new file mode 100644 index 00000000..6edaeb71 --- /dev/null +++ b/src/tilde/test/test_stee_node.cpp @@ -0,0 +1,655 @@ +// Copyright 2021 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/stee_node.hpp" +#include "tilde_msg/msg/stee_point_cloud2.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include + +#include +#include +#include + +using sensor_msgs::msg::PointCloud2; +using tilde::SteeNode; +using tilde_msg::msg::SteePointCloud2; + +class TestSteeNode : public ::testing::Test +{ +public: + void SetUp() override { rclcpp::init(0, nullptr); } + + void TearDown() override { rclcpp::shutdown(); } +}; + +rclcpp::Time get_time(int32_t seconds, uint32_t nanoseconds) +{ + // Use RCL_ROS_TIME because rclcpp::Time(builtin_interfaces::msg::Time) uses + // RCL_ROS_TIME. + return rclcpp::Time(seconds, nanoseconds, RCL_ROS_TIME); +} + +TEST_F(TestSteeNode, stee_publisher_unique_ptr) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto main_node = std::make_shared("stee_node", options); + auto stee_pub = main_node->create_stee_publisher("topic", 1); + + auto checker_node = std::make_shared("checker_node"); + bool received_main = false; + auto main_sub = checker_node->create_subscription( + "topic", 1, [&received_main](PointCloud2::UniquePtr msg) -> void { + EXPECT_EQ(msg->header.frame_id, "unique"); + received_main = true; + }); + bool received_converted = false; + auto converted_sub = checker_node->create_subscription( + "topic/stee", 1, [&received_converted](SteePointCloud2::UniquePtr msg) -> void { + EXPECT_EQ(msg->body.header.frame_id, "unique"); + received_converted = true; + }); + + auto unique_ptr_msg = std::make_unique(); + unique_ptr_msg->header.frame_id = "unique"; + + stee_pub->publish(std::move(unique_ptr_msg)); + + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(received_main); + EXPECT_TRUE(received_converted); +} + +TEST_F(TestSteeNode, stee_publisher_const_reference) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto main_node = std::make_shared("stee_node", options); + auto stee_pub = main_node->create_stee_publisher("topic", 1); + + auto checker_node = std::make_shared("checker_node"); + bool received_main = false; + auto main_sub = checker_node->create_subscription( + "topic", 1, [&received_main](const PointCloud2 & msg) -> void { + EXPECT_EQ(msg.header.frame_id, "unique"); + received_main = true; + }); + bool received_converted = false; + auto converted_sub = checker_node->create_subscription( + "topic/stee", 1, [&received_converted](const SteePointCloud2 & msg) -> void { + EXPECT_EQ(msg.body.header.frame_id, "unique"); + received_converted = true; + }); + + PointCloud2 msg; + msg.header.frame_id = "unique"; + + stee_pub->publish(msg); + + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(received_main); + EXPECT_TRUE(received_converted); +} + +TEST_F(TestSteeNode, stee_subscription_unique_ptr) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto sensor_node = std::make_shared("sensor_node", options); + auto checker_node = std::make_shared("checker_node", options); + auto checker_node2 = std::make_shared("checker_node2", options); + const std::string test_string = "recv_unique_ptr"; + + auto pub = sensor_node->create_stee_publisher("topic", 1); + + bool received = false; + // simulate use defined callback + auto sub = checker_node->create_stee_subscription( + "topic", 1, [test_string, &received](PointCloud2::UniquePtr msg) -> void { + received = true; + EXPECT_EQ(msg->header.frame_id, test_string); + }); + + bool received2 = false; + auto sub2 = checker_node2->create_subscription( + "topic/stee", 1, [test_string, &received2](SteePointCloud2::UniquePtr msg) -> void { + received2 = true; + EXPECT_EQ(msg->body.header.frame_id, test_string); + }); + + PointCloud2 msg; + msg.header.frame_id = test_string; + + pub->publish(msg); + rclcpp::spin_some(checker_node); + rclcpp::spin_some(checker_node2); + + EXPECT_TRUE(received); + EXPECT_TRUE(received2); +} + +TEST_F(TestSteeNode, stee_subscription_shared_const) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto sensor_node = std::make_shared("sensor_node", options); + auto checker_node = std::make_shared("checker_node", options); + auto checker_node2 = std::make_shared("checker_node2", options); + const std::string test_string = "recv_shared_const"; + + auto pub = sensor_node->create_stee_publisher("topic", 1); + + bool received = false; + // simulate use defined callback + auto sub = checker_node->create_stee_subscription( + "topic", 1, [test_string, &received](std::shared_ptr msg) -> void { + received = true; + EXPECT_EQ(msg->header.frame_id, test_string); + }); + + bool received2 = false; + auto sub2 = checker_node2->create_subscription( + "topic/stee", 1, [test_string, &received2](SteePointCloud2::UniquePtr msg) -> void { + received2 = true; + EXPECT_EQ(msg->body.header.frame_id, test_string); + }); + + PointCloud2 msg; + msg.header.frame_id = test_string; + + pub->publish(msg); + rclcpp::spin_some(checker_node); + rclcpp::spin_some(checker_node2); + + EXPECT_TRUE(received); + EXPECT_TRUE(received2); +} + +TEST_F(TestSteeNode, one_sub_msg_one_pub) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + auto checker_node = std::make_shared("checker_node", options); + + auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); + + auto main_pub = main_node->create_stee_publisher("main", 1); + bool main_received = false; + // simulate use defined callback + auto main_sub = main_node->create_stee_subscription( + "sensor", 1, [&main_received](std::shared_ptr msg) -> void { + main_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor"); + }); + + bool checker_received = false; + auto checker_sub = checker_node->create_subscription( + "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 1u); + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 100lu); + }); + + // publish sensor + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(main_received); + EXPECT_FALSE(checker_received); + + // publish main + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_received); +} + +TEST_F(TestSteeNode, two_sub_msg_select_latest) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + auto checker_node = std::make_shared("checker_node", options); + + auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); + + auto main_pub = main_node->create_stee_publisher("main", 1); + bool main_received = false; + // simulate use defined callback + auto main_sub = main_node->create_stee_subscription( + "sensor", 1, [&main_received](std::shared_ptr msg) -> void { + main_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor"); + }); + + bool checker_received = false; + auto checker_sub = checker_node->create_subscription( + "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 1u); + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 200lu); + }); + + // publish sensor (old value) + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(main_received); + EXPECT_FALSE(checker_received); + + // publish sensor (new value) + main_received = false; + + msg.header.stamp = get_time(1, 200); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(main_received); + EXPECT_FALSE(checker_received); + + // publish main + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_received); +} + +TEST_F(TestSteeNode, two_sub_msg_explicit) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + auto checker_node = std::make_shared("checker_node", options); + + auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); + + auto main_pub = main_node->create_stee_publisher("main", 1); + bool main_received = false; + // simulate use defined callback + auto main_sub = main_node->create_stee_subscription( + "sensor", 1, [&main_received](std::shared_ptr msg) -> void { + main_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor"); + }); + + bool checker_received = false; + auto checker_sub = checker_node->create_subscription( + "main/stee", 1, [&checker_received](std::shared_ptr msg) -> void { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 1u); + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 100lu); + }); + + // publish sensor (old value) + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(main_received); + EXPECT_FALSE(checker_received); + + // publish sensor (new value) + main_received = false; + + msg.header.stamp = get_time(1, 200); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(main_received); + EXPECT_FALSE(checker_received); + + // publish main with explicit API + main_pub->add_explicit_input_info("/sensor", get_time(1, 100)); + + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_received); +} + +// https://github.com/tier4/TILDE/issues/61 +TEST_F(TestSteeNode, test_non_fqn_topic_explicit_after_publish) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + auto checker_node = std::make_shared("checker_node", options); + + auto sensor_pub = sensor_node->create_stee_publisher("sensor", 1); + + auto main_pub = main_node->create_stee_publisher("main", 1); + bool main_received = false; + // simulate use defined callback + auto main_sub = main_node->create_stee_subscription( + "sensor", 1, [&main_received](std::shared_ptr msg) -> void { + main_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor"); + }); + + size_t num_sub = 0; + bool checker_received = false; + auto checker_sub = checker_node->create_subscription( + "main/stee", 1, + [&num_sub, &checker_received](std::shared_ptr msg) -> void { + if (num_sub == 0) { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 1u); + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 100lu); + ++num_sub; + } else if (num_sub == 1) { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 1u); + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor"); + EXPECT_EQ(source.stamp.sec, 2); + EXPECT_EQ(source.stamp.nanosec, 200lu); + ++num_sub; + } + }); + + // publish sensor1 + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + main_pub->add_explicit_input_info(main_sub->get_topic_name(), get_time(1, 100)); + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(main_received); + EXPECT_TRUE(checker_received); + EXPECT_EQ(num_sub, 1u); + + // publish sensor2 + main_received = false; + checker_received = false; + + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_sensor"; + + sensor_pub->publish(msg); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + main_pub->add_explicit_input_info(main_sub->get_topic_name(), get_time(2, 200)); + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(main_received); + EXPECT_TRUE(checker_received); + EXPECT_EQ(num_sub, 2u); +} + +TEST_F(TestSteeNode, two_topics_one_pub) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + auto checker_node = std::make_shared("checker_node2", options); + const std::string test_string = "recv_shared_const"; + + auto sensor1_pub = sensor_node->create_stee_publisher("sensor1", 1); + auto sensor2_pub = sensor_node->create_stee_publisher("sensor2", 1); + + auto main_pub = main_node->create_stee_publisher("main", 1); + + // sensor subscription + bool sensor1_received = false; + auto sensor1_sub = main_node->create_stee_subscription( + "sensor1", 1, [test_string, &sensor1_received](std::shared_ptr msg) -> void { + sensor1_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor1"); + }); + bool sensor2_received = false; + auto sensor2_sub = main_node->create_stee_subscription( + "sensor2", 1, [test_string, &sensor2_received](std::shared_ptr msg) -> void { + sensor2_received = true; + EXPECT_EQ(msg->header.frame_id, "by_sensor2"); + }); + + // checker subscription + bool checker_received = false; + auto checker_sub = checker_node->create_subscription( + "main/stee", 1, + [test_string, &checker_received](std::shared_ptr msg) -> void { + checker_received = true; + EXPECT_EQ(msg->body.header.frame_id, "by_main"); + EXPECT_EQ(msg->sources.size(), 2u); + { + const auto & source = msg->sources[0]; + EXPECT_EQ(source.topic, "/sensor1"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 100lu); + } + { + const auto & source = msg->sources[1]; + EXPECT_EQ(source.topic, "/sensor2"); + EXPECT_EQ(source.stamp.sec, 1); + EXPECT_EQ(source.stamp.nanosec, 200lu); + } + }); + + // publish sensor1 + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor1"; + + sensor1_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(sensor1_received); + EXPECT_FALSE(sensor2_received); + EXPECT_FALSE(checker_received); + + // publish sensor2p + msg.header.stamp = get_time(1, 200); + msg.header.frame_id = "by_sensor2"; + + sensor2_pub->publish(msg); + rclcpp::spin_some(main_node); + + EXPECT_TRUE(sensor1_received); + EXPECT_TRUE(sensor2_received); + EXPECT_FALSE(checker_received); + + // publish main + msg.header.stamp = get_time(2, 200); + msg.header.frame_id = "by_main"; + main_pub->publish(msg); + + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_received); +} + +TEST_F(TestSteeNode, param_enable_stee) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + options.append_parameter_override("enable_stee", false); + + // if not enable_stee, we can communicate non-stee publisher and + // stee subscription + auto sensor_node = std::make_shared("sensor_node", options); + auto main_node = std::make_shared("main_node", options); + + auto non_stee_pub = sensor_node->create_publisher("topic", 1); + + bool got_message = false; + auto main_stee_sub = main_node->create_stee_subscription( + "topic", 1, [&got_message](std::shared_ptr msg) -> void { + (void)msg; + got_message = true; + }); + + // publish sensor + PointCloud2 msg; + msg.header.stamp = get_time(1, 100); + msg.header.frame_id = "by_sensor1"; + non_stee_pub->publish(msg); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + EXPECT_TRUE(got_message); +} + +TEST_F(TestSteeNode, remove_duplicated_sources) +{ + /* DAG is + * A --> B --> C --> + * | ^ + * +-----------+ + * + * Check C publishes unique A entry although the same message of A is used by B, and C. + * Originally, A was listed twice. + */ + + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto node_a = std::make_shared("nodeA", options); + auto node_b = std::make_shared("nodeB", options); + auto node_c = std::make_shared("nodeC", options); + auto checker_node = std::make_shared("checker_node", options); + + // nodeA directly publish SteePointCloud2 to fill sources field manually + auto pub_a = node_a->create_publisher("a/stee", 1); + + // subscriptions of A + bool get_b_a{false}; + auto sub_b_a = node_b->create_stee_subscription( + "a", 1, [&get_b_a](std::shared_ptr msg) { + (void)msg; + get_b_a = true; + }); + bool get_c_a{false}; + auto sub_c_a = node_c->create_stee_subscription( + "a", 1, [&get_c_a](std::shared_ptr msg) { + (void)msg; + get_c_a = true; + }); + + auto pub_b = node_b->create_stee_publisher("b", 1); + + auto pub_c = node_c->create_stee_publisher("c", 1); + bool get_c_b{false}; + auto sub_c_b = node_c->create_stee_subscription( + "b", 1, [&pub_c, &get_c_b](std::shared_ptr msg) { + get_c_b = true; + pub_c->publish(*msg); + }); + + bool checker_node_called{false}; + auto sub_checker = checker_node->create_subscription( + "c/stee", 1, [&checker_node_called](std::shared_ptr msg) { + checker_node_called = true; + EXPECT_EQ(msg->sources.size(), 1u); + EXPECT_EQ(msg->sources[0].topic, "in"); + EXPECT_EQ(msg->sources[0].stamp.sec, 8); + EXPECT_EQ(msg->sources[0].stamp.nanosec, 18u); + EXPECT_EQ(msg->sources[0].first_subscription_steady_time.sec, 5); + EXPECT_EQ(msg->sources[0].first_subscription_steady_time.nanosec, 15u); + }); + + SteePointCloud2 msg_a; + msg_a.body.header.stamp = get_time(10, 110); + tilde_msg::msg::SteeSource source; + source.topic = "in"; + source.stamp = get_time(8, 18); + source.first_subscription_steady_time = get_time(5, 15); + msg_a.sources.push_back(source); + + pub_a->publish(msg_a); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node_b); + rclcpp::spin_some(node_c); + + PointCloud2 msg_b; + msg_b.header.stamp = msg_a.body.header.stamp; + pub_b->publish(msg_b); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node_c); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(get_b_a); + EXPECT_TRUE(get_c_a); + EXPECT_TRUE(get_c_b); + EXPECT_TRUE(checker_node_called); +} diff --git a/src/tilde/test/test_stee_source_table.cpp b/src/tilde/test/test_stee_source_table.cpp new file mode 100644 index 00000000..985cb7b0 --- /dev/null +++ b/src/tilde/test/test_stee_source_table.cpp @@ -0,0 +1,370 @@ +// Copyright 2021 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/stee_sources_table.hpp" + +#include + +#include +#include +#include + +using tilde::SteeSourceCmp; +using tilde::SteeSourcesTable; +using tilde_msg::msg::SteeSource; + +TEST(TestSteeSourceCmp, simple_test) +{ + SteeSourceCmp cmp; + tilde_msg::msg::SteeSource lhs; + tilde_msg::msg::SteeSource rhs; + + // topic order differs + lhs.topic = "in1"; + rhs.topic = "in2"; + + EXPECT_TRUE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); + + // same topic order + rhs.topic = lhs.topic; + EXPECT_FALSE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); + + // stamp.sec differs + rhs.stamp.sec = 1; + EXPECT_TRUE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); + + lhs.stamp.sec = rhs.stamp.sec; + + // stamp.nanosec differs + rhs.stamp.nanosec = 1; + EXPECT_TRUE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); + + lhs.stamp.nanosec = rhs.stamp.nanosec; + + // first_subscription_steady_time.sec differs + rhs.first_subscription_steady_time.sec = 1; + EXPECT_TRUE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); + + lhs.first_subscription_steady_time.sec = rhs.first_subscription_steady_time.sec; + + // first_subscription_steady_time.nanosec differs + rhs.first_subscription_steady_time.nanosec = 1; + EXPECT_TRUE(cmp(lhs, rhs)); + EXPECT_FALSE(cmp(rhs, lhs)); +} + +class TestSteeSourcesTable : public ::testing::Test +{ +public: + void SetUp() override {} + + void TearDown() override {} +}; + +rclcpp::Time get_time(int32_t sec, uint32_t nsec) { return rclcpp::Time(sec, nsec, RCL_ROS_TIME); } + +void add_sources( + const std::string & in_topic, const rclcpp::Time & stamp, const rclcpp::Time & steady, + std::vector & out) +{ + SteeSource s; + s.topic = in_topic; + s.stamp = stamp; + s.first_subscription_steady_time = steady; + out.emplace_back(std::move(s)); +} + +TEST_F(TestSteeSourcesTable, set_one_entry) +{ + SteeSourcesTable table(100); + + /** + * node topology: + * "in" -> "topic1" + */ + + // in + auto in_stamp = get_time(100, 0); + auto in_steady = get_time(0, 10); + SteeSourcesTable::SourcesMsg sources_msg; + add_sources("in", in_stamp, in_steady, sources_msg); + + // topic1 + auto topic11_stamp = get_time(101, 0); + table.set("topic1", topic11_stamp, sources_msg); + + auto check = [&in_stamp, &in_steady](const SteeSourcesTable::SourcesMsg & msg) -> void { + EXPECT_EQ(msg.size(), 1u); + + auto in_source = msg[0]; + EXPECT_EQ(in_source.topic, "in"); + EXPECT_EQ(in_source.stamp, in_stamp); + EXPECT_EQ(in_source.first_subscription_steady_time, in_steady); + }; + + // test for get_latest_sources + { + auto latest_sources = table.get_latest_sources(); + + EXPECT_EQ(latest_sources.size(), 1u); + EXPECT_EQ(latest_sources.begin()->first, "topic1"); + + auto in_sources = latest_sources.begin()->second; + check(in_sources); + } + + // test for get_sources + { + auto in_sources = table.get_sources("topic1", topic11_stamp); + EXPECT_EQ(in_sources.size(), 1u); + + check(in_sources); + } + + // test for get_sources, not found cases + { + auto not_found1 = table.get_sources("topic2", topic11_stamp); + EXPECT_EQ(not_found1.size(), 0u); + + auto stamp2 = get_time(0, 1); + auto not_found2 = table.get_sources("topic1", stamp2); + EXPECT_EQ(not_found2.size(), 0u); + } +} + +TEST_F(TestSteeSourcesTable, set_one_entry_multiple_source_topics) +{ + SteeSourcesTable table(100); + + /** + * node topology: + * in1 --> topic1 + * in2 / + */ + + SteeSourcesTable::SourcesMsg sources; + auto in11_stamp = get_time(100, 0); + auto in11_steady = get_time(0, 10); + add_sources("in1", in11_stamp, in11_steady, sources); + + auto in21_stamp = get_time(100, 10); + auto in21_steady = get_time(0, 20); + add_sources("in2", in21_stamp, in21_steady, sources); + + // topic1 input + auto topic11_stamp = get_time(110, 0); + table.set("topic1", topic11_stamp, sources); + + auto check = [&in11_stamp, &in11_steady, &in21_stamp, + &in21_steady](const SteeSourcesTable::SourcesMsg & msg) -> void { + EXPECT_EQ(msg.size(), 2u); + for (const auto & in_source : msg) { + if (in_source.topic == "in1") { + EXPECT_EQ(in_source.stamp, in11_stamp); + EXPECT_EQ(in_source.first_subscription_steady_time, in11_steady); + } else if (in_source.topic == "in2") { + EXPECT_EQ(in_source.stamp, in21_stamp); + EXPECT_EQ(in_source.first_subscription_steady_time, in21_steady); + } else { + FAIL() << "unknown source topic"; + } + } + }; + + // test for get_latest_sources + { + auto latest_sources = table.get_latest_sources(); + + EXPECT_EQ(latest_sources.size(), 1u); + EXPECT_EQ(latest_sources.begin()->first, "topic1"); + + auto in_sources = latest_sources.begin()->second; + check(in_sources); + } + + // test for get_sources + { + auto in_sources = table.get_sources("topic1", topic11_stamp); + check(in_sources); + } +} + +TEST_F(TestSteeSourcesTable, source_topic_has_multiple_stamp) +{ + SteeSourcesTable table(100); + + /** + * node topology: + * in1 --> topic1 + * + * topic1 has multiple stamps. + */ + + // topic1 msg1 input + SteeSourcesTable::SourcesMsg sources11; + auto in11_stamp = get_time(110, 0); + auto in11_steady = get_time(0, 11); + add_sources("in1", in11_stamp, in11_steady, sources11); + + auto topic11_stamp = get_time(110, 11); + table.set("topic1", topic11_stamp, sources11); + + // topic1 msg2 input + SteeSourcesTable::SourcesMsg sources12; + auto in12_stamp = get_time(120, 00); + auto in12_steady = get_time(0, 12); + add_sources("in1", in12_stamp, in12_steady, sources12); + + auto topic12_stamp = get_time(120, 12); + table.set("topic1", topic12_stamp, sources12); + + // test for get_latest sources + auto latest_sources = table.get_latest_sources(); + EXPECT_EQ(latest_sources.size(), 1u); + EXPECT_EQ(latest_sources.begin()->first, "topic1"); + + const auto & in_sources = latest_sources.begin()->second; + EXPECT_EQ(in_sources.size(), 1u); + const auto & in_source = *in_sources.begin(); + EXPECT_EQ(in_source.topic, "in1"); + EXPECT_EQ(in_source.stamp, in12_stamp); + EXPECT_EQ(in_source.first_subscription_steady_time, in12_steady); +} + +TEST_F(TestSteeSourcesTable, source_topic_has_multiple_stamp_skew) +{ + SteeSourcesTable table(100); + + /** + * node topology: + * in1 --> topic1 + * + * topic1 has two stamps, t11 and t12 where t11 < t12. + * But message arrival order is t12 -> t11. + * Thus t11 is the latest message (i.e. skew) + */ + + // topic1 msg2 input + SteeSourcesTable::SourcesMsg sources12; + auto in12_stamp = get_time(120, 00); + auto in12_steady = get_time(0, 12); + add_sources("in1", in12_stamp, in12_steady, sources12); + + auto topic12_stamp = get_time(120, 12); + table.set("topic1", topic12_stamp, sources12); + + // topic1 msg1 input + SteeSourcesTable::SourcesMsg sources11; + auto in11_stamp = get_time(110, 0); + auto in11_steady = get_time(0, 11); + add_sources("in1", in11_stamp, in11_steady, sources11); + + auto topic11_stamp = get_time(110, 11); + table.set("topic1", topic11_stamp, sources11); + + // test for get_latest sources + auto latest_sources = table.get_latest_sources(); + EXPECT_EQ(latest_sources.size(), 1u); + EXPECT_EQ(latest_sources.begin()->first, "topic1"); + + const auto & in_sources = latest_sources.begin()->second; + EXPECT_EQ(in_sources.size(), 1u); + const auto & in_source = *in_sources.begin(); + EXPECT_EQ(in_source.topic, "in1"); + EXPECT_EQ(in_source.stamp, in11_stamp); + EXPECT_EQ(in_source.first_subscription_steady_time, in11_steady); +} + +TEST_F(TestSteeSourcesTable, stamp_deletion) +{ + SteeSourcesTable table(1); + + /** + * node topology: + * in1 --> topic1 + * + * topic1 has multiple stamps. + */ + + // topic1 msg1 input + SteeSourcesTable::SourcesMsg sources11; + auto in11_stamp = get_time(110, 0); + auto in11_steady = get_time(0, 11); + add_sources("in1", in11_stamp, in11_steady, sources11); + + auto topic11_stamp = get_time(110, 11); + table.set("topic1", topic11_stamp, sources11); + + EXPECT_EQ(table.get_sources("topic1", topic11_stamp).size(), 1u); + + // topic1 msg2 input + SteeSourcesTable::SourcesMsg sources12; + auto in12_stamp = get_time(120, 00); + auto in12_steady = get_time(0, 12); + add_sources("in1", in12_stamp, in12_steady, sources12); + + auto topic12_stamp = get_time(120, 12); + table.set("topic1", topic12_stamp, sources12); + + // check topic1 msg1 is purged + EXPECT_EQ(table.get_sources("topic1", topic11_stamp).size(), 0u); +} + +TEST_F(TestSteeSourcesTable, duplicated_stamp) +{ + SteeSourcesTable table(100); + + { + SteeSourcesTable::SourcesMsg sources_msg; + auto stamp = get_time(110, 0); + auto steady = get_time(0, 11); + tilde_msg::msg::SteeSource source; + source.topic = "in"; + source.stamp = stamp; + source.first_subscription_steady_time = steady; + + // set same stamp multiple times + sources_msg.push_back(source); + sources_msg.push_back(source); + sources_msg.push_back(source); + + // set different source + source.topic = "in1"; + sources_msg.push_back(source); + + table.set("topic", stamp, sources_msg); + } + + // expect only two sources + auto latest_sources = table.get_latest_sources()["topic"]; + EXPECT_EQ(latest_sources.size(), 2u); + for (const auto & source : latest_sources) { + if (source.topic == "in") { + EXPECT_EQ(source.stamp.sec, 110); + EXPECT_EQ(source.stamp.nanosec, 0u); + EXPECT_EQ(source.first_subscription_steady_time.sec, 0); + EXPECT_EQ(source.first_subscription_steady_time.nanosec, 11u); + } else if (source.topic == "in1") { + EXPECT_EQ(source.stamp.sec, 110); + EXPECT_EQ(source.stamp.nanosec, 0u); + EXPECT_EQ(source.first_subscription_steady_time.sec, 0); + EXPECT_EQ(source.first_subscription_steady_time.nanosec, 11u); + } + } +} diff --git a/src/tilde/test/test_tilde_node.cpp b/src/tilde/test/test_tilde_node.cpp new file mode 100644 index 00000000..af3c634c --- /dev/null +++ b/src/tilde/test/test_tilde_node.cpp @@ -0,0 +1,476 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" +#include "tilde_msg/msg/test_top_level_stamp.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include + +#include +#include +#include + +using tilde::InputInfo; +using tilde::TildeNode; +using tilde_msg::msg::MessageTrackingTag; + +class TestTildeNode : public ::testing::Test +{ +public: + void SetUp() override { rclcpp::init(0, nullptr); } + + void TearDown() override { rclcpp::shutdown(); } +}; + +std::string str(const builtin_interfaces::msg::Time & time) +{ + std::string s = ""; + s += std::to_string(time.sec); + s += "."; + s += std::to_string(time.nanosec); + return s; +} + +/** + * consider the following system: + * sensor_node -> main_node -> checker_node + * + * Check main_node MessageTrackingTag stamps by checker_node. + * The type of message is PointCloud2. + */ +TEST_F(TestTildeNode, simple_case) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto sensor_node = std::make_shared("sensorNode", options); + auto main_node = std::make_shared("pubNode", options); + auto checker_node = std::make_shared("checkerNode", options); + + auto sensor_pub = sensor_node->create_publisher("in_topic", 1); + auto clock_pub = sensor_node->create_publisher("/clock", 1); + + // apply "/clock" + rosgraph_msgs::msg::Clock clock_msg; + clock_msg.clock.sec = 123; + clock_msg.clock.nanosec = 456; + + clock_pub->publish(clock_msg); + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + // prepare pub/sub + auto main_pub = main_node->create_tilde_publisher("out_topic", 1); + auto main_sub = main_node->create_tilde_subscription( + "in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + std::cout << "main_sub_callback" << std::endl; + (void)msg; + main_pub->publish(std::move(msg)); + }); + + bool checker_sub_called = false; + auto checker_sub = checker_node->create_subscription( + "out_topic/message_tracking_tag", 1, + [&checker_sub_called, + clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { + checker_sub_called = true; + std::cout << "checker_sub_callback" << std::endl; + std::cout << "message_tracking_tag_msg: \n" + << "pub_time: " << str(message_tracking_tag_msg->output_info.pub_time) << "\n" + << "pub_time_steady: " << str(message_tracking_tag_msg->output_info.pub_time_steady) + << "\n" + << std::endl; + EXPECT_EQ(message_tracking_tag_msg->output_info.pub_time, clock_msg.clock); + EXPECT_EQ(message_tracking_tag_msg->output_info.has_header_stamp, true); + }); + + // do scenario + auto sensor_msg = std::make_unique(); + sensor_msg->header.stamp = sensor_node->now(); + sensor_pub->publish(std::move(sensor_msg)); + + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_sub_called); +} + +/** + * consider the following system: + * sensor_node -> main_node -> checker_node + * + * Check main_node MessageTrackingTag stamps by checker_node. + * The type of message is std_msgs::msg::String. + */ +TEST_F(TestTildeNode, no_header_case) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + auto sensor_node = std::make_shared("sensorNode", options); + auto main_node = std::make_shared("pubNode", options); + auto checker_node = std::make_shared("checkerNode", options); + + auto sensor_pub = sensor_node->create_publisher("in_topic", 1); + auto clock_pub = sensor_node->create_publisher("/clock", 1); + + // apply "/clock" + rosgraph_msgs::msg::Clock clock_msg; + clock_msg.clock.sec = 123; + clock_msg.clock.nanosec = 456; + + clock_pub->publish(clock_msg); + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + // prepare pub/sub + auto main_pub = main_node->create_tilde_publisher("out_topic", 1); + auto main_sub = main_node->create_tilde_subscription( + "in_topic", 1, [&main_pub](std_msgs::msg::String::UniquePtr msg) -> void { + std::cout << "main_sub_callback" << std::endl; + (void)msg; + main_pub->publish(std::move(msg)); + }); + + bool checker_sub_called = false; + auto checker_sub = checker_node->create_subscription( + "out_topic/message_tracking_tag", 1, + [&checker_sub_called, + clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { + checker_sub_called = true; + std::cout << "checker_sub_callback" << std::endl; + std::cout << "message_tracking_tag_msg: \n" + << "pub_time: " << str(message_tracking_tag_msg->output_info.pub_time) << "\n" + << "pub_time_steady: " << str(message_tracking_tag_msg->output_info.pub_time_steady) + << "\n" + << std::endl; + EXPECT_EQ(message_tracking_tag_msg->output_info.pub_time, clock_msg.clock); + EXPECT_EQ(message_tracking_tag_msg->output_info.has_header_stamp, false); + }); + + // do scenario + auto msg = std::make_unique(); + sensor_pub->publish(std::move(msg)); + + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + EXPECT_TRUE(checker_sub_called); +} + +TEST_F(TestTildeNode, enable_tilde) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + options.append_parameter_override("enable_tilde", false); + + auto sensor_node = std::make_shared("sensorNode", options); + auto main_node = std::make_shared("pubNode", options); + auto checker_node = std::make_shared("checkerNode", options); + + bool enable_tilde = true; + main_node->get_parameter("enable_tilde", enable_tilde); + EXPECT_EQ(enable_tilde, false); + + auto sensor_pub = sensor_node->create_publisher("in_topic", 1); + auto clock_pub = sensor_node->create_publisher("/clock", 1); + + // apply "/clock" + rosgraph_msgs::msg::Clock clock_msg; + clock_msg.clock.sec = 123; + clock_msg.clock.nanosec = 456; + + clock_pub->publish(clock_msg); + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + // prepare pub/sub + auto main_pub = main_node->create_tilde_publisher("out_topic", 1); + auto main_sub = main_node->create_tilde_subscription( + "in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + std::cout << "main_sub_callback" << std::endl; + (void)msg; + main_pub->publish(std::move(msg)); + }); + + bool checker_sub_called = false; + auto checker_sub = checker_node->create_subscription( + "out_topic/message_tracking_tag", 1, + [&checker_sub_called, + clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { + (void)message_tracking_tag_msg; + checker_sub_called = true; + EXPECT_TRUE(false); // expect never comes here + }); + + // do scenario + auto sensor_msg = std::make_unique(); + sensor_msg->header.stamp = sensor_node->now(); + sensor_pub->publish(std::move(sensor_msg)); + + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + EXPECT_EQ(checker_sub_called, false); +} + +TEST_F(TestTildeNode, register_message_as_input_find_subscription_time) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto sensor_node = std::make_shared("sensorNode", options); + auto main_node = std::make_shared("mainNode", options); + + auto sensor_pub = sensor_node->create_publisher("/in_topic", 1); + auto clock_pub = sensor_node->create_publisher("/clock", 1); + + // prepare pub/sub + auto main_pub = main_node->create_tilde_publisher("/out_topic", 1); + auto main_sub = main_node->create_tilde_subscription( + "/in_topic", 1, [&main_pub](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + std::cout << "main_sub_callback" << std::endl; + (void)msg; + main_pub->publish(std::move(msg)); + }); + + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(main_node); + auto in_topic_resolved_name = node_topics_interface->resolve_topic_name("/in_topic"); + + // publish @123.456 + rosgraph_msgs::msg::Clock clock_msg1; + clock_msg1.clock.sec = 123; + clock_msg1.clock.nanosec = 456; + + clock_pub->publish(clock_msg1); + for (int i = 0; + i < 10 || !(main_node->now() == clock_msg1.clock && sensor_node->now() == clock_msg1.clock); + i++) { + rclcpp::spin_some(sensor_node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + rclcpp::spin_some(main_node); + } + + EXPECT_EQ(sensor_node->now(), clock_msg1.clock); + EXPECT_EQ(main_node->now(), clock_msg1.clock); + + sensor_msgs::msg::PointCloud2 sensor_msg1; + sensor_msg1.header.stamp = sensor_node->now(); + sensor_pub->publish(sensor_msg1); + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + // publish @124.321 + rosgraph_msgs::msg::Clock clock_msg2; + clock_msg2.clock.sec = 124; + clock_msg2.clock.nanosec = 321; + + clock_pub->publish(clock_msg2); + for (int i = 0; + i < 10 || !(main_node->now() == clock_msg2.clock && sensor_node->now() == clock_msg2.clock); + i++) { + rclcpp::spin_some(sensor_node); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + rclcpp::spin_some(main_node); + } + + EXPECT_EQ(sensor_node->now(), clock_msg2.clock); + EXPECT_EQ(main_node->now(), clock_msg2.clock); + + sensor_msgs::msg::PointCloud2 sensor_msg2; + sensor_msg2.header.stamp = sensor_node->now(); + sensor_pub->publish(sensor_msg2); + rclcpp::spin_some(sensor_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(main_node); + + // check + rclcpp::Time subscription_time, subscription_time_steady; + auto found = main_node->find_subscription_time( + &sensor_msg1, in_topic_resolved_name, subscription_time, subscription_time_steady); + EXPECT_TRUE(found); + builtin_interfaces::msg::Time subscription_time_msg = subscription_time; + EXPECT_EQ(subscription_time_msg.sec, clock_msg1.clock.sec); + EXPECT_EQ(subscription_time_msg.nanosec, clock_msg1.clock.nanosec); +} + +TEST_F(TestTildeNode, publish_top_level_stamp) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto main_node = std::make_shared("mainNode", options); + auto checker_node = std::make_shared("checkerNode", options); + + // prepare publishers + auto main_pub = + main_node->create_tilde_publisher("out_topic", 1); + auto clock_pub = main_node->create_publisher("/clock", 1); + + // apply clock + rosgraph_msgs::msg::Clock clock_msg; + clock_msg.clock.sec = 123; + clock_msg.clock.nanosec = 456; + + clock_pub->publish(clock_msg); + rclcpp::spin_some(main_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + // prepare checker subscription + bool checker_sub_called = false; + auto checker_sub = checker_node->create_subscription( + "out_topic/message_tracking_tag", 1, + [&checker_sub_called, + &clock_msg](tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { + (void)message_tracking_tag_msg; + checker_sub_called = true; + EXPECT_TRUE(message_tracking_tag_msg->output_info.has_header_stamp); + }); + + // publish + tilde_msg::msg::TestTopLevelStamp msg; + msg.stamp.sec = 1234; + msg.stamp.nanosec = 5678; + + main_pub->publish(msg); + + rclcpp::spin_some(main_node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(checker_sub_called); +} + +TEST_F(TestTildeNode, param_enable_tilde) +{ + rclcpp::NodeOptions options; + options.append_parameter_override("use_sim_time", true); + + auto main_node = std::make_shared("mainNode", options); + auto checker_node = std::make_shared("checkerNode", options); + + auto main_pub = + main_node->create_tilde_publisher("out_topic", 1); + + bool checker_sub_called = false; + auto checker_sub = checker_node->create_subscription( + "out_topic/message_tracking_tag", 1, + [&checker_sub_called]( + tilde_msg::msg::MessageTrackingTag::UniquePtr message_tracking_tag_msg) -> void { + (void)message_tracking_tag_msg; + checker_sub_called = true; + EXPECT_TRUE(message_tracking_tag_msg->output_info.has_header_stamp); + }); + + // under enable_tilde = true + tilde_msg::msg::TestTopLevelStamp msg; + msg.stamp.sec = 1234; + msg.stamp.nanosec = 5678; + + main_pub->publish(msg); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_TRUE(checker_sub_called); + + // under enable_tilde = false + rclcpp::Parameter param("enable_tilde", false); + main_node->set_parameter(param); + rclcpp::spin_some(main_node); + checker_sub_called = false; + + msg.stamp.sec = 2234; + msg.stamp.nanosec = 5679; + + main_pub->publish(msg); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(checker_node); + + EXPECT_FALSE(checker_sub_called); +} + +class ChildParamCallback : public TildeNode +{ +public: + explicit ChildParamCallback(const rclcpp::NodeOptions & options) + : TildeNode("child_param_callback", options) + { + this->declare_parameter("child_param", "original"); + this->get_parameter("child_param", child_param_); + + param_callback_handle_ = this->add_on_set_parameters_callback( + [this](const std::vector & parameters) { + auto result = rcl_interfaces::msg::SetParametersResult(); + + result.successful = true; + for (const auto & parameter : parameters) { + if (parameter.get_name() == "child_param") { + child_param_ = parameter.as_string(); + } + } + return result; + }); + } + + std::string child_param_; + +private: + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; +}; + +TEST_F(TestTildeNode, child_param_callback) +{ + rclcpp::NodeOptions options; + + auto node = std::make_shared(options); + + // check default + bool enable_tilde = false; + node->get_parameter("enable_tilde", enable_tilde); + EXPECT_TRUE(enable_tilde); + + std::string child_param; + node->get_parameter("child_param", child_param); + EXPECT_EQ(child_param, "original"); + + // update + rclcpp::Parameter enable_tilde_update("enable_tilde", false); + node->set_parameter(enable_tilde_update); + rclcpp::Parameter child_param_update("child_param", "updated"); + node->set_parameter(child_param_update); + + // check + node->get_parameter("enable_tilde", enable_tilde); + EXPECT_FALSE(enable_tilde); + + node->get_parameter("child_param", child_param); + EXPECT_EQ(child_param, "updated"); +} diff --git a/src/tilde/test/test_tilde_publisher.cpp b/src/tilde/test/test_tilde_publisher.cpp new file mode 100644 index 00000000..42765e09 --- /dev/null +++ b/src/tilde/test/test_tilde_publisher.cpp @@ -0,0 +1,369 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "tilde/tilde_publisher.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" + +#include + +#include +#include + +using tilde::InputInfo; +using tilde::TildePublisherBase; +using tilde_msg::msg::MessageTrackingTag; + +class TestTildePublisher : public ::testing::Test +{ +public: + void SetUp() {} +}; + +void expect_near(const rclcpp::Time && lhs, const rclcpp::Time && rhs, uint64_t thres_ms = 1) +{ + uint64_t thres_ns = thres_ms * 1000 * 1000; + EXPECT_NEAR(lhs.nanoseconds(), rhs.nanoseconds(), thres_ns); +} + +TEST_F(TestTildePublisher, set_implicit_and_fill_input_info) +{ + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + + TildePublisherBase pub(clock, steady_clock, "node_name"); + auto info = std::make_shared(); + info->sub_time = rclcpp::Time(1, 0); + info->sub_time_steady = steady_clock->now(); + info->has_header_stamp = true; + info->header_stamp = rclcpp::Time(0, 1); + const std::string TOPIC = "sample_topic"; + + pub.set_implicit_input_info(TOPIC, info); + + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(1, 0)); + expect_near( + rclcpp::Time(msg.input_infos[0].sub_time_steady, RCL_STEADY_TIME), steady_clock->now()); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, rclcpp::Time(0, 1)); +} + +TEST_F(TestTildePublisher, add_explicit_input_info_sub_time_not_found) +{ + // set input_info & explicit_input_info + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + + TildePublisherBase pub(clock, steady_clock, "node_name"); + auto now = clock->now(); + auto info_stamp = now - rclcpp::Duration(1, 0); + auto search_stamp = now - rclcpp::Duration(2, 0); + + auto info = std::make_shared(); + info->sub_time = now; + info->sub_time_steady = steady_clock->now(); + info->has_header_stamp = true; + info->header_stamp = info_stamp; + const std::string TOPIC = "sample_topic"; + + // they should be ignored + pub.set_implicit_input_info(TOPIC, info); + pub.set_implicit_input_info(TOPIC + "2", info); + + // use TOPIC + search_stamp explicit info + // although corresponding explicit sub_time doesn't exist + pub.add_explicit_input_info(TOPIC, search_stamp); + + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(0, 0)); + EXPECT_EQ(msg.input_infos[0].sub_time_steady, rclcpp::Time(0, 0)); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, search_stamp); +} + +TEST_F(TestTildePublisher, set_explicit_subscription_time_success_then_purged) +{ + // set input_info & explicit_input_info + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + TildePublisherBase pub(clock, steady_clock, "node_name"); + + const std::string TOPIC = "sample_topic"; + auto now = clock->now(); + auto recv_base = now - rclcpp::Duration(10, 0); + auto recv_base_steady = steady_clock->now(); + auto stamp_base = now - rclcpp::Duration(20, 0); + pub.set_max_sub_callback_infos_sec(21); // pseudo-boundary condition + + for (int i = 0; i < 10; i++) { + auto input_info = std::make_shared(); + input_info->sub_time = recv_base + rclcpp::Duration(i, 0); + input_info->sub_time_steady = recv_base_steady + rclcpp::Duration(i, 0); + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base + rclcpp::Duration(i, 0); + + pub.set_explicit_subscription_time(TOPIC, input_info); + } + + // normal case, middle of data + pub.add_explicit_input_info(TOPIC, stamp_base + rclcpp::Duration(5, 0)); + + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, recv_base + rclcpp::Duration(5, 0)); + EXPECT_EQ(msg.input_infos[0].sub_time_steady, recv_base_steady + rclcpp::Duration(5, 0)); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base + rclcpp::Duration(5, 0)); + + // boundary condition: the 1st element exists + pub.add_explicit_input_info(TOPIC, stamp_base); + + pub.fill_input_info(msg); + + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, recv_base); + EXPECT_EQ(msg.input_infos[0].sub_time_steady, recv_base_steady); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); + + // 1st element should be deleted + pub.set_max_sub_callback_infos_sec(20); // pseudo-boundary condition + auto input_info = std::make_shared(); + input_info->sub_time = recv_base + rclcpp::Duration(10, 0); + input_info->sub_time_steady = recv_base_steady + rclcpp::Duration(10, 0); + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base + rclcpp::Duration(10, 0); + pub.set_explicit_subscription_time(TOPIC, input_info); + + pub.add_explicit_input_info(TOPIC, stamp_base); + + pub.fill_input_info(msg); + + // check explicit info is deleted + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, rclcpp::Time(0, 0)); + EXPECT_EQ(msg.input_infos[0].sub_time_steady, rclcpp::Time(0, 0)); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); +} + +TEST_F(TestTildePublisher, set_multiple_topic) +{ + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + TildePublisherBase pub(clock, steady_clock, "node_name"); + const std::string TOPIC1 = "sample_topic1"; + const std::string TOPIC2 = "sample_topic2"; + + auto now = clock->now(); + auto recv_base = now - rclcpp::Duration(10, 0); + auto recv_base_steady = steady_clock->now(); + auto stamp_base = now - rclcpp::Duration(20, 0); + pub.set_max_sub_callback_infos_sec(100); // large value to prevent cleanup + + auto input_info = std::make_shared(); + input_info->sub_time = recv_base; + input_info->sub_time_steady = recv_base_steady; + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base; + + pub.set_explicit_subscription_time(TOPIC1, input_info); + pub.add_explicit_input_info(TOPIC1, stamp_base); + + auto recv_base2 = recv_base + rclcpp::Duration(2, 1); + auto recv_base2_steady = recv_base_steady + rclcpp::Duration(2, 1); + auto stamp_base2 = stamp_base + rclcpp::Duration(1, 1); + + auto input_info2 = std::make_shared(); + input_info2->sub_time = recv_base2; + input_info2->sub_time_steady = recv_base2_steady; + input_info2->has_header_stamp = true; + input_info2->header_stamp = stamp_base2; + + pub.set_explicit_subscription_time(TOPIC2, input_info2); + pub.add_explicit_input_info(TOPIC2, stamp_base2); + + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + + EXPECT_EQ(msg.input_infos.size(), 2ul); + int idx1 = 0; + int idx2 = 1; + if (msg.input_infos[0].topic_name == TOPIC2) { + idx1 = 1; + idx2 = 0; + } + + EXPECT_EQ(msg.input_infos[idx1].topic_name, TOPIC1); + EXPECT_EQ(msg.input_infos[idx1].sub_time, recv_base); + EXPECT_EQ(msg.input_infos[idx1].sub_time_steady, recv_base_steady); + EXPECT_EQ(msg.input_infos[idx1].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[idx1].header_stamp, stamp_base); + + EXPECT_EQ(msg.input_infos[idx2].topic_name, TOPIC2); + EXPECT_EQ(msg.input_infos[idx2].sub_time, recv_base2); + EXPECT_EQ(msg.input_infos[idx2].sub_time_steady, recv_base2_steady); + EXPECT_EQ(msg.input_infos[idx2].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[idx2].header_stamp, stamp_base2); +} + +TEST_F(TestTildePublisher, no_explicit_after_add_explicit) +{ + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + TildePublisherBase pub(clock, steady_clock, "node_name"); + pub.set_max_sub_callback_infos_sec(100); // large value to prevent cleanup + + const std::string TOPIC = "sample_topic"; + + auto now = clock->now(); + auto now_steady = steady_clock->now(); + auto stamp_base = now; + + // (1) use explicit API + /// TOPIC subscription + { + auto input_info = std::make_shared(); + input_info->sub_time = now; + input_info->sub_time_steady = now_steady; + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base; + + pub.set_implicit_input_info(TOPIC, input_info); + pub.set_explicit_subscription_time(TOPIC, input_info); + } + + /// use explicit API + pub.add_explicit_input_info(TOPIC, stamp_base); + + /// publish + { + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + + /// check + EXPECT_EQ(msg.input_infos.size(), 1ul); + EXPECT_EQ(msg.input_infos[0].topic_name, TOPIC); + EXPECT_EQ(msg.input_infos[0].sub_time, now); + EXPECT_EQ(msg.input_infos[0].sub_time_steady, now_steady); + EXPECT_EQ(msg.input_infos[0].has_header_stamp, true); + EXPECT_EQ(msg.input_infos[0].header_stamp, stamp_base); + } + + // (test case1) publish without subscription + /// publish + { + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + /// check + EXPECT_EQ(msg.input_infos.size(), 0ul); + } + + // (test case2) publish after subscription but no explicit API + auto now2 = now + rclcpp::Duration(5, 0); + auto now_steady2 = now_steady + rclcpp::Duration(5, 0); + auto stamp_base2 = now2; + + /// sub + { + auto input_info = std::make_shared(); + input_info->sub_time = now2; + input_info->sub_time_steady = now_steady2; + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base2; + + pub.set_explicit_subscription_time(TOPIC, input_info); + + pub.set_implicit_input_info(TOPIC, input_info); + pub.set_explicit_subscription_time(TOPIC, input_info); + } + + /// publish + { + tilde_msg::msg::MessageTrackingTag msg; + pub.fill_input_info(msg); + /// check + EXPECT_EQ(msg.input_infos.size(), 0ul); + } +} + +TEST_F(TestTildePublisher, get_input_info) +{ + auto clock = std::make_shared(); + auto steady_clock = std::make_shared(RCL_STEADY_TIME); + TildePublisherBase pub(clock, steady_clock, "node_name"); + pub.set_max_sub_callback_infos_sec(1000); // large value to prevent cleanup + + const std::string TOPIC = "sample_topic"; + + // register [TOPIC][stamp_base] + auto now = clock->now(); + auto now_steady = steady_clock->now(); + auto stamp_base = now; + + auto input_info = std::make_shared(); + input_info->sub_time = now; + input_info->sub_time_steady = now_steady; + input_info->has_header_stamp = true; + input_info->header_stamp = stamp_base; + + pub.set_implicit_input_info(TOPIC, input_info); + pub.set_explicit_subscription_time(TOPIC, input_info); + + // register [TOPIC][stamp_base2] + rclcpp::Duration dur(1, 2); + auto now2 = now + dur; + auto now_steady2 = now_steady + dur; + auto stamp_base2 = now + dur; + + auto input_info2 = std::make_shared(); + input_info2->sub_time = now2; + input_info2->sub_time_steady = now_steady2; + input_info2->has_header_stamp = true; + input_info2->header_stamp = stamp_base2; + + pub.set_implicit_input_info(TOPIC, input_info2); + pub.set_explicit_subscription_time(TOPIC, input_info2); + + // topic not found + InputInfo out; + EXPECT_FALSE(pub.get_input_info("topic", stamp_base, out)); + EXPECT_FALSE(out.has_header_stamp); + + // topic found but stamp not found + EXPECT_FALSE(pub.get_input_info(TOPIC, clock->now(), out)); + EXPECT_FALSE(out.has_header_stamp); + + // found [TOPIC][stamp_base] + EXPECT_TRUE(pub.get_input_info(TOPIC, stamp_base, out)); + EXPECT_EQ(out, *input_info); + + // found [TOPIC][stamp_base2] + EXPECT_TRUE(pub.get_input_info(TOPIC, stamp_base2, out)); + EXPECT_EQ(out, *input_info2); +} diff --git a/src/tilde_cmake/CMakeLists.txt b/src/tilde_cmake/CMakeLists.txt new file mode 100644 index 00000000..5205d42d --- /dev/null +++ b/src/tilde_cmake/CMakeLists.txt @@ -0,0 +1,20 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_cmake) + +# find dependencies +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +list(APPEND ${PROJECT_NAME}_CONFIG_EXTRAS + "tilde_cmake-extras.cmake" +) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_auto_package( + INSTALL_TO_SHARE + cmake +) diff --git a/src/tilde_cmake/README.md b/src/tilde_cmake/README.md new file mode 100644 index 00000000..f005b0e5 --- /dev/null +++ b/src/tilde_cmake/README.md @@ -0,0 +1,18 @@ +# tilde_cmake + +This package provides CMake scripts for Tilde. + +## Usage + +### tilde_package.cmake + +Call `tilde_package()` before defining build targets, which will set common options for Tilde. + +```cmake +cmake_minimum_required(VERSION 3.5) +project(package_name) + +find_package(tilde_cmake REQUIRED) +tilde_package() +find_package(tilde) +``` diff --git a/src/tilde_cmake/cmake/tilde_package.cmake b/src/tilde_cmake/cmake/tilde_package.cmake new file mode 100644 index 00000000..aabac4ac --- /dev/null +++ b/src/tilde_cmake/cmake/tilde_package.cmake @@ -0,0 +1,25 @@ +# 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. + +macro(tilde_package) + # Set ROS_DISTRO macros + set(ROS_DISTRO $ENV{ROS_DISTRO}) + if(${ROS_DISTRO} STREQUAL "rolling") + add_compile_definitions(ROS_DISTRO_ROLLING) + elseif(${ROS_DISTRO} STREQUAL "galactic") + add_compile_definitions(ROS_DISTRO_GALACTIC) + elseif(${ROS_DISTRO} STREQUAL "humble") + add_compile_definitions(ROS_DISTRO_HUMBLE) + endif() +endmacro() diff --git a/src/tilde_cmake/package.xml b/src/tilde_cmake/package.xml new file mode 100644 index 00000000..72afce88 --- /dev/null +++ b/src/tilde_cmake/package.xml @@ -0,0 +1,18 @@ + + + + tilde_cmake + 0.0.0 + TODO: Package description + y-okumura + Apache License 2.0 + + ament_cmake + + ament_lint_auto + caret_lint_common + + + ament_cmake + + diff --git a/src/tilde_cmake/tilde_cmake-extras.cmake b/src/tilde_cmake/tilde_cmake-extras.cmake new file mode 100644 index 00000000..24c4a358 --- /dev/null +++ b/src/tilde_cmake/tilde_cmake-extras.cmake @@ -0,0 +1,15 @@ +# 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_cmake_DIR}/tilde_package.cmake") diff --git a/src/tilde_deadline_detector/CMakeLists.txt b/src/tilde_deadline_detector/CMakeLists.txt new file mode 100644 index 00000000..9b2a930e --- /dev/null +++ b/src/tilde_deadline_detector/CMakeLists.txt @@ -0,0 +1,73 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_deadline_detector) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +include_directories(include) + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(tilde_msg REQUIRED) +find_package(rclcpp_components REQUIRED) + +add_library(tilde_deadline_detector_node SHARED + src/forward_estimator.cpp + src/tilde_deadline_detector_node.cpp) +ament_target_dependencies(tilde_deadline_detector_node + rclcpp + rclcpp_components + tilde_msg) + +rclcpp_components_register_node(tilde_deadline_detector_node + PLUGIN "tilde_deadline_detector::TildeDeadlineDetectorNode" + EXECUTABLE tilde_deadline_detector_node_exe) + + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() + + find_package(tilde REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(builtin_interfaces REQUIRED) + + ament_add_gtest(test_forward_estimator + src/forward_estimator.cpp + test/test_forward_estimator.cpp) + ament_target_dependencies(test_forward_estimator + rclcpp + tilde_msg) + + ament_add_gtest(test_tilde_deadline_detector_node + test/test_tilde_deadline_detector_node.cpp) + ament_target_dependencies(test_tilde_deadline_detector_node + rclcpp + builtin_interfaces + sensor_msgs + tilde + tilde_msg) + target_link_libraries(test_tilde_deadline_detector_node + tilde_deadline_detector_node) + +endif() + +install(TARGETS + tilde_deadline_detector_node + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin) + +ament_package() diff --git a/src/tilde_deadline_detector/README.md b/src/tilde_deadline_detector/README.md new file mode 100644 index 00000000..5164c096 --- /dev/null +++ b/src/tilde_deadline_detector/README.md @@ -0,0 +1,59 @@ +# tilde_deadline_detector + +## Description + +Deadline detector by TILDE MessageTrackingTag. +You can detect deadlines in some scenarios: + +- from the sensor to some node +- a specific path + +## Requirement + +- ROS 2 galactic +- TILDE enabled application +- The Messages should have the `header.stamp` field. + +## Build + +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 +$ ros2 run tilde_deadline_detector tilde_deadline_detector_node_exe \ + --ros-args --params-file src/tilde_deadline_detector/autoware_sensors.yaml +``` + +By default, it prints subscribed topics, then runs silently. +The deadline notification is not implemented yet. + +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` | topics of which you want to detect deadline | +| | `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_deadline_detector/autoware_sensors.yaml b/src/tilde_deadline_detector/autoware_sensors.yaml new file mode 100644 index 00000000..07b14e70 --- /dev/null +++ b/src/tilde_deadline_detector/autoware_sensors.yaml @@ -0,0 +1,50 @@ +tilde_deadline_detector_node: + ros__parameters: + sensor_topics: [ + # gsm8 + "/sensing/lidar/front_lower/self_cropped/pointcloud_ex", + "/sensing/lidar/front_upper/self_cropped/pointcloud_ex", + "/sensing/lidar/left_lower/self_cropped/pointcloud_ex", + "/sensing/lidar/left_upper/self_cropped/pointcloud_ex", + "/sensing/lidar/rear_lower/self_cropped/pointcloud_ex", + "/sensing/lidar/rear_upper/self_cropped/pointcloud_ex", + "/sensing/lidar/right_lower/self_cropped/pointcloud_ex", + "/sensing/lidar/right_upper/self_cropped/pointcloud_ex", + # office map + "/sensing/lidar/top/self_cropped/pointcloud_ex", + "/sensing/lidar/left/self_cropped/pointcloud_ex", + "/sensing/lidar/right/self_cropped/pointcloud_ex", + # loop + "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias", + ] + target_topics: [ + # "/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias" # loop of EKF/NDT + # "/localization/kinematic_state", # output of localization + # "/localization/pose_twist_fusion_filter/kinematic_state" + "/control/trajectory_follower/longitudinal/control_cmd", + ] + # specify deadline ms for topics in target_topics order. + # 0 means no deadline, and negative values are replaced by 0 + deadline_ms: [0] + skips_main_out: [ + # office map + "/sensing/lidar/top/rectified/pointcloud_ex", + "/sensing/lidar/left/rectified/pointcloud_ex", + "/sensing/lidar/right/rectified/pointcloud_ex", + "/sensing/lidar/top/rectified/pointcloud", + "/sensing/lidar/left/rectified/pointcloud", + "/sensing/lidar/right/rectified/pointcloud", + "/sensing/lidar/no_ground/pointcloud", + ] + skips_main_in: + [ + "/sensing/lidar/top/mirror_cropped/pointcloud_ex", + "/sensing/lidar/left/mirror_cropped/pointcloud_ex", + "/sensing/lidar/right/mirror_cropped/pointcloud_ex", + "/sensing/lidar/top/mirror_cropped/pointcloud_ex", + "/sensing/lidar/left/mirror_cropped/pointcloud_ex", + "/sensing/lidar/right/mirror_cropped/pointcloud_ex", + "/sensing/lidar/concatenated/pointcloud", + ] + print_report: true + print_pending_messages: false diff --git a/src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp b/src/tilde_deadline_detector/include/tilde_deadline_detector/forward_estimator.hpp new file mode 100644 index 00000000..c6227dca --- /dev/null +++ b/src/tilde_deadline_detector/include/tilde_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_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ +#define TILDE_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_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_deadline_detector + +#endif // TILDE_DEADLINE_DETECTOR__FORWARD_ESTIMATOR_HPP_ diff --git a/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp b/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp new file mode 100644 index 00000000..15c8a1aa --- /dev/null +++ b/src/tilde_deadline_detector/include/tilde_deadline_detector/tilde_deadline_detector_node.hpp @@ -0,0 +1,93 @@ +// 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_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ +#define TILDE_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "tilde_deadline_detector/forward_estimator.hpp" +#include "tilde_msg/msg/deadline_notification.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" + +#include + +#include +#include +#include +#include + +namespace tilde_deadline_detector +{ +struct PerformanceCounter +{ + void add(float v); + + float avg{0.0}; + float max{0.0}; + uint64_t cnt{0}; +}; + +class TildeDeadlineDetectorNode : public rclcpp::Node +{ + using MessageTrackingTagSubscription = rclcpp::Subscription; + +public: + RCLCPP_PUBLIC + explicit TildeDeadlineDetectorNode( + const std::string & node_name, const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// see corresponding rclcpp::Node constructor + RCLCPP_PUBLIC + explicit TildeDeadlineDetectorNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + RCLCPP_PUBLIC + explicit TildeDeadlineDetectorNode(const rclcpp::NodeOptions & options); + + RCLCPP_PUBLIC + virtual ~TildeDeadlineDetectorNode(); + + std::set get_message_tracking_tag_topics() const; + +private: + ForwardEstimator fe; + std::set sensor_topics_; + std::set target_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); +}; + +} // namespace tilde_deadline_detector + +#endif // TILDE_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ diff --git a/src/tilde_deadline_detector/package.xml b/src/tilde_deadline_detector/package.xml new file mode 100644 index 00000000..93a3182c --- /dev/null +++ b/src/tilde_deadline_detector/package.xml @@ -0,0 +1,24 @@ + + + + tilde_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_msg + + + ament_cmake + + diff --git a/src/tilde_deadline_detector/src/forward_estimator.cpp b/src/tilde_deadline_detector/src/forward_estimator.cpp new file mode 100644 index 00000000..3e6a63bd --- /dev/null +++ b/src/tilde_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_deadline_detector/forward_estimator.hpp" + +#include +#include +#include +#include +#include +#include + +namespace tilde_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_deadline_detector diff --git a/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp b/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp new file mode 100644 index 00000000..93583e8d --- /dev/null +++ b/src/tilde_deadline_detector/src/tilde_deadline_detector_node.cpp @@ -0,0 +1,290 @@ +// 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_deadline_detector/tilde_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 + +using std::chrono::milliseconds; +using tilde_msg::msg::MessageTrackingTag; + +namespace tilde_deadline_detector +{ +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++; +} + +TildeDeadlineDetectorNode::TildeDeadlineDetectorNode( + const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + init(); +} + +TildeDeadlineDetectorNode::TildeDeadlineDetectorNode( + const std::string & node_name, const std::string & namespace_, + const rclcpp::NodeOptions & options) +: Node(node_name, namespace_, options) +{ + init(); +} + +TildeDeadlineDetectorNode::TildeDeadlineDetectorNode(const rclcpp::NodeOptions & options) +: Node("tilde_deadline_detector_node", options) +{ + init(); +} + +TildeDeadlineDetectorNode::~TildeDeadlineDetectorNode() {} + +std::set TildeDeadlineDetectorNode::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; +} + +void TildeDeadlineDetectorNode::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_ + 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; + } + + // wait discovery done + std::set topics; + 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(); + } + + 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( + &TildeDeadlineDetectorNode::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 << "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 << std::endl; + this->fe.debug_print(); + } + }); + + 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 << 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; +} + +void TildeDeadlineDetectorNode::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; + + // 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; + } + + // send warning if overruns + if (print_report_) { + auto is = fe.get_input_sources(target, stamp); + print_report(target, stamp, is); + } + + 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; + } + + // check deadline and send notification if necessary + tilde_msg::msg::DeadlineNotification notification_msg; + notification_msg.header.stamp = this->now(); + notification_msg.topic_name = target; + notification_msg.stamp = stamp; + + 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; + auto elapsed = rclcpp::Time(pub_time_steady) - rclcpp::Time(src->output_info.pub_time_steady); + source_msg.elapsed = elapsed; + if (RCUTILS_MS_TO_NS(deadline_ms) <= elapsed.nanoseconds()) { + source_msg.is_overrun = true; + is_overrun = true; + } + notification_msg.sources.push_back(source_msg); + } + + if (is_overrun) { + notification_pub_->publish(notification_msg); + } + + // 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_deadline_detector + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_deadline_detector::TildeDeadlineDetectorNode) diff --git a/src/tilde_deadline_detector/test/test_forward_estimator.cpp b/src/tilde_deadline_detector/test/test_forward_estimator.cpp new file mode 100644 index 00000000..08a26073 --- /dev/null +++ b/src/tilde_deadline_detector/test/test_forward_estimator.cpp @@ -0,0 +1,582 @@ +// 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 "builtin_interfaces/msg/time.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tilde_deadline_detector/forward_estimator.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" +#include "tilde_msg/msg/sub_topic_time_info.hpp" + +#include + +#include +#include +#include +#include + +using tilde_deadline_detector::ForwardEstimator; +using tilde_msg::msg::MessageTrackingTag; +using tilde_msg::msg::SubTopicTimeInfo; +using TimeMsg = builtin_interfaces::msg::Time; + +TimeMsg get_time(int sec, int nsec) +{ + TimeMsg t; + t.sec = sec; + t.nanosec = nsec; + return t; +} + +std::unique_ptr create_message_tracking_tag( + const std::string & topic, const TimeMsg & time) +{ + auto message_tracking_tag = std::make_unique(); + message_tracking_tag->output_info.topic_name = topic; + message_tracking_tag->output_info.has_header_stamp = true; + message_tracking_tag->output_info.header_stamp = time; + + return message_tracking_tag; +} + +void add_input_info( + MessageTrackingTag * target_message_tracking_tag, + const MessageTrackingTag * const in_message_tracking_tag) +{ + auto ii = SubTopicTimeInfo(); + ii.topic_name = in_message_tracking_tag->output_info.topic_name; + ii.has_header_stamp = true; + ii.header_stamp = in_message_tracking_tag->output_info.header_stamp; + target_message_tracking_tag->input_infos.push_back(ii); +} + +TEST(TestForwardEstimator, one_sensor) +{ + auto fe = ForwardEstimator(); + const std::string topic = "sensor"; + + const auto time1 = get_time(10, 100); + + auto is0 = fe.get_input_sources(topic, time1); + EXPECT_EQ(is0.size(), 0u); + + // msg 1 + auto message_tracking_tag1 = create_message_tracking_tag(topic, time1); + fe.add(std::move(message_tracking_tag1)); + + auto is1 = fe.get_input_sources(topic, time1); + EXPECT_NE(is1.find(topic), is1.end()); + EXPECT_EQ(is1[topic].size(), 1u); + EXPECT_EQ(*is1[topic].begin(), time1); + + // msg 2 + auto time2 = get_time(11, 110); + auto message_tracking_tag2 = create_message_tracking_tag(topic, time2); + + fe.add(std::move(message_tracking_tag2)); + + auto is2 = fe.get_input_sources(topic, time2); + EXPECT_NE(is2.find(topic), is2.end()); + EXPECT_EQ(is2[topic].size(), 1u); + EXPECT_EQ(*is2[topic].begin(), time2); +} + +TEST(TestForwardEstimator, two_sensors) +{ + auto fe = ForwardEstimator(); + + // sensor1 msg1 + const std::string topic1 = "sensor1"; + const auto time11 = get_time(10, 100); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + fe.add(std::move(message_tracking_tag11)); + + auto is11 = fe.get_input_sources(topic1, time11); + EXPECT_NE(is11.find(topic1), is11.end()); + EXPECT_EQ(is11[topic1].size(), 1u); + EXPECT_EQ(*is11[topic1].begin(), time11); + + // sensor2 msg1 + const std::string topic2 = "sensor2"; + const auto time21 = get_time(21, 210); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + + fe.add(std::move(message_tracking_tag21)); + + auto is21 = fe.get_input_sources(topic2, time21); + EXPECT_NE(is21.find(topic2), is21.end()); + EXPECT_EQ(is21[topic2].size(), 1u); + EXPECT_EQ(*is21[topic2].begin(), time21); + + // skew + auto is1_21 = fe.get_input_sources(topic1, time21); // topic1 of time21 + EXPECT_EQ(is1_21.find(topic1), is1_21.end()); +} + +TEST(TestForwardEstimator, simple_flow_stamp_preserved) +{ + // A -> B -> C. header.stamp is preserved + auto fe = ForwardEstimator(); + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + // sensor1 msg1 + const auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // sensor2 msg1 + const auto time21 = time11; + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + + // sensor3 msg1 + rclcpp::Time time31 = time11; + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + // check + auto is21 = fe.get_input_sources(topic2, time21); + EXPECT_EQ(is21.size(), 1u); + EXPECT_NE(is21.find(topic1), is21.end()); + EXPECT_EQ(is21[topic1].size(), 1u); + EXPECT_EQ(*is21[topic1].begin(), time11); + + auto is31 = fe.get_input_sources(topic3, time31); + EXPECT_EQ(is31.size(), 1u); + EXPECT_NE(is31.find(topic1), is31.end()); + EXPECT_EQ(is31[topic1].size(), 1u); + EXPECT_EQ(*is31[topic1].begin(), time11); +} + +TEST(TestForwardEstimator, simple_flow_stamp_update) +{ + // A -> B -> C. header.stamp is updated + auto fe = ForwardEstimator(); + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + // sensor1 msg1 + const auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // sensor2 msg1 + const auto time21 = get_time(21, 210); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + + // sensor3 msg1 + const auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + // check + auto is21 = fe.get_input_sources(topic2, time21); + EXPECT_EQ(is21.size(), 1u); + EXPECT_NE(is21.find(topic1), is21.end()); + EXPECT_EQ(is21[topic1].size(), 1u); + EXPECT_EQ(*is21[topic1].begin(), time11); + + auto is31 = fe.get_input_sources(topic3, time31); + EXPECT_EQ(is31.size(), 1u); + EXPECT_NE(is31.find(topic1), is31.end()); + EXPECT_EQ(is31[topic1].size(), 1u); + EXPECT_EQ(*is31[topic1].begin(), time11); +} + +TEST(TestForwardEstimator, merged_flow) +{ + // A,B -> C. header.stamp + auto fe = ForwardEstimator(); + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + // sensor1 msg1 + const auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // sensor2 msg1 + const auto time21 = get_time(21, 210); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + + // sensor2 msg2 + const auto time22 = get_time(22, 220); + auto message_tracking_tag22 = create_message_tracking_tag(topic2, time22); + + // sensor3 msg1 consists of msg 11, 21, 22 + const auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag22.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag22)); + fe.add(std::move(message_tracking_tag31)); + + // check + auto is31 = fe.get_input_sources(topic3, time31); + EXPECT_EQ(is31.size(), 2u); + EXPECT_NE(is31.find(topic1), is31.end()); + EXPECT_EQ(is31[topic1].size(), 1u); + EXPECT_EQ(*is31[topic1].begin(), time11); + + EXPECT_NE(is31.find(topic2), is31.end()); + EXPECT_EQ(is31[topic2].size(), 2u); + EXPECT_NE(is31[topic2].find(time21), is31[topic2].end()); + EXPECT_NE(is31[topic2].find(time22), is31[topic2].end()); +} + +TEST(TestForwardEstimator, reverse_order2) +{ + // DAG is "A -> B -> C", + // but MessageTrackingTag comes C -> A -> B + auto fe = ForwardEstimator(); + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + // MessageTrackingTag of A + const auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // MessageTrackingTag of B + const auto time21 = get_time(21, 210); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + + // MessageTrackingTag of C + const auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + // add MessageTrackingTag in C -> A -> B order + fe.add(std::move(message_tracking_tag31)); + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + + auto is31 = fe.get_input_sources(topic3, time31); + EXPECT_EQ(is31.size(), 1u); + EXPECT_NE(is31.find(topic1), is31.end()); + EXPECT_EQ(is31[topic1].size(), 1u); + EXPECT_EQ(*is31[topic1].begin(), time11); +} + +TEST(TestForwardEstimator, reverse_order) +{ + // DAG is "A -> B -> C", + // but MessageTrackingTag comes C -> B -> A + auto fe = ForwardEstimator(); + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + // MessageTrackingTag of A + const auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // MessageTrackingTag of B + const auto time21 = get_time(21, 210); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + + // MessageTrackingTag of C + const auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + // add MessageTrackingTag in C -> B -> A order + fe.add(std::move(message_tracking_tag31)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag11)); + + auto is31 = fe.get_input_sources(topic3, time31); + EXPECT_EQ(is31.size(), 1u); + EXPECT_NE(is31.find(topic1), is31.end()); + EXPECT_EQ(is31[topic1].size(), 1u); + EXPECT_EQ(*is31[topic1].begin(), time11); +} + +TEST(TestForwardEstimator, reverse_order_with_merge) +{ + // DAG + // 1 --> 3 --> 4 + // / + // 2 -- + // + // MessageTrackingTag order: 4 -> 3 -> 1 -> 2 + + auto fe = ForwardEstimator(); + + const std::string topic1 = "topic1"; + auto time11 = get_time(11, 110); + const std::string topic2 = "topic2"; + auto time21 = get_time(21, 210); + const std::string topic3 = "topic3"; + auto time31 = get_time(31, 310); + const std::string topic4 = "topic4"; + auto time41 = get_time(41, 410); + + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + auto message_tracking_tag41 = create_message_tracking_tag(topic4, time41); + add_input_info(message_tracking_tag41.get(), message_tracking_tag31.get()); + + // 4 -> 3 -> 1 -> 2 + fe.add(std::move(message_tracking_tag41)); + fe.add(std::move(message_tracking_tag31)); + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + + auto is41 = fe.get_input_sources(topic4, time41); + EXPECT_EQ(is41.size(), 2u); +} + +TEST(TestForwardEstimator, expire_at_the_same_time) +{ + // DAG + // A -> B -> C + // MessageTrackingTag: A -> B -> C + // expire at the same time + auto fe = ForwardEstimator(); + + const std::string topic1 = "topic1"; + const std::string topic2 = "topic2"; + const std::string topic3 = "topic3"; + auto time = get_time(11, 110); + + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + EXPECT_EQ(fe.get_input_sources(topic3, time).size(), 1u); + + fe.delete_expired(time); + + EXPECT_EQ(fe.get_input_sources(topic3, time).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time).size(), 0u); +} + +TEST(TestForwardEstimator, expire_step_by_step) +{ + // DAG + // A -> B -> C + // MessageTrackingTag: A -> B -> C + // expire from C to A + auto fe = ForwardEstimator(); + + const std::string topic1 = "topic1"; + const std::string topic2 = "topic2"; + const std::string topic3 = "topic3"; + auto time11 = get_time(11, 110); + auto time21 = get_time(21, 210); + auto time31 = get_time(31, 310); + + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 1u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); + + fe.delete_expired(time11); + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); + + // ForwardEstimator internal data is maintained + fe.delete_expired(time21); + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); + + fe.delete_expired(time31); + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); +} + +TEST(TestForwardEstimator, expire_step_by_step_skew) +{ + // DAG + // A -> B -> C + // MessageTrackingTag: A -> B -> C + // expire from C to A + auto fe = ForwardEstimator(); + + const std::string topic1 = "topic1"; + const std::string topic2 = "topic2"; + const std::string topic3 = "topic3"; + // skew timestamp for controlling timeout + auto time11 = get_time(31, 310); + auto time21 = get_time(21, 210); + auto time31 = get_time(11, 110); + + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 1u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); + + fe.delete_expired(time31); + + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 1u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); + + fe.delete_expired(time21); + + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 1u); + + fe.delete_expired(time11); + EXPECT_EQ(fe.get_input_sources(topic3, time31).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic2, time21).size(), 0u); + EXPECT_EQ(fe.get_input_sources(topic1, time11).size(), 0u); +} + +TEST(TestForwardEstimator, get_oldest_sensor_stamp) +{ + // DAG + // A, B -> C + + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + auto time12 = get_time(12, 120); + auto message_tracking_tag12 = create_message_tracking_tag(topic1, time12); + + auto time21 = get_time(21, 110); + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time21); + auto time22 = get_time(22, 120); + auto message_tracking_tag22 = create_message_tracking_tag(topic2, time22); + + auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + + add_input_info(message_tracking_tag31.get(), message_tracking_tag11.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag12.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + add_input_info(message_tracking_tag31.get(), message_tracking_tag22.get()); + + auto fe = ForwardEstimator(); + fe.add(std::move(message_tracking_tag11)); + fe.add(std::move(message_tracking_tag21)); + fe.add(std::move(message_tracking_tag31)); + + auto oldest = fe.get_oldest_sensor_stamp(topic3, time31); + if (!oldest) { + FAIL() << "nil oldest"; + } else { + EXPECT_EQ(*oldest, rclcpp::Time(time11)); + } + + fe.delete_expired(time11); + + auto oldest_without_11 = fe.get_oldest_sensor_stamp(topic3, time31); + if (!oldest_without_11) { + FAIL() << "nil oldest_without_11"; + } else { + EXPECT_EQ(*oldest_without_11, rclcpp::Time(time21)); + } + + fe.delete_expired(time21); + + auto oldest_without_21 = fe.get_oldest_sensor_stamp(topic3, time31); + if (!oldest_without_21) { + SUCCEED(); + } else { + FAIL() << "zombie oldest_without_21"; + } +} + +TEST(TestForwardEstimator, skip_topic) +{ + // DAG + // A -> B -> C + // skip B + + const std::string topic1 = "topicA"; + const std::string topic2 = "topicB"; + const std::string topic3 = "topicC"; + + std::map skip_out_to_in; + skip_out_to_in[topic2] = topic1; + + auto time11 = get_time(11, 110); + auto message_tracking_tag11 = create_message_tracking_tag(topic1, time11); + + // skipped message should have the same timestamp as input message + auto message_tracking_tag21 = create_message_tracking_tag(topic2, time11); + add_input_info(message_tracking_tag21.get(), message_tracking_tag11.get()); + + auto time31 = get_time(31, 310); + auto message_tracking_tag31 = create_message_tracking_tag(topic3, time31); + add_input_info(message_tracking_tag31.get(), message_tracking_tag21.get()); + + auto fe = ForwardEstimator(); + fe.set_skip_out_to_in(skip_out_to_in); + + fe.add(std::move(message_tracking_tag11)); + // ignore 21 to verify skip + fe.add(std::move(message_tracking_tag31)); + + auto input_sources31 = fe.get_input_sources(topic3, time31); + + EXPECT_EQ(input_sources31.size(), 1u); + auto s = input_sources31.begin(); + EXPECT_EQ(s->first, topic1); + EXPECT_EQ(s->second.size(), 1u); + EXPECT_EQ(*s->second.begin(), time11); +} diff --git a/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp b/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp new file mode 100644 index 00000000..1ab7f7d9 --- /dev/null +++ b/src/tilde_deadline_detector/test/test_tilde_deadline_detector_node.cpp @@ -0,0 +1,275 @@ +// 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 "builtin_interfaces/msg/time.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde_deadline_detector/tilde_deadline_detector_node.hpp" +#include "tilde_msg/msg/message_tracking_tag.hpp" +#include "tilde_msg/msg/sub_topic_time_info.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include + +#include +#include +#include +#include +#include +#include + +using tilde_deadline_detector::TildeDeadlineDetectorNode; + +class TestTildeDeadlineDetectorNode : public ::testing::Test +{ +public: + void SetUp() override { rclcpp::init(0, nullptr); } + + void TearDown() override { rclcpp::shutdown(); } +}; + +builtin_interfaces::msg::Time get_time(int sec, int nsec) +{ + builtin_interfaces::msg::Time t; + t.sec = sec; + t.nanosec = nsec; + return t; +} + +builtin_interfaces::msg::Duration get_duration(int32_t sec, uint32_t nsec) +{ + return rclcpp::Duration(sec, nsec); +} + +TEST_F(TestTildeDeadlineDetectorNode, get_message_tracking_tag_topics) +{ + auto tilde_node = tilde::TildeNode("tilde_node"); + auto pub = tilde_node.create_tilde_publisher("topic", 1); + auto pub2 = tilde_node.create_publisher("topic2", 1); + auto pub3 = + tilde_node.create_publisher("topic2/message_tracking_tag", 1); + + auto det_node = TildeDeadlineDetectorNode("node"); + auto topics = det_node.get_message_tracking_tag_topics(); + + EXPECT_EQ(topics.size(), 1u); + EXPECT_EQ(*topics.begin(), "/topic/message_tracking_tag"); +} + +tilde_msg::msg::MessageTrackingTag get_message_tracking_tag( + const std::string & topic, builtin_interfaces::msg::Time stamp, + builtin_interfaces::msg::Time pub_steady) +{ + tilde_msg::msg::MessageTrackingTag tag; + tag.output_info.topic_name = topic; + tag.output_info.pub_time_steady = pub_steady; + tag.output_info.has_header_stamp = true; + tag.output_info.header_stamp = stamp; + return tag; +} + +void add_input_info( + tilde_msg::msg::MessageTrackingTag & tag, const tilde_msg::msg::MessageTrackingTag & in_tag) +{ + tilde_msg::msg::SubTopicTimeInfo sub_info; + sub_info.topic_name = in_tag.output_info.topic_name; + sub_info.has_header_stamp = true; + sub_info.header_stamp = in_tag.output_info.header_stamp; + tag.input_infos.push_back(sub_info); +} + +TEST_F(TestTildeDeadlineDetectorNode, test_deadline_detection) +{ + auto tag_sender_node = std::make_shared("tag_sender_node"); + auto sensor_pub = tag_sender_node->create_publisher( + "sensor/message_tracking_tag", rclcpp::QoS(1).best_effort()); + auto next_pub = tag_sender_node->create_publisher( + "next/message_tracking_tag", rclcpp::QoS(1).best_effort()); + + rclcpp::NodeOptions options; + options.append_parameter_override("target_topics", std::vector{"next"}); + int deadline_ms = 10; + options.append_parameter_override("deadline_ms", std::vector{deadline_ms}); + + auto deadline_detector_node = + std::make_shared("deadline_detector_node", options); + + auto checker_node = std::make_shared("checker_node"); + bool notification_called = false; + auto checker_sub = checker_node->create_subscription( + "deadline_notification", rclcpp::QoS(1).best_effort(), + [this, ¬ification_called, + deadline_ms](tilde_msg::msg::DeadlineNotification::UniquePtr msg) -> void { + notification_called = true; + EXPECT_EQ(msg->topic_name, "next"); + EXPECT_EQ(msg->stamp, get_time(200, 0)); + EXPECT_EQ(msg->deadline_setting, get_duration(0, deadline_ms * 1000 * 1000)); + auto & sources = msg->sources; + EXPECT_EQ(sources.size(), 1u); + auto & s = sources[0]; + EXPECT_EQ(s.topic, "sensor"); + EXPECT_EQ(s.stamp, get_time(200, 0)); + EXPECT_EQ(s.elapsed, get_duration(0, deadline_ms * 1000 * 1000)); + EXPECT_TRUE(s.is_overrun); + }); + + auto spin = [tag_sender_node, deadline_detector_node, checker_node]() -> void { + rclcpp::spin_some(tag_sender_node); + rclcpp::spin_some(deadline_detector_node); + rclcpp::spin_some(checker_node); + }; + + // case1: not overrun + { + auto t1 = get_time(100, 0); + auto t2 = get_time(100, deadline_ms * 1000 * 1000 - 1); // ms to ns + tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor", t1, t1); + + tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); + add_input_info(next_tag, sensor_tag); + + sensor_pub->publish(sensor_tag); + spin(); + + next_pub->publish(next_tag); + spin(); + + EXPECT_FALSE(notification_called); + } + + // case2: overrun + { + auto t1 = get_time(200, 0); + auto t2 = get_time(200, deadline_ms * 1000 * 1000); // ms to ns + tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor", t1, t1); + + tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); + add_input_info(next_tag, sensor_tag); + + sensor_pub->publish(sensor_tag); + spin(); + + next_pub->publish(next_tag); + spin(); + + EXPECT_TRUE(notification_called); + + // we check deadline notification message in subscription callback + } +} + +TEST_F(TestTildeDeadlineDetectorNode, test_deadline_detection_multiple_input) +{ + auto tag_sender_node = std::make_shared("tag_sender_node"); + auto sensor1_pub = tag_sender_node->create_publisher( + "sensor1/message_tracking_tag", rclcpp::QoS(1).best_effort()); + auto sensor2_pub = tag_sender_node->create_publisher( + "sensor2/message_tracking_tag", rclcpp::QoS(1).best_effort()); + auto next_pub = tag_sender_node->create_publisher( + "next/message_tracking_tag", rclcpp::QoS(1).best_effort()); + + rclcpp::NodeOptions options; + options.append_parameter_override("target_topics", std::vector{"next"}); + int deadline_ms = 10; + options.append_parameter_override("deadline_ms", std::vector{deadline_ms}); + + auto deadline_detector_node = + std::make_shared("deadline_detector_node", options); + + auto checker_node = std::make_shared("checker_node"); + bool notification_called = false; + auto checker_sub = checker_node->create_subscription( + "deadline_notification", rclcpp::QoS(1).best_effort(), + [this, ¬ification_called, + deadline_ms](tilde_msg::msg::DeadlineNotification::UniquePtr msg) -> void { + notification_called = true; + EXPECT_EQ(msg->topic_name, "next"); + EXPECT_EQ(msg->stamp, get_time(200, 0)); + EXPECT_EQ(msg->deadline_setting, get_duration(0, deadline_ms * 1000 * 1000)); + + auto & sources = msg->sources; + EXPECT_EQ(sources.size(), 2u); + + size_t sensor1_idx = 0; + size_t sensor2_idx = 1; + if (sources[sensor1_idx].topic == "sensor2") { + sensor1_idx = 1; + sensor2_idx = 0; + } + + auto & s1 = sources[sensor1_idx]; + EXPECT_EQ(s1.topic, "sensor1"); + EXPECT_EQ(s1.stamp, get_time(200, 0)); + EXPECT_EQ(s1.elapsed, get_duration(0, deadline_ms * 1000 * 1000)); + EXPECT_TRUE(s1.is_overrun); + + auto & s2 = sources[sensor2_idx]; + EXPECT_EQ(s2.topic, "sensor2"); + EXPECT_EQ(s2.stamp, get_time(200, (deadline_ms / 2) * 1000 * 1000)); + EXPECT_EQ(s2.elapsed, get_duration(0, (deadline_ms / 2) * 1000 * 1000)); + EXPECT_FALSE(s2.is_overrun); + }); + + auto spin = [tag_sender_node, deadline_detector_node, checker_node]() -> void { + rclcpp::spin_some(tag_sender_node); + rclcpp::spin_some(deadline_detector_node); + rclcpp::spin_some(checker_node); + }; + + // case1: not overrun + { + auto t1 = get_time(100, 0); + auto t2 = get_time(100, deadline_ms * 1000 * 1000 - 1); // ms to ns + tilde_msg::msg::MessageTrackingTag sensor_tag = get_message_tracking_tag("sensor1", t1, t1); + + tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t2); + add_input_info(next_tag, sensor_tag); + + sensor1_pub->publish(sensor_tag); + spin(); + + next_pub->publish(next_tag); + spin(); + + EXPECT_FALSE(notification_called); + } + + // case2: overrun. There are 2 inputs, and sensor1 is overrun. + { + auto t1 = get_time(200, 0); + auto t2 = get_time(200, (deadline_ms / 2) * 1000 * 1000); + auto t3 = get_time(200, deadline_ms * 1000 * 1000); + + tilde_msg::msg::MessageTrackingTag sensor1_tag = get_message_tracking_tag("sensor1", t1, t1); + tilde_msg::msg::MessageTrackingTag sensor2_tag = get_message_tracking_tag("sensor2", t2, t2); + + tilde_msg::msg::MessageTrackingTag next_tag = get_message_tracking_tag("next", t1, t3); + add_input_info(next_tag, sensor1_tag); + add_input_info(next_tag, sensor2_tag); + + sensor1_pub->publish(sensor1_tag); + spin(); + + sensor2_pub->publish(sensor2_tag); + spin(); + + next_pub->publish(next_tag); + spin(); + + EXPECT_TRUE(notification_called); + + // we check deadline notification message in subscription callback + } +} 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..cef7ed70 --- /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/TILDE/tree/master/doc](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..1086bc92 --- /dev/null +++ b/src/tilde_early_deadline_detector/autoware_sensors.yaml @@ -0,0 +1,30 @@ +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..e9ef26a5 --- /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_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..fb0f85e9 --- /dev/null +++ b/src/tilde_early_deadline_detector/include/tilde_early_deadline_detector/tilde_early_deadline_detector_node.hpp @@ -0,0 +1,157 @@ +// 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_deadline_detector + +#endif // TILDE_EARLY_DEADLINE_DETECTOR__TILDE_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..f2c1e58a --- /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_msg + tilde_deadline_detector + + + 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..651211ed --- /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_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..2074c5f7 --- /dev/null +++ b/src/tilde_early_deadline_detector/src/tilde_early_deadline_detector_node.cpp @@ -0,0 +1,521 @@ +// 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 +#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!=0 || fp!=0 || fn!=0){ + accuracy = (tp + tn) / (tp + tn + fp + fn); + precision = tp / (tp + fp); + recall = tp / (tp + fn); + if(precision != 0 || 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_message_filters/CMakeLists.txt b/src/tilde_message_filters/CMakeLists.txt new file mode 100644 index 00000000..109d2c99 --- /dev/null +++ b/src/tilde_message_filters/CMakeLists.txt @@ -0,0 +1,145 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_message_filters) + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(tilde) +find_package(message_filters) +find_package(rclcpp_lifecycle REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +include_directories(include) +ament_export_include_directories(include) +install( + DIRECTORY include/ + DESTINATION include) + +add_library(sample_tilde_message_filter SHARED + src/sample_tilde_subscriber.cpp + src/sample_tilde_synchronizer.cpp + src/sample_sync_publisher.cpp) +ament_target_dependencies(sample_tilde_message_filter + "tilde" + "message_filters" + "rclcpp_components" + "std_msgs" + "sensor_msgs") +rclcpp_components_register_node(sample_tilde_message_filter + PLUGIN "sample_tilde_message_filter::SampleSubscriberWithHeader" + EXECUTABLE subscriber_with_header) +rclcpp_components_register_node(sample_tilde_message_filter + PLUGIN "sample_tilde_message_filter::SampleTildeSynchronizer2" + EXECUTABLE sample_synchronizer2) +rclcpp_components_register_node(sample_tilde_message_filter + PLUGIN "sample_tilde_message_filter::SampleTildeSynchronizer3" + EXECUTABLE sample_synchronizer3) +rclcpp_components_register_node(sample_tilde_message_filter + PLUGIN "sample_tilde_message_filter::SamplePublisherWithHeader" + EXECUTABLE publisher_with_header) + +install(TARGETS + sample_tilde_message_filter) +install( + TARGETS sample_tilde_message_filter EXPORT sample_tilde_message_filter + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +if(BUILD_TESTING) + find_package(tilde) + find_package(message_filters REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_lint_auto REQUIRED) + find_package(sensor_msgs REQUIRED) + find_package(std_msgs REQUIRED) + + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + # set(ament_cmake_cpplint_FOUND TRUE) + list(APPEND AMENT_LINT_AUTO_EXCLUDE + ament_cmake_cpplint + ament_cmake_cppcheck + ) + ament_lint_auto_find_test_dependencies() + + # we skip cpplint of borrowed files + find_package(ament_cmake_cpplint) + ament_cpplint(EXCLUDE + "test/test_from_original_subscriber.cpp" + "test/test_from_original_synchronizer.cpp") + + # TODO(y-okumura-isp): syntaxError but success to build & UT + find_package(ament_cmake_cppcheck) + ament_cppcheck(EXCLUDE + "tilde_subscriber.hpp" + ) + + ament_add_gtest(test_tilde_subscriber + test/test_tilde_subscriber.cpp) + target_include_directories(test_tilde_subscriber + PUBLIC + $ + $) + ament_target_dependencies(test_tilde_subscriber + "tilde" + "message_filters" + "sensor_msgs" + "std_msgs") + + ament_add_gtest(test_tilde_synchronizer + test/test_tilde_synchronizer.cpp) + target_include_directories(test_tilde_synchronizer + PUBLIC + $ + $) + ament_target_dependencies(test_tilde_synchronizer + "tilde" + "message_filters" + "sensor_msgs" + "std_msgs") + + ament_add_gtest(test_from_original_subscriber + test/test_from_original_subscriber.cpp) + target_include_directories(test_from_original_subscriber + PUBLIC + $ + $) + ament_target_dependencies(test_from_original_subscriber + "tilde" + "message_filters" + "rclcpp_lifecycle" + "sensor_msgs" + "std_msgs") + + ament_add_gtest(test_from_original_synchronizer + test/test_from_original_synchronizer.cpp) + target_include_directories(test_from_original_synchronizer + PUBLIC + $ + $) + ament_target_dependencies(test_from_original_synchronizer + "tilde" + "message_filters" + "sensor_msgs" + "std_msgs") +endif() + +ament_package() diff --git a/src/tilde_message_filters/README.md b/src/tilde_message_filters/README.md new file mode 100644 index 00000000..ee902df3 --- /dev/null +++ b/src/tilde_message_filters/README.md @@ -0,0 +1,51 @@ +## TODO + +- [ ] consider pointer from TildeSynchronizer or TildeSubscriber to TildeNode + - originally, it is not required + - shared_ptr could be cyclic pointers + - now raw pointer is used (weak_pointer may be a good replacement) + +## UT + +### compatibility test + +To check compatibility, we use the following tests: + +- test_from_original_subscriber.cpp +- test_from_original_synchronizer.cpp + +See bellow to know how to maintenance. + +#### test_subscriber.cpp + +- Manually copy & rename corresponding files +- Prepare TildeSubscriber + - add `#include "tilde_message_filters/tilde_subscriber.hpp"` + - add `using namespace tilde_message_filters;` + - replace `Subscriber` to `TildeSubscriber` +- Use `tilde::TildeNode` instead of `rclcpp::Node` + - add `#include "tilde/tilde_node.hpp"` + - replace `rclcpp::Node` to `tilde::TildeNode` +- Comment tests using LifecycleNode + - `TEST(TildeSubscriber, lifecycle)` on 20220421 + +#### test_synchronizer.cpp + +- Manually copy & rename corresponding files +- Prepare TildeSynchronizer + + - add `#include "tilde_message_filters/tilde_synchronizer.hpp"` + - add `using namespace tilde_message_filters;` + - replace `Synchronizer` to `TildeSynchronizer` of TEST + - don't change `typedef Synchronizer Sync;` in `struct NullPolicy` + - Add `TildeNode` to TildeSynchronizer constructors + - `nullptr` may work + - non-null instance is needed for message handling tests. + (add2, add3, and so on) + If null, the results are unexpected. + +- Use `tilde::TildeNode` instead of `rclcpp::Node` + - add `#include "tilde/tilde_node.hpp"` + - replace `rclcpp::Node` to `tilde::TildeNode` +- Comment tests using LifecycleNode + - `TEST(TildeSubscriber, lifecycle)` on 20220421 diff --git a/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp b/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp new file mode 100644 index 00000000..0209b7d0 --- /dev/null +++ b/src/tilde_message_filters/include/tilde_message_filters/tilde_subscriber.hpp @@ -0,0 +1,290 @@ +// 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_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ +#define TILDE_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ + +#include "message_filters/connection.h" +#include "message_filters/subscriber.h" +#include "tilde/tilde_node.hpp" + +#include +#include +#include + +namespace tilde_message_filters +{ +template +inline constexpr bool always_false_v = false; + +template +inline constexpr bool is_subscription_message() +{ + using ConstRef = const MessageT &; + using UniquePtr = std::unique_ptr; + using SharedConstPtr = std::shared_ptr; + using ConstRefSharedConstPtr = const std::shared_ptr &; + using SharedPtr = std::shared_ptr; + + return std::is_same_v || std::is_same_v || + std::is_same_v || std::is_same_v || + std::is_same_v; +} + +template +class TildeSubscriber +{ + using NodePtr = std::shared_ptr; + using MConstPtr = std::shared_ptr; + using EventType = message_filters::MessageEvent; + +public: + TildeSubscriber() = default; + + // cppcheck syntaxError + TildeSubscriber( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node, topic, qos); + } + + TildeSubscriber( + NodePtr node, const std::string & topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node, topic, qos); + } + + void subscribe( + NodePtr node, const std::string & topic, const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + subscribe(node.get(), topic, qos); + node_raw_ = nullptr; + node_shared_ = node; + } + + void subscribe( + NodeType * node, const std::string & topic, + const rmw_qos_profile_t qos = rmw_qos_profile_default) + { + node_raw_ = node; + subscriber_.subscribe(node, topic, qos); + subscriber_.registerCallback(&TildeSubscriber::register_message_callback, this); + } + + void subscribe() { subscriber_.subscribe(); } + + void unsubscribe() { subscriber_.unsubscribe(); } + + /// get non-FQDN topic name as in message_filter::Subscriber, + std::string getTopic() const { return subscriber_.getTopic(); } + + /// \sa message_filters::Subscriber::add() + void add(const EventType & e) { (void)e; } + + /// \sa message_filters::Subscriber::connectInput() + template + void connectInput(F & f) + { + (void)f; + } + + /* + // original callbacks + template + message_filters::Connection registerCallback(const C& callback) + { + return subscriber_.registerCallback(callback); + } + + template + message_filters::Connection registerCallback(const std::function& callback) + { + return subscriber_.registerCallback(callback); + } + + template + message_filters::Connection registerCallback(void(*callback)(P)) + { + return subscriber_.registerCallback(callback); + } + + template + message_filters::Connection registerCallback(void(T::*callback)(P), T* t) + { + return subscriber_.registerCallback(callback, t); + } + */ + + template < + typename C, typename CallbackArgT = + typename rclcpp::function_traits::function_traits::template argument_type<0>> + message_filters::Connection registerCallback(const C & callback) + { + const auto topic = getTopicFQDN(); + + auto new_callback = [this, callback, topic](CallbackArgT msg) -> void { + auto pnode = get_node(); + + // msg.get() in below codes: + // we can use msg.get() because + // - message_filters::simple_filter assumes that + // typename C should be compatible with std::function + // - MConstPtr is std::shared_ptr + + // on TildeSubscriber, default callback saves sub_time + rclcpp::Time sub_time, sub_time_steady; + pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); + + // update implicit input info + pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + + callback(msg); + }; + return subscriber_.registerCallback(new_callback); + } + + template > + message_filters::Connection registerCallback(const std::function & callback) + { + // We may use SFINAE instead of if constexpr to shorten this function. + // But SFINAE may change the entry point. On the other hand, `if constexpr` does not change it. + if constexpr (!is_subscription_message()) { + return subscriber_.registerCallback(callback); + } else { + const auto topic = getTopicFQDN(); + // As std::function use vtable, we use auto type, + // not std::function new_callback = ... + // https://stackoverflow.com/questions/25848690/should-i-use-stdfunction-or-a-function-pointer-in-c + auto new_callback = [this, callback, topic](P msg) -> void { + auto pnode = get_node(); + + // we use msg.get() because + // even in this pattern, + // P looks to be const shared_ptr (see message_filters::MessageEvent). + // TODO(y-okumura-isp): Is ConstRef also possible?? + + // on TildeSubscriber, default callback saves sub_time + rclcpp::Time sub_time, sub_time_steady; + pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); + + // update implicit input info + pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + + callback(msg); + }; + return subscriber_.registerCallback(new_callback); + } + } + + template + message_filters::Connection registerCallback(void (*callback)(P)) + { + const auto topic = getTopicFQDN(); + // We use lambda not original function pointer + // because we cannot convert captured lambda to function pointer. + // https://stackoverflow.com/questions/28746744/passing-capturing-lambda-as-function-pointer + auto new_callback = [this, callback, topic](P msg) -> void { + auto pnode = get_node(); + + // we use msg.get() because + // even in this pattern, + // P looks to be const shared_ptr (see message_filters::MessageEvent). + // TODO(y-okumura-isp): Is ConstRef also possible?? + + // on TildeSubscriber, default callback saves sub_time + rclcpp::Time sub_time, sub_time_steady; + pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); + + // update implicit input info + pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + + callback(msg); + }; + return subscriber_.registerCallback(new_callback); + } + + template + message_filters::Connection registerCallback(void (T::*callback)(P), T * t) + { + const auto topic = getTopicFQDN(); + auto bind_callback = std::bind(callback, t, std::placeholders::_1); + auto new_callback = [this, bind_callback, topic](P msg) -> void { + auto pnode = get_node(); + + // we use msg.get() because + // even in this pattern, + // P looks to be const shared_ptr (see message_filters::MessageEvent). + // TODO(y-okumura-isp): Can the msg type be ConstRef? + + // on TildeSubscriber, default callback saves sub_time + rclcpp::Time sub_time, sub_time_steady; + pnode->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); + + // update implicit input info + pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + + bind_callback(msg); + }; + // We need to wrap new_callback because P is sometimes non MConstPtr. + // If P is non MConstPtr, `registerCallback(const C& callback)` is called and + // subsequent `signal_.addCallback(Callback(callback))` fails because + // Callback = std::function. + // So we wrap new_callback by std::function, and use + // `registerCallback(const std::function& callback)`. + std::function new_callback2 = std::bind(new_callback, std::placeholders::_1); + return subscriber_.registerCallback(new_callback2); + } + +private: + NodePtr node_shared_; + NodeType * node_raw_{nullptr}; + message_filters::Subscriber subscriber_; + + NodeType * get_node() + { + if (node_shared_) { + return node_shared_.get(); + } else { + return node_raw_; + } + } + + /// get topic FQDN + std::string getTopicFQDN() + { + auto topic = subscriber_.getTopic(); + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(get_node()); + auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic); + return resolved_topic_name; + } + + /// TildeSubscriber 1st callback + /** + * Save sub_time for other callbacks or TildeSynchronizer + */ + void register_message_callback(const MConstPtr msg) + { + auto topic = getTopicFQDN(); + auto pnode = get_node(); + auto sub_time = pnode->now(); + auto sub_time_steady = pnode->get_steady_time(); + + pnode->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + } +}; +} // namespace tilde_message_filters + +#endif // TILDE_MESSAGE_FILTERS__TILDE_SUBSCRIBER_HPP_ diff --git a/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp b/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp new file mode 100644 index 00000000..50ff3ee4 --- /dev/null +++ b/src/tilde_message_filters/include/tilde_message_filters/tilde_synchronizer.hpp @@ -0,0 +1,1191 @@ +// 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_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ +#define TILDE_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ + +#include "message_filters/subscriber.h" +#include "message_filters/time_synchronizer.h" +#include "tilde/tilde_node.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" + +#include +#include +#include + +namespace tilde_message_filters +{ + +template +constexpr bool true_v = true; + +template +constexpr bool false_v = false; + +template +constexpr bool is_subscriber() +{ + if constexpr (std::is_same_v>) { + return true_v; + } + return false_v; +} + +template +class TildeSynchronizer +{ + using Sync = message_filters::Synchronizer; + + typedef typename Policy::Messages Messages; + + typedef typename std::tuple_element<0, Messages>::type M0; + typedef typename std::tuple_element<1, Messages>::type M1; + typedef typename std::tuple_element<2, Messages>::type M2; + typedef typename std::tuple_element<3, Messages>::type M3; + typedef typename std::tuple_element<4, Messages>::type M4; + typedef typename std::tuple_element<5, Messages>::type M5; + typedef typename std::tuple_element<6, Messages>::type M6; + typedef typename std::tuple_element<7, Messages>::type M7; + typedef typename std::tuple_element<8, Messages>::type M8; + +public: + template + TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1) : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + } + + template + TildeSynchronizer(tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1) : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + } + + template + TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2) : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + } + + template + TildeSynchronizer(tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + } + + template + TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3) : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + } + + template + TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + } + + template + TildeSynchronizer(tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5) + : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, + F5 & f5) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6) + : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, + F5 & f5, F6 & f6) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7) + : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6, f7); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + init_topic_name<7, M7>(f7); + } + + template + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, + F5 & f5, F6 & f6, F7 & f7) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6, f7); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + init_topic_name<7, M7>(f7); + } + + template < + class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> + TildeSynchronizer( + tilde::TildeNode * node, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, F5 & f5, F6 & f6, F7 & f7, + F8 & f8) + : node_(node) + { + sync_ptr_ = std::make_shared(f0, f1, f2, f3, f4, f5, f6, f7, f8); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + init_topic_name<7, M7>(f7); + init_topic_name<8, M8>(f8); + } + + template < + class F0, class F1, class F2, class F3, class F4, class F5, class F6, class F7, class F8> + TildeSynchronizer( + tilde::TildeNode * node, const Policy & policy, F0 & f0, F1 & f1, F2 & f2, F3 & f3, F4 & f4, + F5 & f5, F6 & f6, F7 & f7, F8 & f8) + : node_(node) + { + sync_ptr_ = std::make_shared(policy, f0, f1, f2, f3, f4, f5, f6, f7, f8); + init_topic_name<0, M0>(f0); + init_topic_name<1, M1>(f1); + init_topic_name<2, M2>(f2); + init_topic_name<3, M3>(f3); + init_topic_name<4, M4>(f4); + init_topic_name<5, M5>(f5); + init_topic_name<6, M6>(f6); + init_topic_name<7, M7>(f7); + init_topic_name<8, M8>(f8); + } + + explicit TildeSynchronizer(tilde::TildeNode * node) : node_(node) + { + sync_ptr_ = std::make_shared(); + } + + TildeSynchronizer(tilde::TildeNode * node, const Policy & policy) : node_(node) + { + sync_ptr_ = std::make_shared(policy); + } + + // (const C& callback) + template < + class C, std::size_t Arity = 2, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback](CallbackArgT0 msg0, CallbackArgT1 msg1) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + + callback(msg0, msg1); + }; + + return sync_ptr_->registerCallback( + std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); + } + + template < + class C, std::size_t Arity = 3, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + + callback(msg0, msg1, msg2); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } + + template < + class C, std::size_t Arity = 4, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + + callback(msg0, msg1, msg2, msg3); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4)); + } + + template < + class C, std::size_t Arity = 5, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + + callback(msg0, msg1, msg2, msg3, msg4); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + } + + template < + class C, std::size_t Arity = 6, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + + callback(msg0, msg1, msg2, msg3, msg4, msg5); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } + + template < + class C, std::size_t Arity = 7, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); + } + + template < + class C, std::size_t Arity = 8, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8)); + } + + template < + class C, std::size_t Arity = 9, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>, + typename CallbackArgT8 = + typename rclcpp::function_traits::function_traits::template argument_type<8>> + message_filters::Connection registerCallback(const C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7, CallbackArgT8 msg8) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + register_ith_message_as_input<8>(msg8); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8, std::placeholders::_9)); + } + + // non-const C callback& + template < + class C, std::size_t Arity = 2, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback](CallbackArgT0 msg0, CallbackArgT1 msg1) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + + callback(msg0, msg1); + }; + + return sync_ptr_->registerCallback( + std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); + } + + template < + class C, std::size_t Arity = 3, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + + callback(msg0, msg1, msg2); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } + + template < + class C, std::size_t Arity = 4, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + + callback(msg0, msg1, msg2, msg3); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4)); + } + + template < + class C, std::size_t Arity = 5, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + + callback(msg0, msg1, msg2, msg3, msg4); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + } + + template < + class C, std::size_t Arity = 6, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + + callback(msg0, msg1, msg2, msg3, msg4, msg5); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } + + template < + class C, std::size_t Arity = 7, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); + } + + template < + class C, std::size_t Arity = 8, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8)); + } + + template < + class C, std::size_t Arity = 9, + typename std::enable_if::value>::type * = + nullptr, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>, + typename CallbackArgT8 = + typename rclcpp::function_traits::function_traits::template argument_type<8>> + message_filters::Connection registerCallback(C & callback) + { + auto new_callback_lambda = [this, callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7, CallbackArgT8 msg8) { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + register_ith_message_as_input<8>(msg8); + + callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); + }; + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8, std::placeholders::_9)); + } + + // (const C& callback, T* t) + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 2, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind(callback, t, std::placeholders::_1, std::placeholders::_2); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + + bind_callback(msg0, msg1); + }; + + return sync_ptr_->registerCallback( + std::bind(new_callback_lambda, std::placeholders::_1, std::placeholders::_2)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 3, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = + std::bind(callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3); + auto new_callback_lambda = + [this, bind_callback](CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + + bind_callback(msg0, msg1, msg2); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 4, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + + bind_callback(msg0, msg1, msg2, msg3); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 5, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + + bind_callback(msg0, msg1, msg2, msg3, msg4); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 6, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, + CallbackArgT5 msg5) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + + bind_callback(msg0, msg1, msg2, msg3, msg4, msg5); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 7, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + + bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 8, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + + bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8)); + } + + template < + typename ReturnTypeT, typename... Args, typename T, typename C = ReturnTypeT(Args...), + std::size_t Arity = 9, typename std::enable_if::type = true, + typename CallbackArgT0 = + typename rclcpp::function_traits::function_traits::template argument_type<0>, + typename CallbackArgT1 = + typename rclcpp::function_traits::function_traits::template argument_type<1>, + typename CallbackArgT2 = + typename rclcpp::function_traits::function_traits::template argument_type<2>, + typename CallbackArgT3 = + typename rclcpp::function_traits::function_traits::template argument_type<3>, + typename CallbackArgT4 = + typename rclcpp::function_traits::function_traits::template argument_type<4>, + typename CallbackArgT5 = + typename rclcpp::function_traits::function_traits::template argument_type<5>, + typename CallbackArgT6 = + typename rclcpp::function_traits::function_traits::template argument_type<6>, + typename CallbackArgT7 = + typename rclcpp::function_traits::function_traits::template argument_type<7>, + typename CallbackArgT8 = + typename rclcpp::function_traits::function_traits::template argument_type<8>> + message_filters::Connection registerCallback(ReturnTypeT (T::*callback)(Args...), T * t) + { + auto bind_callback = std::bind( + callback, t, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8, std::placeholders::_9); + auto new_callback_lambda = [this, bind_callback]( + CallbackArgT0 msg0, CallbackArgT1 msg1, CallbackArgT2 msg2, + CallbackArgT3 msg3, CallbackArgT4 msg4, CallbackArgT5 msg5, + CallbackArgT6 msg6, CallbackArgT7 msg7, + CallbackArgT8 msg8) -> void { + register_ith_message_as_input<0>(msg0); + register_ith_message_as_input<1>(msg1); + register_ith_message_as_input<2>(msg2); + register_ith_message_as_input<3>(msg3); + register_ith_message_as_input<4>(msg4); + register_ith_message_as_input<5>(msg5); + register_ith_message_as_input<6>(msg6); + register_ith_message_as_input<7>(msg7); + register_ith_message_as_input<8>(msg8); + + bind_callback(msg0, msg1, msg2, msg3, msg4, msg5, msg6, msg7, msg8); + }; + + return sync_ptr_->registerCallback(std::bind( + new_callback_lambda, std::placeholders::_1, std::placeholders::_2, std::placeholders::_3, + std::placeholders::_4, std::placeholders::_5, std::placeholders::_6, std::placeholders::_7, + std::placeholders::_8, std::placeholders::_9)); + } + + // (C& callback, T* t) + // TODO(y-okumura-isp): implement me + +private: + std::shared_ptr sync_ptr_; + tilde::TildeNode * node_; + // message id vs topic name. + // 9 means the number of max messages. + std::vector topic_names_{9}; + + // helper function for topic name initialization. + // I: the index of message + // F: filter class such as Subscriber + // M: message type such as PointCloud2 + template + void init_topic_name(F & f) + { + // if constexpr (std::is_same_v>) { + if constexpr (std::is_same_v>) { + using rclcpp::node_interfaces::get_node_topics_interface; + auto node_topics_interface = get_node_topics_interface(node_); + auto topic_name = f.getTopic(); + auto resolved_topic_name = node_topics_interface->resolve_topic_name(topic_name); + // std::cout << "topic_name[ " << I << "]: "<< resolved_topic_name << std::endl; + topic_names_[I] = resolved_topic_name; + } + } + + // helper function for node->register_message_as_input + template + void register_ith_message_as_input(CallbackArgT msg) + { + static_assert(I < 9); + + using MessageT = typename std::tuple_element::type; + // TODO(y-okumura-isp): support custom deleter + using MessageDeleter = std::default_delete; + using ConstRef = const MessageT &; + using UniquePtr = std::unique_ptr; + using SharedConstPtr = std::shared_ptr; + using ConstRefSharedConstPtr = const std::shared_ptr &; + using SharedPtr = std::shared_ptr; + + const auto & topic = topic_names_[I]; + if (topic.empty()) { + return; + } + + rclcpp::Time sub_time, sub_time_steady; + if constexpr ( + std::is_same_v || std::is_same_v || + std::is_same_v || + std::is_same_v) { + node_->find_subscription_time(msg.get(), topic, sub_time, sub_time_steady); + + // update implicit input info + node_->register_message_as_input(msg.get(), topic, sub_time, sub_time_steady); + } else if constexpr (std::is_same_v) { + node_->find_subscription_time(&msg, topic, sub_time, sub_time_steady); + + // update implicit input info + node_->register_message_as_input(&msg, topic, sub_time, sub_time_steady); + } else { + // todo(y-okumura-isp): implement me + } + } +}; +} // namespace tilde_message_filters + +#endif // TILDE_MESSAGE_FILTERS__TILDE_SYNCHRONIZER_HPP_ diff --git a/src/tilde_message_filters/package.xml b/src/tilde_message_filters/package.xml new file mode 100644 index 00000000..5b524c7c --- /dev/null +++ b/src/tilde_message_filters/package.xml @@ -0,0 +1,24 @@ + + + + tilde_message_filters + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + message_filters + rclcpp_lifecycle + sensor_msgs + std_msgs + tilde + + ament_lint_auto + caret_lint_common + + + ament_cmake + + diff --git a/src/tilde_message_filters/src/sample_sync_publisher.cpp b/src/tilde_message_filters/src/sample_sync_publisher.cpp new file mode 100644 index 00000000..c07c2245 --- /dev/null +++ b/src/tilde_message_filters/src/sample_sync_publisher.cpp @@ -0,0 +1,150 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; + +namespace sample_tilde_message_filter +{ + +/** + * This class sends messages with header.stamp field. + * see msg.header.frame_id to get the sequence number. + */ +class SamplePublisherWithHeader : public rclcpp::Node +{ + using PublisherPtr = rclcpp::Publisher::SharedPtr; + +public: + explicit SamplePublisherWithHeader(const rclcpp::NodeOptions & options) + : Node("talker_with_header", options) + { + const std::string TIMER_MS = "timer_ms"; + const std::string N_PUBLISHERS = "n_publishers"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + + declare_parameter(N_PUBLISHERS, n_publishers_); + n_publishers_ = get_parameter(N_PUBLISHERS).get_value(); + assert(n_publishers_ >= 2); + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + for (auto i = 0; i < n_publishers_; i++) { + std::string topic = "in"; + topic += std::to_string(i); + pub_pcs_.push_back(this->create_publisher(topic, qos)); + } + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto publish_message = [this]() -> void { + auto time_now = this->now(); + + sensor_msgs::msg::PointCloud2 msg_pc; + msg_pc.header.stamp = time_now; + msg_pc.header.frame_id = std::to_string(count_); + + for (const auto & pub : pub_pcs_) { + pub->publish(msg_pc); + } + + RCLCPP_INFO( + this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, + time_now.nanoseconds()); + + count_++; + }; + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_message); + } + +private: + size_t count_ = 0; + int64_t n_publishers_ = 9; + std::vector pub_pcs_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +/** + * This class sends messages without header.stamp field. + */ +class SamplePublisherWithoutHeader : public rclcpp::Node +{ +public: + explicit SamplePublisherWithoutHeader(const rclcpp::NodeOptions & options) + : Node("talker_without_header", options) + { + const std::string TIMER_MS = "timer_ms"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto publish_message = [this]() -> void { + auto time_now = this->now(); + + msg_string_ = std::make_unique(); + msg_string_->data = std::to_string(count_); + pub_string_->publish(std::move(msg_string_)); + + RCLCPP_INFO( + this->get_logger(), "Publishing String: '%ld' at '%lu'", count_, time_now.nanoseconds()); + + count_++; + }; + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + pub_string_ = this->create_publisher("topic_without_header", qos); + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_message); + } + +private: + size_t count_ = 0; + + // message without standard header, especially header.stamp + std::unique_ptr msg_string_; + rclcpp::Publisher::SharedPtr pub_string_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace sample_tilde_message_filter + +RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SamplePublisherWithHeader) +RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SamplePublisherWithoutHeader) diff --git a/src/tilde_message_filters/src/sample_tilde_subscriber.cpp b/src/tilde_message_filters/src/sample_tilde_subscriber.cpp new file mode 100644 index 00000000..5926398c --- /dev/null +++ b/src/tilde_message_filters/src/sample_tilde_subscriber.cpp @@ -0,0 +1,164 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +typedef sensor_msgs::msg::PointCloud2 Msg; +typedef std::shared_ptr MsgConstPtr; +typedef std::shared_ptr MsgPtr; + +namespace sample_tilde_message_filter +{ +struct NonConstHelper +{ + explicit NonConstHelper(std::shared_ptr> pub) : pub_(pub) {} + + void cb(const MsgPtr msg) { pub_->publish(*msg); } + + MsgPtr msg_; + std::shared_ptr> pub_; +}; + +std::shared_ptr> g_pub_callback_fn_; + +void callback_fn(MsgConstPtr msg) { g_pub_callback_fn_->publish(*msg); } + +template +void func(CallbackT && callback) // add [[deprecated]] to show deduced type +{ + auto callback_addr = &callback; +} + +// Create a Talker class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class SampleSubscriberWithHeader : public tilde::TildeNode +{ +public: + explicit SampleSubscriberWithHeader(const rclcpp::NodeOptions & options) + : TildeNode("sub_with_header", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + + sub_pc_.subscribe(this, "in1", qos.get_rmw_qos_profile()); + + // lambda lvalue + // registerCallback(const C& callback) with C = sample_tilde_message_filter::..::(lambda)... + pub_lambda_lvalue_ = create_tilde_publisher("out_lambda_lvalue", 1); + auto sub_callback = [this](MsgConstPtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "sub_callback"); + // use reference because rclcpp::Publisher::publish does not have shared_ptr version + pub_lambda_lvalue_->publish(*msg); + }; + sub_pc_.registerCallback(sub_callback); + + // lambda rvalue + // const C& callback (be aware `C& callback` not defined) + pub_lambda_rvalue_ = create_tilde_publisher("out_lambda_rvalue", 1); + sub_pc_.registerCallback([this](MsgConstPtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "rvalue lambda"); + pub_lambda_rvalue_->publish(*msg); + }); + + /* type check + std::cout << &sub_callback << std::endl; + func(sub_callback); + func([this](MsgConstPtr msg) -> void + { + (void) msg; + RCLCPP_INFO(this->get_logger(), "rvalue lambda"); + }); + func([this](MsgConstPtr msg) -> void + { + (void) msg; + RCLCPP_INFO(this->get_logger(), "rvalue lambda"); + }); + */ + + // std::function + // registerCallback(const std::function& callback) + pub_callback2_ = create_tilde_publisher("out_std_function_lvalue", 1); + std::function std_func = + std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1); + sub_pc_.registerCallback(std_func); + + /* + * We can not use unique_ptr because message_event.h does not accept + * registerCallback(const std::function& callback) + */ + // std::function)> std_func_unique_ptr = + // std::bind(&SampleSubscriberWithHeader::callback_unique_ptr, this, std::placeholders::_1); + // sub_pc_.registerCallback(std_func_unique_ptr); + + // std::bind rvalue + // const C& callback + sub_pc_.registerCallback( + std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1)); + + // std::bind lvalue but use auto + // const C& callback. => with C = std::_Bind + auto auto_func = std::bind(&SampleSubscriberWithHeader::callback2, this, std::placeholders::_1); + sub_pc_.registerCallback(auto_func); + + // registerCallback(void(*callback)(P)) + // void(*callback)(P) + g_pub_callback_fn_ = create_tilde_publisher("out_callback_fn", 1); + sub_pc_.registerCallback(callback_fn); + + // registerCallback(void(T::*callback)(P), T* t) + pub_non_const_helper_ = create_tilde_publisher("out_non_const_helper", 1); + non_const_helper_ = std::make_shared(pub_non_const_helper_); + sub_pc_.registerCallback(&NonConstHelper::cb, non_const_helper_.get()); + } + +private: + tilde_message_filters::TildeSubscriber sub_pc_; + std::shared_ptr> pub_lambda_lvalue_; + std::shared_ptr> pub_lambda_rvalue_; + std::shared_ptr> pub_std_function_lvalue; + std::shared_ptr> pub_callback2_; + std::shared_ptr> pub_non_const_helper_; + std::shared_ptr non_const_helper_; + + void callback2(MsgConstPtr msg) + { + RCLCPP_INFO(this->get_logger(), "callback2"); + pub_callback2_->publish(*msg); + } + + void callback_unique_ptr(std::unique_ptr msg) + { + RCLCPP_INFO(this->get_logger(), "callback_unique_ptr"); + pub_callback2_->publish(std::move(msg)); + } + + MsgPtr msg_; +}; + +} // namespace sample_tilde_message_filter + +RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleSubscriberWithHeader) diff --git a/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp b/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp new file mode 100644 index 00000000..e35a1e3f --- /dev/null +++ b/src/tilde_message_filters/src/sample_tilde_synchronizer.cpp @@ -0,0 +1,114 @@ +// 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 "message_filters/sync_policies/exact_time.h" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" +#include "tilde_message_filters/tilde_synchronizer.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include + +typedef sensor_msgs::msg::PointCloud2 Msg; +typedef std::shared_ptr MsgConstPtr; +typedef std::shared_ptr MsgPtr; + +namespace sample_tilde_message_filter +{ +// 2 means the callback argc to simplify the template programming +class SampleTildeSynchronizer2 : public tilde::TildeNode +{ + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = tilde_message_filters::TildeSynchronizer; + using Subscriber = message_filters::Subscriber; + using Publisher = tilde::TildePublisher::SharedPtr; + +public: + explicit SampleTildeSynchronizer2(const rclcpp::NodeOptions & options) + : TildeNode("sample_tilde_sync2", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + auto rmw_qos = qos.get_rmw_qos_profile(); + + sub_pc1_.subscribe(this, "in1", rmw_qos); + sub_pc2_.subscribe(this, "in2", rmw_qos); + + sync_ptr_ = std::make_shared(this, SyncPolicy(5), sub_pc1_, sub_pc2_); + + pub_ = create_tilde_publisher("out2", 1); + + // registerCallback(const C& callback) version: + // <- (const C&) can bind rvalue + sync_ptr_->registerCallback(std::bind( + &SampleTildeSynchronizer2::sub_callback, this, std::placeholders::_1, std::placeholders::_2)); + } + +private: + Subscriber sub_pc1_, sub_pc2_; + std::shared_ptr sync_ptr_; + Publisher pub_; + + void sub_callback(const MsgConstPtr & msg1, const MsgConstPtr & msg2) + { + (void)msg2; + pub_->publish(*msg1); + } +}; + +// 3 means the callback argc to simplify the template programming +class SampleTildeSynchronizer3 : public tilde::TildeNode +{ + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = tilde_message_filters::TildeSynchronizer; + using Subscriber = message_filters::Subscriber; + +public: + explicit SampleTildeSynchronizer3(const rclcpp::NodeOptions & options) + : TildeNode("sample_tilde_sync3", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + auto rmw_qos = qos.get_rmw_qos_profile(); + + sub_pc1_.subscribe(this, "in1", rmw_qos); + sub_pc2_.subscribe(this, "in2", rmw_qos); + sub_pc3_.subscribe(this, "in3", rmw_qos); + + sync_ptr_ = std::make_shared(this, SyncPolicy(5), sub_pc1_, sub_pc2_, sub_pc3_); + + // registerCallback(const C& callback) version: + // <- (const C&) can bind rvalue + sync_ptr_->registerCallback(std::bind( + &SampleTildeSynchronizer3::sub_callback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3)); + } + +private: + Subscriber sub_pc1_, sub_pc2_, sub_pc3_; + std::shared_ptr sync_ptr_; + + void sub_callback(const MsgConstPtr & msg1, const MsgConstPtr & msg2, const MsgConstPtr & msg3) + { + (void)msg1; + (void)msg2; + (void)msg3; + } +}; + +} // namespace sample_tilde_message_filter + +RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleTildeSynchronizer2) +RCLCPP_COMPONENTS_REGISTER_NODE(sample_tilde_message_filter::SampleTildeSynchronizer3) diff --git a/src/tilde_message_filters/test/test_from_original_subscriber.cpp b/src/tilde_message_filters/test/test_from_original_subscriber.cpp new file mode 100644 index 00000000..aef89254 --- /dev/null +++ b/src/tilde_message_filters/test/test_from_original_subscriber.cpp @@ -0,0 +1,307 @@ +// 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. +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include + +// see ros2/rclcpp#1619,1713 +// TODO: remove this comment, and the `NonConstHelper` tests // NOLINT +// once the deprecated signatures have been discontinued. +#define RCLCPP_AVOID_DEPRECATIONS_FOR_UNIT_TESTS 1 +#include "message_filters/chain.h" +#include "tilde/tilde_node.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" + +#include +#include + +#include "sensor_msgs/msg/imu.hpp" + +using namespace message_filters; // NOLINT +using namespace tilde_message_filters; // NOLINT +typedef sensor_msgs::msg::Imu Msg; +typedef std::shared_ptr MsgConstPtr; +typedef std::shared_ptr MsgPtr; + +class Helper +{ +public: + Helper() : count_(0) {} + + void cb(const MsgConstPtr) { ++count_; } + + int32_t count_; +}; + +TEST(TildeSubscriber, simple) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +TEST(TildeSubscriber, simple_raw) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node.get(), "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +TEST(TildeSubscriber, sub_UnSub_Sub) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + + sub.unsubscribe(); + sub.subscribe(); + + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +TEST(TildeSubscriber, sub_UnSub_Sub_raw) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node.get(), "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + + sub.unsubscribe(); + sub.subscribe(); + + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +TEST(TildeSubscriber, switchRawAndShared) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic2", 10); + + sub.unsubscribe(); + sub.subscribe(node.get(), "test_topic2"); + + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +TEST(TildeSubscriber, subInChain) +{ + auto node = std::make_shared("test_node"); + Helper h; + Chain c; + c.addFilter(std::make_shared >(node, "test_topic")); + c.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + } + + ASSERT_GT(h.count_, 0); +} + +struct ConstHelper +{ + void cb(const MsgConstPtr msg) { msg_ = msg; } + + MsgConstPtr msg_; +}; + +struct NonConstHelper +{ + void cb(const MsgPtr msg) { msg_ = msg; } + + MsgPtr msg_; +}; + +TEST(TildeSubscriber, singleNonConstCallback) +{ + auto node = std::make_shared("test_node"); + NonConstHelper h; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(&NonConstHelper::cb, &h); + auto pub = node->create_publisher("test_topic", 10); + Msg msg; + pub->publish(Msg()); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + + ASSERT_TRUE(h.msg_); + ASSERT_EQ(msg, *h.msg_.get()); +} + +TEST(TildeSubscriber, multipleNonConstCallbacksFilterTildeSubscriber) +{ + auto node = std::make_shared("test_node"); + NonConstHelper h, h2; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(&NonConstHelper::cb, &h); + sub.registerCallback(&NonConstHelper::cb, &h2); + auto pub = node->create_publisher("test_topic", 10); + auto msg = std::make_unique(); + pub->publish(std::move(msg)); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + + ASSERT_TRUE(h.msg_); + ASSERT_TRUE(h2.msg_); + EXPECT_NE(msg.get(), h.msg_.get()); + EXPECT_NE(msg.get(), h2.msg_.get()); + EXPECT_NE(h.msg_.get(), h2.msg_.get()); +} + +TEST(TildeSubscriber, multipleCallbacksSomeFilterSomeDirect) +{ + auto node = std::make_shared("test_node"); + NonConstHelper h, h2; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(&NonConstHelper::cb, &h); + auto sub2 = node->create_subscription( + "test_topic", 10, std::bind(&NonConstHelper::cb, &h2, std::placeholders::_1)); + + auto pub = node->create_publisher("test_topic", 10); + auto msg = std::make_unique(); + pub->publish(std::move(msg)); + + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node); + + ASSERT_TRUE(h.msg_); + ASSERT_TRUE(h2.msg_); + EXPECT_NE(msg.get(), h.msg_.get()); + EXPECT_NE(msg.get(), h2.msg_.get()); + EXPECT_NE(h.msg_.get(), h2.msg_.get()); +} + +/* +TEST(TildeSubscriber, lifecycle) +{ + auto node = std::make_shared("test_node"); + Helper h; + TildeSubscriber sub(node, "test_topic"); + sub.registerCallback(std::bind(&Helper::cb, &h, std::placeholders::_1)); + auto pub = node->create_publisher("test_topic", 10); + pub->on_activate(); + rclcpp::Clock ros_clock; + auto start = ros_clock.now(); + while (h.count_ == 0 && (ros_clock.now() - start) < rclcpp::Duration(1, 0)) + { + pub->publish(Msg()); + rclcpp::Rate(50).sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + } + + ASSERT_GT(h.count_, 0); +} +*/ + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + + rclcpp::init(argc, argv); + + return RUN_ALL_TESTS(); +} diff --git a/src/tilde_message_filters/test/test_from_original_synchronizer.cpp b/src/tilde_message_filters/test/test_from_original_synchronizer.cpp new file mode 100644 index 00000000..7be82ea4 --- /dev/null +++ b/src/tilde_message_filters/test/test_from_original_synchronizer.cpp @@ -0,0 +1,517 @@ +// 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. +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2008, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the Willow Garage nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include "tilde_message_filters/tilde_synchronizer.hpp" + +#include + +#include + +#include + +using namespace message_filters; // NOLINT +using namespace tilde_message_filters; // NOLINT +using namespace std::placeholders; // NOLINT + +struct Header +{ + rclcpp::Time stamp; +}; + +struct Msg +{ + Header header; + int data; +}; +typedef std::shared_ptr MsgPtr; +typedef std::shared_ptr MsgConstPtr; + +template < + typename M0, typename M1, typename M2 = NullType, typename M3 = NullType, typename M4 = NullType, + typename M5 = NullType, typename M6 = NullType, typename M7 = NullType, typename M8 = NullType> +struct NullPolicy : public PolicyBase +{ + typedef Synchronizer Sync; + typedef PolicyBase Super; + typedef typename Super::Messages Messages; + typedef typename Super::Signal Signal; + typedef typename Super::Events Events; + typedef typename Super::RealTypeCount RealTypeCount; + + NullPolicy() + { + for (int i = 0; i < RealTypeCount::value; ++i) { + added_[i] = 0; + } + } + + void initParent(Sync *) {} + + template + void add(const typename std::tuple_element::type &) + { + ++added_.at(i); + } + + std::array added_; +}; +typedef NullPolicy Policy2; +typedef NullPolicy Policy3; +typedef NullPolicy Policy4; +typedef NullPolicy Policy5; +typedef NullPolicy Policy6; +typedef NullPolicy Policy7; +typedef NullPolicy Policy8; +typedef NullPolicy Policy9; + +TEST(TildeSynchronizer, compile2) +{ + NullFilter f0, f1; + TildeSynchronizer sync(nullptr, f0, f1); +} + +TEST(TildeSynchronizer, compile3) +{ + NullFilter f0, f1, f2; + TildeSynchronizer sync(nullptr, f0, f1, f2); +} + +TEST(TildeSynchronizer, compile4) +{ + NullFilter f0, f1, f2, f3; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3); +} + +TEST(TildeSynchronizer, compile5) +{ + NullFilter f0, f1, f2, f3, f4; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4); +} + +TEST(TildeSynchronizer, compile6) +{ + NullFilter f0, f1, f2, f3, f4, f5; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5); +} + +TEST(TildeSynchronizer, compile7) +{ + NullFilter f0, f1, f2, f3, f4, f5, f6; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6); +} + +TEST(TildeSynchronizer, compile8) +{ + NullFilter f0, f1, f2, f3, f4, f5, f6, f7; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6, f7); +} + +TEST(TildeSynchronizer, compile9) +{ + NullFilter f0, f1, f2, f3, f4, f5, f6, f7, f8; + TildeSynchronizer sync(nullptr, f0, f1, f2, f3, f4, f5, f6, f7, f8); +} + +void function2(const MsgConstPtr &, const MsgConstPtr &) {} +void function3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} +void function4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) +{ +} +void function6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function8( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) +{ +} +void function9( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const MessageEvent &, const MessageEvent &, const MsgConstPtr &) +{ +} + +TEST(TildeSynchronizer, compileFunction2) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function2); +} + +TEST(TildeSynchronizer, compileFunction3) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function3); +} + +TEST(TildeSynchronizer, compileFunction4) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function4); +} + +TEST(TildeSynchronizer, compileFunction5) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function5); +} + +TEST(TildeSynchronizer, compileFunction6) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function6); +} + +TEST(TildeSynchronizer, compileFunction7) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function7); +} + +TEST(TildeSynchronizer, compileFunction8) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function8); +} + +/* +TEST(TildeSynchronizer, compileFunction9) +{ + TildeSynchronizer sync(nullptr); + sync.registerCallback(function9); +} +*/ + +struct MethodHelper +{ + void method2(const MsgConstPtr &, const MsgConstPtr &) {} + void method3(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) {} + void method4(const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) + { + } + void method5( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &) + { + } + void method6( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &) + { + } + void method7( + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &, + const MsgConstPtr &, const MsgConstPtr &, const MsgConstPtr &) + { + } + void method8( + const MsgConstPtr &, MsgConstPtr, const MsgPtr &, MsgPtr, const Msg &, Msg, + const MessageEvent &, const MessageEvent &) + { + } + // Can only do 8 here because the object instance counts as a parameter and bind only supports 9 +}; + +TEST(TildeSynchronizer, compileMethod2) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method2, &h); +} + +TEST(TildeSynchronizer, compileMethod3) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method3, &h); +} + +TEST(TildeSynchronizer, compileMethod4) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method4, &h); +} + +TEST(TildeSynchronizer, compileMethod5) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method5, &h); +} + +TEST(TildeSynchronizer, compileMethod6) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method6, &h); +} + +TEST(TildeSynchronizer, compileMethod7) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method7, &h); +} + +/// cannot build this. too many placeholders? +/* +TEST(TildeSynchronizer, compileMethod8) +{ + MethodHelper h; + TildeSynchronizer sync(nullptr); + sync.registerCallback(&MethodHelper::method8, &h); +} +*/ + +/// add() or cb() are called internally, +/// so we can skip these tests +/* +TEST(TildeSynchronizer, add2) +{ + TildeSynchronizer sync(nullptr); + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); +} + +TEST(TildeSynchronizer, add3) +{ + TildeSynchronizer sync(nullptr); + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); +} + +TEST(TildeSynchronizer, add4) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); +} + +TEST(TildeSynchronizer, add5) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); + ASSERT_EQ(sync.added_[4], 0); + sync.add<4>(m); + ASSERT_EQ(sync.added_[4], 1); +} + +TEST(TildeSynchronizer, add6) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); + ASSERT_EQ(sync.added_[4], 0); + sync.add<4>(m); + ASSERT_EQ(sync.added_[4], 1); + ASSERT_EQ(sync.added_[5], 0); + sync.add<5>(m); + ASSERT_EQ(sync.added_[5], 1); +} + +TEST(TildeSynchronizer, add7) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); + ASSERT_EQ(sync.added_[4], 0); + sync.add<4>(m); + ASSERT_EQ(sync.added_[4], 1); + ASSERT_EQ(sync.added_[5], 0); + sync.add<5>(m); + ASSERT_EQ(sync.added_[5], 1); + ASSERT_EQ(sync.added_[6], 0); + sync.add<6>(m); + ASSERT_EQ(sync.added_[6], 1); +} + +TEST(TildeSynchronizer, add8) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); + ASSERT_EQ(sync.added_[4], 0); + sync.add<4>(m); + ASSERT_EQ(sync.added_[4], 1); + ASSERT_EQ(sync.added_[5], 0); + sync.add<5>(m); + ASSERT_EQ(sync.added_[5], 1); + ASSERT_EQ(sync.added_[6], 0); + sync.add<6>(m); + ASSERT_EQ(sync.added_[6], 1); + ASSERT_EQ(sync.added_[7], 0); + sync.add<7>(m); + ASSERT_EQ(sync.added_[7], 1); +} + +TEST(TildeSynchronizer, add9) +{ + TildeSynchronizer sync; + MsgPtr m(std::make_shared()); + + ASSERT_EQ(sync.added_[0], 0); + sync.add<0>(m); + ASSERT_EQ(sync.added_[0], 1); + ASSERT_EQ(sync.added_[1], 0); + sync.add<1>(m); + ASSERT_EQ(sync.added_[1], 1); + ASSERT_EQ(sync.added_[2], 0); + sync.add<2>(m); + ASSERT_EQ(sync.added_[2], 1); + ASSERT_EQ(sync.added_[3], 0); + sync.add<3>(m); + ASSERT_EQ(sync.added_[3], 1); + ASSERT_EQ(sync.added_[4], 0); + sync.add<4>(m); + ASSERT_EQ(sync.added_[4], 1); + ASSERT_EQ(sync.added_[5], 0); + sync.add<5>(m); + ASSERT_EQ(sync.added_[5], 1); + ASSERT_EQ(sync.added_[6], 0); + sync.add<6>(m); + ASSERT_EQ(sync.added_[6], 1); + ASSERT_EQ(sync.added_[7], 0); + sync.add<7>(m); + ASSERT_EQ(sync.added_[7], 1); + ASSERT_EQ(sync.added_[8], 0); + sync.add<8>(m); + ASSERT_EQ(sync.added_[8], 1); +} +*/ +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/src/tilde_message_filters/test/test_tilde_subscriber.cpp b/src/tilde_message_filters/test/test_tilde_subscriber.cpp new file mode 100644 index 00000000..b5ae4a62 --- /dev/null +++ b/src/tilde_message_filters/test/test_tilde_subscriber.cpp @@ -0,0 +1,178 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include + +#include +#include +#include + +using TildeNode = tilde::TildeNode; +template +using TildePublisher = tilde::TildePublisher; +template +using TildeSubscriber = tilde_message_filters::TildeSubscriber; + +using Node = rclcpp::Node; +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointCloud2Ptr = std::shared_ptr; +using PointCloud2ConstPtr = std::shared_ptr; + +using Clock = rosgraph_msgs::msg::Clock; +using MessageTrackingTag = tilde_msg::msg::MessageTrackingTag; +using MessageTrackingTagPtr = MessageTrackingTag::UniquePtr; +using TimeMsg = builtin_interfaces::msg::Time; + +using PointCloudPublisher = std::shared_ptr>; +using PointCloudPublishers = std::vector; +using PointCloudTildeSubscriber = TildeSubscriber; +using PointCloudTildeSubscribers = std::vector; + +class TestTildeSubscriber : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + options.append_parameter_override("use_sim_time", true); + qos.keep_last(10); + + // setup pub node + pub_node = std::make_shared("pub_node", options); + for (auto i = 0u; i < pubs.size(); i++) { + auto topic = std::string("in") + std::to_string(i + 1); + pubs[i] = pub_node->create_publisher(topic, qos); + } + clock_pub = pub_node->create_publisher("/clock", qos); + + // setup sub node + sub_node = std::make_shared("sub_node", options); + // skip init subs. Get subs via init_and_get_sub(). + out_pub = sub_node->create_tilde_publisher("out", qos); + + // setup val node + val_node = std::make_shared("val_node", options); + } + + void TearDown() override { rclcpp::shutdown(); } + + Clock send_clock(int32_t sec, uint32_t nsec) + { + Clock clock_msg; + clock_msg.clock.sec = sec; + clock_msg.clock.nanosec = nsec; + clock_pub->publish(clock_msg); + return clock_msg; + } + + Clock send_clock_and_spin(int32_t sec, uint32_t nsec) + { + auto clock = send_clock(sec, nsec); + rclcpp::Rate(50).sleep(); + spin(); + return clock; + } + + void spin() + { + rclcpp::spin_some(pub_node); + rclcpp::spin_some(sub_node); + rclcpp::spin_some(val_node); + } + + rclcpp::NodeOptions options; + rclcpp::QoS qos{10}; + + std::shared_ptr pub_node; + PointCloudPublishers pubs{9}; + std::shared_ptr> clock_pub; + + std::shared_ptr sub_node; + PointCloudTildeSubscriber sub; + std::shared_ptr> out_pub; + + std::shared_ptr val_node; +}; + +void EXPECT_CLOCK(rclcpp::Time time, int sec, uint32_t nsec) +{ + TimeMsg t = time; + EXPECT_EQ(t.sec, sec); + EXPECT_EQ(t.nanosec, nsec); +} + +TEST_F(TestTildeSubscriber, no_arg_constructor) +{ + auto ts = TildeSubscriber(); + SUCCEED(); +} + +TEST_F(TestTildeSubscriber, shared_ptr_constructor) +{ + auto ts = TildeSubscriber(sub_node, "topic"); + SUCCEED(); +} + +TEST_F(TestTildeSubscriber, ptr_constructor) +{ + auto ts = TildeSubscriber(sub_node.get(), "topic"); + SUCCEED(); +} + +TEST_F(TestTildeSubscriber, hold_sub_time) +{ + auto & pub1 = pubs[0]; + sub.subscribe(sub_node, "in1", qos.get_rmw_qos_profile()); + + // time + const int32_t t1_sec{1}, t2_sec{1}, t3_sec{2}; + const uint32_t t1_nsec{50}, t2_nsec{100}, t3_nsec{150}; + + // PointCloud2 message to publish + PointCloud2 msg; + + // t=t1 + send_clock(t1_sec, t1_nsec); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), t1_sec, t1_nsec); + + // in1: stamp=t2 @ t1 + msg.header.stamp.sec = t2_sec; + msg.header.stamp.nanosec = t2_nsec; + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // t=t3 + send_clock(t3_sec, t3_nsec); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), t3_sec, t3_nsec); + + // check + rclcpp::Time sub_time, sub_time_steady; + sub_node->find_subscription_time(&msg, "/in1", sub_time, sub_time_steady); + (void)sub_time_steady; + EXPECT_CLOCK(sub_time, t1_sec, t1_nsec); +} diff --git a/src/tilde_message_filters/test/test_tilde_synchronizer.cpp b/src/tilde_message_filters/test/test_tilde_synchronizer.cpp new file mode 100644 index 00000000..e9736a28 --- /dev/null +++ b/src/tilde_message_filters/test/test_tilde_synchronizer.cpp @@ -0,0 +1,489 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "tilde_message_filters/tilde_subscriber.hpp" +#include "tilde_message_filters/tilde_synchronizer.hpp" + +#include "rosgraph_msgs/msg/clock.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include + +#include +#include +#include +#include + +template +using Subscriber = message_filters::Subscriber; +template +using PassThrough = message_filters::PassThrough; + +using TildeNode = tilde::TildeNode; +template +using TildePublisher = tilde::TildePublisher; +template +using TildeSubscriber = tilde_message_filters::TildeSubscriber; +template +using TildeSynchronizer = tilde_message_filters::TildeSynchronizer; + +using Node = rclcpp::Node; +using PointCloud2 = sensor_msgs::msg::PointCloud2; +using PointCloud2Ptr = std::shared_ptr; +using PointCloud2ConstPtr = std::shared_ptr; + +using Clock = rosgraph_msgs::msg::Clock; +using MessageTrackingTag = tilde_msg::msg::MessageTrackingTag; +using MessageTrackingTagPtr = MessageTrackingTag::UniquePtr; +using TimeMsg = builtin_interfaces::msg::Time; + +using PointCloudPublisher = std::shared_ptr>; +using PointCloudPublishers = std::vector; +using PointCloudTildeSubscriber = TildeSubscriber; +using PointCloudTildeSubscribers = std::vector; + +class TestSynchronizer : public ::testing::Test +{ +public: + void SetUp() override + { + rclcpp::init(0, nullptr); + + options.append_parameter_override("use_sim_time", true); + qos.keep_last(10); + + // setup pub node + pub_node = std::make_shared("pub_node", options); + for (auto i = 0u; i < subs.size(); i++) { + auto topic = std::string("in") + std::to_string(i + 1); + pubs.push_back(pub_node->create_publisher(topic, qos)); + } + clock_pub = pub_node->create_publisher("/clock", qos); + + // setup sub node + sub_node = std::make_shared("sub_node", options); + // skip init subs. Get subs via init_and_get_sub(). + out_pub = sub_node->create_tilde_publisher("out", qos); + + // setup val node + val_node = std::make_shared("val_node", options); + } + + void TearDown() override { rclcpp::shutdown(); } + + Clock send_clock(int32_t sec, uint32_t nsec) + { + Clock clock_msg; + clock_msg.clock.sec = sec; + clock_msg.clock.nanosec = nsec; + clock_pub->publish(clock_msg); + return clock_msg; + } + + Clock send_clock_and_spin(int32_t sec, uint32_t nsec) + { + auto clock = send_clock(sec, nsec); + rclcpp::Rate(50).sleep(); + spin(); + return clock; + } + + void spin() + { + rclcpp::spin_some(pub_node); + rclcpp::spin_some(sub_node); + rclcpp::spin_some(val_node); + } + + template + PointCloudTildeSubscriber & init_and_get_sub() + { + static_assert(I < 9); + auto topic = std::string("in") + std::to_string(I + 1); + subs[I].subscribe(sub_node, topic, qos.get_rmw_qos_profile()); + return subs[I]; + } + + rclcpp::NodeOptions options; + rclcpp::QoS qos{10}; + + std::shared_ptr pub_node; + PointCloudPublishers pubs; + std::shared_ptr> clock_pub; + + std::shared_ptr sub_node; + PointCloudTildeSubscribers subs{9}; + std::shared_ptr> out_pub; + + std::shared_ptr val_node; +}; + +void EXPECT_CLOCK(rclcpp::Time time, int sec, uint32_t nsec) +{ + TimeMsg t = time; + EXPECT_EQ(t.sec, sec); + EXPECT_EQ(t.nanosec, nsec); +} + +TEST_F(TestSynchronizer, exact_policy_2msgs) +{ + auto pub1 = pubs[0]; + auto pub2 = pubs[1]; + auto & sub1 = init_and_get_sub<0>(); + auto & sub2 = init_and_get_sub<1>(); + + // setup sub_node synchronizer + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = TildeSynchronizer; + auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); + + bool sync_callback_called = false; + sync->registerCallback( + [this, &sync_callback_called]( + const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { + (void)msg1; + (void)msg2; + sync_callback_called = true; + out_pub->publish(*msg1); + }); + + // validation node + bool val_callback_called = false; + auto val_sub = val_node->create_subscription( + "out/message_tracking_tag", qos, + [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { + val_callback_called = true; + EXPECT_EQ(pi->output_info.header_stamp.sec, 123); + EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); + EXPECT_EQ(pi->input_infos.size(), 2u); + + std::map topic2idx; + for (size_t i = 0; i < pi->input_infos.size(); i++) { + topic2idx[pi->input_infos[i].topic_name] = i; + } + + EXPECT_EQ(topic2idx.size(), 2u); + for (size_t i = 1; i <= 2; i++) { + auto topic = std::string("/in") + std::to_string(i); + auto idx = topic2idx[topic]; + const auto & in_info = pi->input_infos[idx]; + + switch (i) { + case 1: + EXPECT_EQ(in_info.topic_name, "/in1"); + EXPECT_EQ(in_info.sub_time.sec, 123); + EXPECT_EQ(in_info.sub_time.nanosec, 456u); + break; + case 2: + EXPECT_EQ(in_info.topic_name, "/in2"); + EXPECT_EQ(in_info.sub_time.sec, 124); + EXPECT_EQ(in_info.sub_time.nanosec, 321u); + break; + default: + // never comes here + EXPECT_TRUE(false); + } + } + }); + + // apply "/clock" + send_clock(123, 456); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), 123, 456u); + + // PointCloud2 message to publish + PointCloud2 msg; + msg.header.stamp = pub_node->now(); + + // pub1 & sub + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // update clock + send_clock(124, 321); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), 124, 321); + + // pub2 + EXPECT_FALSE(val_callback_called); + msg.header.frame_id = 2; + pub2->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // verify + EXPECT_TRUE(sync_callback_called); + EXPECT_TRUE(val_callback_called); +} + +/// TildeSynchronizer only collaborates with TildeSynchronizer +TEST_F(TestSynchronizer, sub_and_tilde_sub) +{ + auto pub1 = pubs[0]; + auto pub2 = pubs[1]; + auto & sub1 = init_and_get_sub<0>(); + Subscriber sub2; + sub2.subscribe(sub_node, "in2", qos.get_rmw_qos_profile()); + + // setup sub_node synchronizer + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = TildeSynchronizer; + auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); + + bool sync_callback_called = false; + sync->registerCallback( + [this, &sync_callback_called]( + const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { + (void)msg1; + (void)msg2; + sync_callback_called = true; + out_pub->publish(*msg1); + }); + + // validation node + bool val_callback_called = false; + auto val_sub = val_node->create_subscription( + "out/message_tracking_tag", qos, + [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { + val_callback_called = true; + EXPECT_EQ(pi->output_info.header_stamp.sec, 123); + EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); + EXPECT_EQ(pi->input_infos.size(), 1u); + + for (const auto & in_info : pi->input_infos) { + if (in_info.topic_name == "/in1") { + EXPECT_EQ(in_info.sub_time.sec, 123); + EXPECT_EQ(in_info.sub_time.nanosec, 456u); + } else { + // never comes here + FAIL() << "invalid topic: " << in_info.topic_name; + } + } + }); + + // apply "/clock" + send_clock(123, 456); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), 123, 456u); + + // PointCloud2 message to publish + PointCloud2 msg; + msg.header.stamp = pub_node->now(); + + // pub1 & sub + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // update clock + send_clock(124, 321); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), 124, 321); + + // pub2 + EXPECT_FALSE(val_callback_called); + msg.header.frame_id = 2; + pub2->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // verify + EXPECT_TRUE(sync_callback_called); + EXPECT_TRUE(val_callback_called); +} + +/// ignore general filter type +TEST_F(TestSynchronizer, work_with_passthrough) +{ + // passthrough msg1 to msg2 + auto pub1 = pubs[0]; + auto & sub1 = init_and_get_sub<0>(); + PassThrough passthrough; + sub1.registerCallback( + [this, &passthrough](const PointCloud2ConstPtr msg) -> void { passthrough.add(msg); }); + + // setup sub_node synchronizer + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = TildeSynchronizer; + auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, passthrough); + + bool sync_callback_called = false; + sync->registerCallback( + [this, &sync_callback_called]( + const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { + (void)msg1; + (void)msg2; + sync_callback_called = true; + out_pub->publish(*msg1); + }); + + // validation node + bool val_callback_called = false; + auto val_sub = val_node->create_subscription( + "out/message_tracking_tag", qos, + [this, &val_callback_called](MessageTrackingTagPtr pi) -> void { + val_callback_called = true; + EXPECT_EQ(pi->output_info.header_stamp.sec, 123); + EXPECT_EQ(pi->output_info.header_stamp.nanosec, 456u); + EXPECT_EQ(pi->input_infos.size(), 1u); + + for (const auto & in_info : pi->input_infos) { + if (in_info.topic_name == "/in1") { + EXPECT_EQ(in_info.sub_time.sec, 123); + EXPECT_EQ(in_info.sub_time.nanosec, 456u); + } else { + // never comes here + FAIL() << "invalid topic: " << in_info.topic_name; + } + } + }); + + // apply "/clock" + send_clock(123, 456); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), 123, 456u); + + // PointCloud2 message to publish + PointCloud2 msg; + msg.header.stamp = pub_node->now(); + + // pub1 & sub + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // update clock + send_clock(124, 321); + spin(); + rclcpp::Rate(50).sleep(); + EXPECT_CLOCK(sub_node->now(), 124, 321); + + // verify + EXPECT_TRUE(sync_callback_called); + EXPECT_TRUE(val_callback_called); +} + +/// (in1.stamp=t2) -> (in1.stamp=t1) -> (in2.stamp=t2) +TEST_F(TestSynchronizer, order_inversion) +{ + auto pub1 = pubs[0]; + auto pub2 = pubs[1]; + auto & sub1 = init_and_get_sub<0>(); + auto & sub2 = init_and_get_sub<1>(); + + // time + const int32_t t1_sec{1}, t2_sec{1}, t3_sec{2}; + const uint32_t t1_nsec{50}, t2_nsec{100}, t3_nsec{150}; + + // setup sub_node synchronizer + using SyncPolicy = message_filters::sync_policies::ExactTime; + using Sync = TildeSynchronizer; + auto sync = std::make_shared(sub_node.get(), SyncPolicy(5), sub1, sub2); + + bool sync_callback_called = false; + sync->registerCallback( + [this, &sync_callback_called]( + const PointCloud2ConstPtr & msg1, const PointCloud2ConstPtr & msg2) -> void { + (void)msg1; + (void)msg2; + sync_callback_called = true; + out_pub->publish(*msg1); + }); + + bool val_callback_called = false; + auto val_sub = val_node->create_subscription( + "out/message_tracking_tag", qos, + [this, &val_callback_called, t1_sec, t2_sec, t3_sec, t1_nsec, t2_nsec, + t3_nsec](MessageTrackingTagPtr pi) -> void { + val_callback_called = true; + EXPECT_EQ(pi->output_info.header_stamp.sec, t2_sec); + EXPECT_EQ(pi->output_info.header_stamp.nanosec, t2_nsec); + EXPECT_EQ(pi->input_infos.size(), 2u); + + for (const auto & in_info : pi->input_infos) { + if (in_info.topic_name == "/in1") { + EXPECT_EQ(in_info.sub_time.sec, t1_sec); + EXPECT_EQ(in_info.sub_time.nanosec, t1_nsec); + } else if (in_info.topic_name == "/in2") { + EXPECT_EQ(in_info.sub_time.sec, t3_sec); + EXPECT_EQ(in_info.sub_time.nanosec, t3_nsec); + } else { + FAIL() << "invalid topic" << in_info.topic_name; + } + } + }); + + // PointCloud2 message to publish + PointCloud2 msg; + + // t=t1 + send_clock(t1_sec, t1_nsec); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), t1_sec, t1_nsec); + + // in1: stamp=t2 @ t1 + msg.header.stamp.sec = t2_sec; + msg.header.stamp.nanosec = t2_nsec; + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // t=t2 + send_clock(t2_sec, t2_nsec); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), t2_sec, t2_nsec); + + // in1: stamp=t1 @ t2 + EXPECT_FALSE(val_callback_called); + msg.header.stamp.sec = t1_sec; + msg.header.stamp.nanosec = t1_nsec; + msg.header.frame_id = 1; + pub1->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // t=t3 + send_clock(t3_sec, t3_nsec); + rclcpp::Rate(50).sleep(); + spin(); + EXPECT_CLOCK(sub_node->now(), t3_sec, t3_nsec); + + // in2: stamp=t2 @ t3 + EXPECT_FALSE(val_callback_called); + msg.header.stamp.sec = t2_sec; + msg.header.stamp.nanosec = t2_nsec; + msg.header.frame_id = 2; + pub2->publish(msg); + rclcpp::Rate(50).sleep(); + spin(); + + // verify + EXPECT_TRUE(sync_callback_called); + EXPECT_TRUE(val_callback_called); +} diff --git a/src/tilde_msg/CMakeLists.txt b/src/tilde_msg/CMakeLists.txt new file mode 100644 index 00000000..a0064fd2 --- /dev/null +++ b/src/tilde_msg/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_msg) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +find_package(autoware_auto_geometry_msgs REQUIRED) +find_package(autoware_auto_perception_msgs REQUIRED) +find_package(autoware_auto_planning_msgs REQUIRED) +find_package(autoware_auto_control_msgs REQUIRED) + +set(msg_files + "msg/SubTopicTimeInfo.msg" + "msg/PubTopicTimeInfo.msg" + "msg/MessageTrackingTag.msg" + "msg/TestTopLevelStamp.msg" + "msg/Source.msg" + "msg/DeadlineNotification.msg" + + "msg/SteeSource.msg" + + # sensor_msgs + "msg/SteePointCloud2.msg" + "msg/SteeImu.msg" + + # geometry_msgs + "msg/SteePolygonStamped.msg" + "msg/SteePoseStamped.msg" + "msg/SteePoseWithCovarianceStamped.msg" + "msg/SteeTwistStamped.msg" + "msg/SteeTwistWithCovarianceStamped.msg" + + # nav_msgs + "msg/SteeOccupancyGrid.msg" + "msg/SteeOdometry.msg" + + # autoware_auto_perception_msgs + "msg/SteeDetectedObjects.msg" + "msg/SteePredictedObjects.msg" + "msg/SteeTrackedObjects.msg" + "msg/SteeTrafficLightRoiArray.msg" + "msg/SteeTrafficSignalArray.msg" + + # autoware_auto_planning_msgs + "msg/SteePathWithLaneId.msg" + "msg/SteePath.msg" + "msg/SteeTrajectory.msg" + + # autoware_auto_control_msgs + "msg/SteeAckermannControlCommand.msg" + "msg/SteeAckermannLateralCommand.msg" + "msg/SteeLongitudinalCommand.msg" + ) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES + builtin_interfaces + std_msgs + sensor_msgs + geometry_msgs nav_msgs + autoware_auto_geometry_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_control_msgs +) +ament_export_dependencies( + rosidl_default_runtime + sensor_msgs + geometry_msgs + nav_msgs + autoware_auto_geometry_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + autoware_auto_control_msgs + ) + +ament_package() diff --git a/src/tilde_msg/msg/DeadlineNotification.msg b/src/tilde_msg/msg/DeadlineNotification.msg new file mode 100644 index 00000000..fab275f0 --- /dev/null +++ b/src/tilde_msg/msg/DeadlineNotification.msg @@ -0,0 +1,5 @@ +std_msgs/Header header +string topic_name +builtin_interfaces/Time stamp +builtin_interfaces/Duration deadline_setting +Source[] sources diff --git a/src/tilde_msg/msg/MessageTrackingTag.msg b/src/tilde_msg/msg/MessageTrackingTag.msg new file mode 100644 index 00000000..cc43636f --- /dev/null +++ b/src/tilde_msg/msg/MessageTrackingTag.msg @@ -0,0 +1,4 @@ +std_msgs/Header header + +PubTopicTimeInfo output_info +SubTopicTimeInfo[] input_infos diff --git a/src/tilde_msg/msg/PubTopicTimeInfo.msg b/src/tilde_msg/msg/PubTopicTimeInfo.msg new file mode 100644 index 00000000..40ebde3f --- /dev/null +++ b/src/tilde_msg/msg/PubTopicTimeInfo.msg @@ -0,0 +1,8 @@ +string topic_name +string node_fqn +int64 seq + +builtin_interfaces/Time pub_time +builtin_interfaces/Time pub_time_steady +bool has_header_stamp +builtin_interfaces/Time header_stamp diff --git a/src/tilde_msg/msg/Source.msg b/src/tilde_msg/msg/Source.msg new file mode 100644 index 00000000..db4efa68 --- /dev/null +++ b/src/tilde_msg/msg/Source.msg @@ -0,0 +1,4 @@ +string topic +builtin_interfaces/Time stamp +builtin_interfaces/Duration elapsed +bool is_overrun diff --git a/src/tilde_msg/msg/SteeAckermannControlCommand.msg b/src/tilde_msg/msg/SteeAckermannControlCommand.msg new file mode 100644 index 00000000..8752608a --- /dev/null +++ b/src/tilde_msg/msg/SteeAckermannControlCommand.msg @@ -0,0 +1,2 @@ +autoware_auto_control_msgs/AckermannControlCommand body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeAckermannLateralCommand.msg b/src/tilde_msg/msg/SteeAckermannLateralCommand.msg new file mode 100644 index 00000000..52dd2a2d --- /dev/null +++ b/src/tilde_msg/msg/SteeAckermannLateralCommand.msg @@ -0,0 +1,2 @@ +autoware_auto_control_msgs/AckermannLateralCommand body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeDetectedObjects.msg b/src/tilde_msg/msg/SteeDetectedObjects.msg new file mode 100644 index 00000000..838144ae --- /dev/null +++ b/src/tilde_msg/msg/SteeDetectedObjects.msg @@ -0,0 +1,2 @@ +autoware_auto_perception_msgs/DetectedObjects body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeImu.msg b/src/tilde_msg/msg/SteeImu.msg new file mode 100644 index 00000000..cb0e407a --- /dev/null +++ b/src/tilde_msg/msg/SteeImu.msg @@ -0,0 +1,2 @@ +sensor_msgs/Imu body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeLongitudinalCommand.msg b/src/tilde_msg/msg/SteeLongitudinalCommand.msg new file mode 100644 index 00000000..f87a458b --- /dev/null +++ b/src/tilde_msg/msg/SteeLongitudinalCommand.msg @@ -0,0 +1,2 @@ +autoware_auto_control_msgs/LongitudinalCommand body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeOccupancyGrid.msg b/src/tilde_msg/msg/SteeOccupancyGrid.msg new file mode 100644 index 00000000..7fbf7046 --- /dev/null +++ b/src/tilde_msg/msg/SteeOccupancyGrid.msg @@ -0,0 +1,2 @@ +nav_msgs/OccupancyGrid body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeOdometry.msg b/src/tilde_msg/msg/SteeOdometry.msg new file mode 100644 index 00000000..67084887 --- /dev/null +++ b/src/tilde_msg/msg/SteeOdometry.msg @@ -0,0 +1,2 @@ +nav_msgs/Odometry body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePath.msg b/src/tilde_msg/msg/SteePath.msg new file mode 100644 index 00000000..650e1fb0 --- /dev/null +++ b/src/tilde_msg/msg/SteePath.msg @@ -0,0 +1,2 @@ +autoware_auto_planning_msgs/Path body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePathWithLaneId.msg b/src/tilde_msg/msg/SteePathWithLaneId.msg new file mode 100644 index 00000000..6881f629 --- /dev/null +++ b/src/tilde_msg/msg/SteePathWithLaneId.msg @@ -0,0 +1,2 @@ +autoware_auto_planning_msgs/PathWithLaneId body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePointCloud2.msg b/src/tilde_msg/msg/SteePointCloud2.msg new file mode 100644 index 00000000..fabc368e --- /dev/null +++ b/src/tilde_msg/msg/SteePointCloud2.msg @@ -0,0 +1,2 @@ +sensor_msgs/PointCloud2 body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePolygonStamped.msg b/src/tilde_msg/msg/SteePolygonStamped.msg new file mode 100644 index 00000000..4ec0b18c --- /dev/null +++ b/src/tilde_msg/msg/SteePolygonStamped.msg @@ -0,0 +1,2 @@ +geometry_msgs/PolygonStamped body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePoseStamped.msg b/src/tilde_msg/msg/SteePoseStamped.msg new file mode 100644 index 00000000..1971ea36 --- /dev/null +++ b/src/tilde_msg/msg/SteePoseStamped.msg @@ -0,0 +1,2 @@ +geometry_msgs/PoseStamped body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg b/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg new file mode 100644 index 00000000..adc4c3fd --- /dev/null +++ b/src/tilde_msg/msg/SteePoseWithCovarianceStamped.msg @@ -0,0 +1,2 @@ +geometry_msgs/PoseWithCovarianceStamped body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteePredictedObjects.msg b/src/tilde_msg/msg/SteePredictedObjects.msg new file mode 100644 index 00000000..3637acd6 --- /dev/null +++ b/src/tilde_msg/msg/SteePredictedObjects.msg @@ -0,0 +1,2 @@ +autoware_auto_perception_msgs/PredictedObjects body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeSource.msg b/src/tilde_msg/msg/SteeSource.msg new file mode 100644 index 00000000..573e9f26 --- /dev/null +++ b/src/tilde_msg/msg/SteeSource.msg @@ -0,0 +1,3 @@ +string topic +builtin_interfaces/Time stamp +builtin_interfaces/Time first_subscription_steady_time diff --git a/src/tilde_msg/msg/SteeTrackedObjects.msg b/src/tilde_msg/msg/SteeTrackedObjects.msg new file mode 100644 index 00000000..9c3640ed --- /dev/null +++ b/src/tilde_msg/msg/SteeTrackedObjects.msg @@ -0,0 +1,2 @@ +autoware_auto_perception_msgs/TrackedObjects body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg b/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg new file mode 100644 index 00000000..5adcc05f --- /dev/null +++ b/src/tilde_msg/msg/SteeTrafficLightRoiArray.msg @@ -0,0 +1,2 @@ +autoware_auto_perception_msgs/TrafficLightRoiArray body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrafficSignalArray.msg b/src/tilde_msg/msg/SteeTrafficSignalArray.msg new file mode 100644 index 00000000..f12cf212 --- /dev/null +++ b/src/tilde_msg/msg/SteeTrafficSignalArray.msg @@ -0,0 +1,2 @@ +autoware_auto_perception_msgs/TrafficSignalArray body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTrajectory.msg b/src/tilde_msg/msg/SteeTrajectory.msg new file mode 100644 index 00000000..2915da37 --- /dev/null +++ b/src/tilde_msg/msg/SteeTrajectory.msg @@ -0,0 +1,2 @@ +autoware_auto_planning_msgs/Trajectory body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTwistStamped.msg b/src/tilde_msg/msg/SteeTwistStamped.msg new file mode 100644 index 00000000..b900140c --- /dev/null +++ b/src/tilde_msg/msg/SteeTwistStamped.msg @@ -0,0 +1,2 @@ +geometry_msgs/TwistStamped body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg b/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg new file mode 100644 index 00000000..ca28f413 --- /dev/null +++ b/src/tilde_msg/msg/SteeTwistWithCovarianceStamped.msg @@ -0,0 +1,2 @@ +geometry_msgs/TwistWithCovarianceStamped body +SteeSource[] sources diff --git a/src/tilde_msg/msg/SubTopicTimeInfo.msg b/src/tilde_msg/msg/SubTopicTimeInfo.msg new file mode 100644 index 00000000..a1df91a6 --- /dev/null +++ b/src/tilde_msg/msg/SubTopicTimeInfo.msg @@ -0,0 +1,5 @@ +string topic_name +builtin_interfaces/Time sub_time +builtin_interfaces/Time sub_time_steady +bool has_header_stamp +builtin_interfaces/Time header_stamp diff --git a/src/tilde_msg/msg/TestTopLevelStamp.msg b/src/tilde_msg/msg/TestTopLevelStamp.msg new file mode 100644 index 00000000..4b20d319 --- /dev/null +++ b/src/tilde_msg/msg/TestTopLevelStamp.msg @@ -0,0 +1 @@ +builtin_interfaces/Time stamp diff --git a/src/tilde_msg/package.xml b/src/tilde_msg/package.xml new file mode 100644 index 00000000..a58c79d3 --- /dev/null +++ b/src/tilde_msg/package.xml @@ -0,0 +1,31 @@ + + + + tilde_msg + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + autoware_auto_control_msgs + autoware_auto_geometry_msgs + autoware_auto_perception_msgs + autoware_auto_planning_msgs + geometry_msgs + nav_msgs + sensor_msgs + std_msgs + + ament_lint_auto + caret_lint_common + + + ament_cmake + + diff --git a/src/tilde_sample/CMakeLists.txt b/src/tilde_sample/CMakeLists.txt new file mode 100644 index 00000000..4180bfaa --- /dev/null +++ b/src/tilde_sample/CMakeLists.txt @@ -0,0 +1,80 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_sample) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(tilde_cmake REQUIRED) +tilde_package() +find_package(tilde REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +include_directories(include) + +add_library(tilde_sample SHARED + src/sample_publisher.cpp + src/relay_timer.cpp + src/relay_timer_with_buffer.cpp + src/goal.cpp + src/sample_stee_publisher.cpp) +ament_target_dependencies(tilde_sample + "rclcpp" + "rclcpp_components" + "std_msgs" + "tilde" + "sensor_msgs") + +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::SamplePublisherWithStamp" + EXECUTABLE publisher_with_stamp) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::SamplePublisherWithoutStamp" + EXECUTABLE publisher_without_stamp) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::RelayTimer" + EXECUTABLE relay_timer) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::RelayTimerWithBuffer" + EXECUTABLE relay_timer_with_buffer) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::Goal" + EXECUTABLE goal) +rclcpp_components_register_node(tilde_sample + PLUGIN "tilde_sample::SampleSteePublisherNode" + EXECUTABLE sample_stee_publisher) + +install(TARGETS + tilde_sample) +install( + TARGETS tilde_sample EXPORT tilde_sample + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + + +ament_package() diff --git a/src/tilde_sample/README.md b/src/tilde_sample/README.md new file mode 100644 index 00000000..ee6788ce --- /dev/null +++ b/src/tilde_sample/README.md @@ -0,0 +1,355 @@ +# TILDE Samples + +## About + +Sample classes for TILDE. + +## Nodes + +As TILDE uses header.stamp, we have publishers/subscriptions with and without a header field. + +| class | about | +| --------------------------- | ------------------------------------------------------------------ | +| SamplePublisherWithStamp | Publish message with standard header (PointCloud2) | +| SamplePublisherWithoutStamp | Publish message without standard header (String) | +| RelayTimer | Subscribe PointCloud2 and String, and relay them | +| RelayTimerWithBuffer | Similar with RelayTimer, but it has buffers and calls explicit API | + +## Run + +### (1) Publisher and Subscription with standard header + +```bash +$ ros2 launch tilde_sample publisher_relay_with_header.launch.py +(snip) +[publisher_with_stamp-1] [INFO] [1646122380.746137604] [talker_with_stamp]: Publishing PointCloud2: 7 stamp: 1646122380745891914 +[relay_timer-2] [INFO] [1646122380.746543541] [relay_timer]: RelayTimer sub PointCloud2 seq: 7 stamp: 1646122380745891914 +[relay_timer-2] [INFO] [1646122380.757507728] [relay_timer]: RelayTimer pub PointCloud2 seq: 7 stamp: 1646122380745891914 +(snip) +``` + +See MessageTrackingTag by `ros2 topic echo /message_tracking_tag`. +In this example, you can see `MessageTrackingTag.output_info.stamp` is filled. + +```bash +$ ros2 topic echo /relay_with_stamp/message_tracking_tag +header: + stamp: + sec: 1646122380 + nanosec: 757608873 + frame_id: '' +output_info: + topic_name: /relay_with_stamp + node_fqn: '' + seq: 7 + pub_time: + sec: 1646122380 + nanosec: 757615453 + pub_time_steady: + sec: 6934823 + nanosec: 865332706 + has_header_stamp: true + header_stamp: # matches stamp of seq 7 of relay_timer + sec: 1646122380 + nanosec: 745891914 +input_infos: +- topic_name: /topic_with_stamp + sub_time: + sec: 1646122380 + nanosec: 746446909 + sub_time_steady: + sec: 6934823 + nanosec: 854165185 + has_header_stamp: true + header_stamp: # matches stamp of seq 7 of talker_with_stamp + sec: 1646122380 + nanosec: 745891914 +--- +``` + +### (2) Publisher and Subscription without standard header + +```bash +$ ros2 launch tilde_sample publisher_relay_without_header.launch.py +[publisher_without_stamp-1] [INFO] [1646122584.868273517] [talker_without_stamp]: Publishing String: '3' at '1646122584868032019' +[relay_timer-2] [INFO] [1646122584.868610655] [relay_timer]: RelayTimer sub String seq: 3 +[relay_timer-2] [INFO] [1646122584.882195449] [relay_timer]: RelayTimer pub String seq: 3 +``` + +In this example, you can see `MessageTrackingTag.output_info.has_header` is false, +and `stamp` has no meaning. + +```bash +ros2 topic echo /relay_without_stamp/message_tracking_tag +``` + +
+Result + +```bash +header: + stamp: + sec: 1646122584 + nanosec: 882317785 + frame_id: '' +output_info: + topic_name: /relay_without_stamp + node_fqn: '' + seq: 3 + pub_time: + sec: 1646122584 + nanosec: 882326545 + pub_time_steady: + sec: 6935027 + nanosec: 983860696 + has_header_stamp: false # no stamp + header_stamp: + sec: 0 + nanosec: 0 +input_infos: +- topic_name: /topic_without_stamp + sub_time: + sec: 1646122584 + nanosec: 868535499 + sub_time_steady: + sec: 6935027 + nanosec: 970071208 + has_header_stamp: false # no stamp + header_stamp: + sec: 0 + nanosec: 0 +--- +``` + +
+ +### (3) Multiple Publishers and Subscription + +```bash +ros2 launch tilde_sample multi_publisher_relay.launch.py +``` + +In this example, two publishers are launched. +One has the standard header field, and another doesn't. + +RelayTimer is launched for the subscription, and relays +both publisher messages. + +You can see: + +- MessageTrackingTag has multiple input_info field + +#### multiple input + with stamp + +```bash +$ ros2 topic echo /relay_with_stamp/message_tracking_tag +[publisher_with_stamp-1] [INFO] [1646122873.266387175] [talker_with_stamp]: Publishing PointCloud2: 5 stamp: 1646122873266132674 +[relay_timer-3] [INFO] [1646122873.266819191] [relay_timer]: RelayTimer sub PointCloud2 seq: 5 stamp: 1646122873266132674 +[publisher_without_stamp-2] [INFO] [1646122873.270087090] [talker_without_stamp]: Publishing String: '5' at '1646122873269881680' +[relay_timer-3] [INFO] [1646122873.270392454] [relay_timer]: RelayTimer sub String seq: 5 +[relay_timer-3] [INFO] [1646122873.287324103] [relay_timer]: RelayTimer pub PointCloud2 seq: 5 stamp: 1646122873266132674 +[relay_timer-3] [INFO] [1646122873.287641253] [relay_timer]: RelayTimer pub String seq: 5 +``` + +Result + +```bash +header: + stamp: + sec: 1646122873 + nanosec: 287424898 + frame_id: '' +output_info: + topic_name: /relay_with_stamp + node_fqn: '' + seq: 5 + pub_time: + sec: 1646122873 + nanosec: 287431161 + pub_time_steady: + sec: 6935316 + nanosec: 380230053 + has_header_stamp: true + header_stamp: # matches stamp of relay_timer seq 5 + sec: 1646122873 + nanosec: 266132674 +input_infos: +- topic_name: /topic_with_stamp + sub_time: + sec: 1646122873 + nanosec: 266725249 + sub_time_steady: + sec: 6935316 + nanosec: 359525561 + has_header_stamp: true + header_stamp: # matches stamp of talker_with_stamp seq 5 + sec: 1646122873 + nanosec: 266132674 +- topic_name: /topic_without_stamp + sub_time: + sec: 1646122873 + nanosec: 270354306 + sub_time_steady: + sec: 6935316 + nanosec: 363153994 + has_header_stamp: false # no stamp + header_stamp: + sec: 0 + nanosec: 0 +``` + +#### multiple input + without stamp + +Now, let's see `/relay_without_stamp` MessageTrackingTag + +```bash +ros2 topic echo /relay_without_stamp/message_tracking_tag +``` + +Result: + +```bash +header: + stamp: + sec: 1646122966 + nanosec: 781916993 + frame_id: '' +output_info: + topic_name: /relay_without_stamp + node_fqn: '' + seq: 2 + pub_time: + sec: 1646122966 + nanosec: 781919913 + pub_time_steady: + sec: 6935409 + nanosec: 871887129 + has_header_stamp: false # no stamp + header_stamp: + sec: 0 + nanosec: 0 +input_infos: +- topic_name: /topic_with_stamp + sub_time: + sec: 1646122966 + nanosec: 768327652 + sub_time_steady: + sec: 6935409 + nanosec: 858296849 + has_header_stamp: true + header_stamp: + sec: 1646122966 + nanosec: 767682758 +- topic_name: /topic_without_stamp + sub_time: + sec: 1646122966 + nanosec: 771331749 + sub_time_steady: + sec: 6935409 + nanosec: 861299976 + has_header_stamp: false + header_stamp: + sec: 0 + nanosec: 0 +``` + +### (4) Multiple Publishers and buffered Subscription + +```bash +ros2 launch tilde_sample multi_publisher_buffered_relay.launch.py +``` + +In this example, two publishers are launched as in (3). + +For the subscription side, RelayTimerWithBuffer is launched. + +As RelayTimerWithBuffer uses explicit API, +you can see MessageTrackingTag has single input_info field. + +### buffer with stamp + +```bash +$ ros2 topic echo /relay_buffer_with_stamp/message_tracking_tag +header: + stamp: + sec: 1646123155 + nanosec: 485587145 + frame_id: '' +output_info: + topic_name: /relay_buffer_with_stamp + node_fqn: '' + seq: 5 + pub_time: + sec: 1646123155 + nanosec: 485593383 + pub_time_steady: + sec: 6935598 + nanosec: 569846981 + has_header_stamp: true + header_stamp: + sec: 1646123155 + nanosec: 472929593 +input_infos: # only one entry +- topic_name: /topic_with_stamp + sub_time: + sec: 1646123155 + nanosec: 473451355 + sub_time_steady: + sec: 6935598 + nanosec: 557705720 + has_header_stamp: true + header_stamp: + sec: 1646123155 + nanosec: 472929593 +``` + +### buffer without stamp + +We cannot call explicit API because this topic has no `header.stamp`. +So, there are two input info entries. + +```bash +$ ros2 topic echo /relay_buffer_without_stamp/message_tracking_tag +header: + stamp: + sec: 1646123029 + nanosec: 102143530 + frame_id: '' +output_info: + topic_name: /relay_buffer_without_stamp + node_fqn: '' + seq: 9 + pub_time: + sec: 1646123029 + nanosec: 102146186 + pub_time_steady: + sec: 6935472 + nanosec: 190226558 + has_header_stamp: false + header_stamp: + sec: 0 + nanosec: 0 +input_infos: # multiple input because explicit API is not called +- topic_name: /topic_with_stamp + sub_time: + sec: 1646123029 + nanosec: 88254671 + sub_time_steady: + sec: 6935472 + nanosec: 176335881 + has_header_stamp: true + header_stamp: + sec: 1646123029 + nanosec: 87745929 +- topic_name: /topic_without_stamp + sub_time: + sec: 1646123029 + nanosec: 78947692 + sub_time_steady: + sec: 6935472 + nanosec: 167029850 + has_header_stamp: false + header_stamp: + sec: 0 + nanosec: 0 +``` diff --git a/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py b/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py new file mode 100644 index 00000000..22ec7e21 --- /dev/null +++ b/src/tilde_sample/launch/multi_publisher_buffered_relay.launch.py @@ -0,0 +1,35 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_sample', + executable='publisher_with_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='publisher_without_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='relay_timer_with_buffer', + output='screen'), + ]) diff --git a/src/tilde_sample/launch/multi_publisher_relay.launch.py b/src/tilde_sample/launch/multi_publisher_relay.launch.py new file mode 100644 index 00000000..bcc1d049 --- /dev/null +++ b/src/tilde_sample/launch/multi_publisher_relay.launch.py @@ -0,0 +1,35 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_sample', + executable='publisher_with_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='publisher_without_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='relay_timer', + output='screen'), + ]) diff --git a/src/tilde_sample/launch/publisher_relay_with_header.launch.py b/src/tilde_sample/launch/publisher_relay_with_header.launch.py new file mode 100644 index 00000000..912bc10a --- /dev/null +++ b/src/tilde_sample/launch/publisher_relay_with_header.launch.py @@ -0,0 +1,31 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_sample', + executable='publisher_with_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='relay_timer', + output='screen'), + ]) diff --git a/src/tilde_sample/launch/publisher_relay_without_header.launch.py b/src/tilde_sample/launch/publisher_relay_without_header.launch.py new file mode 100644 index 00000000..1394a678 --- /dev/null +++ b/src/tilde_sample/launch/publisher_relay_without_header.launch.py @@ -0,0 +1,31 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_sample', + executable='publisher_without_stamp', + output='screen'), + Node( + package='tilde_sample', + executable='relay_timer', + output='screen'), + ]) diff --git a/src/tilde_sample/package.xml b/src/tilde_sample/package.xml new file mode 100644 index 00000000..1852e335 --- /dev/null +++ b/src/tilde_sample/package.xml @@ -0,0 +1,27 @@ + + + + tilde_sample + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tilde + tilde_cmake + + ament_lint_auto + caret_lint_common + + ros2launch + + + ament_cmake + + diff --git a/src/tilde_sample/src/goal.cpp b/src/tilde_sample/src/goal.cpp new file mode 100644 index 00000000..f28539a6 --- /dev/null +++ b/src/tilde_sample/src/goal.cpp @@ -0,0 +1,55 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" + +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +namespace tilde_sample +{ +// Create a Talker class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class Goal : public tilde::TildeNode +{ +public: + explicit Goal(const rclcpp::NodeOptions & options) : TildeNode("talker", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto sub_callback = [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + (void)msg; + RCLCPP_INFO(this->get_logger(), "received"); + }; + sub_pc_ = + this->create_tilde_subscription("in", qos, sub_callback); + } + +private: + rclcpp::Subscription::SharedPtr sub_pc_; +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::Goal) diff --git a/src/tilde_sample/src/relay_timer.cpp b/src/tilde_sample/src/relay_timer.cpp new file mode 100644 index 00000000..e2c68389 --- /dev/null +++ b/src/tilde_sample/src/relay_timer.cpp @@ -0,0 +1,135 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; +const int64_t PROC_MS_DEFAULT = 10; + +namespace tilde_sample +{ + +// Create a Talker class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class RelayTimer : public tilde::TildeNode +{ +public: + explicit RelayTimer(const rclcpp::NodeOptions & options) : TildeNode("relay_timer", options) + { + const std::string TIMER_MS = "timer_ms"; + const std::string PROC_MS = "proc_ms"; + const std::string UPDATE_STAMP = "update_stamp"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + declare_parameter(PROC_MS, PROC_MS_DEFAULT); + declare_parameter(UPDATE_STAMP, false); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + auto proc_ms = get_parameter(PROC_MS).get_value(); + auto update_stamp = get_parameter(UPDATE_STAMP).get_value(); + + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + sub_pc_ = this->create_tilde_subscription( + "topic_with_stamp", qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + RCLCPP_INFO( + this->get_logger(), "RelayTimer sub PointCloud2 seq: %s stamp: %lu", + msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); + msg_pc_ = std::move(msg); + }); + + sub_str_ = this->create_tilde_subscription( + "topic_without_stamp", qos, [this](std_msgs::msg::String::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "RelayTimer sub String seq: %s", msg->data.c_str()); + msg_str_ = std::move(msg); + }); + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto proc_dur = std::chrono::duration(proc_ms); + auto timer_callback = [this, proc_dur, update_stamp]() -> void { + std::this_thread::sleep_for(proc_dur); + + if (msg_pc_) { + // We should call explicit API because we don't use String msg. + // See RelayTimerWithBuffer to know how explicit API changes the MessageTrackingTag + + auto stamp = msg_pc_->header.stamp; + + if (update_stamp) { + stamp = this->now(); + msg_pc_->header.stamp = stamp; + } + + RCLCPP_INFO( + this->get_logger(), "RelayTimer pub PointCloud2 seq: %s stamp: %lu", + msg_pc_->header.frame_id.c_str(), nanoseconds(msg_pc_->header.stamp)); + + pub_pc_->publish(std::move(msg_pc_)); + msg_pc_.reset(nullptr); + } + + if (msg_str_) { + // We omit the explicit API. See `if(msg_pc_)` sentence above. + + RCLCPP_INFO(this->get_logger(), "RelayTimer pub String seq: %s", msg_str_->data.c_str()); + + pub_str_->publish(std::move(msg_str_)); + msg_str_.reset(nullptr); + } + }; + + // Create a publisher with a custom Quality of Service profile. + pub_pc_ = this->create_tilde_publisher("relay_with_stamp", qos); + pub_str_ = this->create_tilde_publisher("relay_without_stamp", qos); + + // Use a timer to schedule periodic message publishing. + auto timer_dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(timer_dur, timer_callback); + } + +private: + std::unique_ptr msg_pc_; + rclcpp::Subscription::SharedPtr sub_pc_; + tilde::TildePublisher::SharedPtr pub_pc_; + + std::unique_ptr msg_str_; + rclcpp::Subscription::SharedPtr sub_str_; + tilde::TildePublisher::SharedPtr pub_str_; + + rclcpp::TimerBase::SharedPtr timer_; + + uint64_t nanoseconds(const builtin_interfaces::msg::Time & time_msg) + { + rclcpp::Time time(time_msg); + return time.nanoseconds(); + } +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimer) diff --git a/src/tilde_sample/src/relay_timer_with_buffer.cpp b/src/tilde_sample/src/relay_timer_with_buffer.cpp new file mode 100644 index 00000000..95918054 --- /dev/null +++ b/src/tilde_sample/src/relay_timer_with_buffer.cpp @@ -0,0 +1,142 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; +const int64_t PROC_MS_DEFAULT = 10; + +namespace tilde_sample +{ +// Create a Talker class that subclasses the generic rclcpp::Node base class. +// The main function below will instantiate the class as a ROS node. +class RelayTimerWithBuffer : public tilde::TildeNode +{ +public: + explicit RelayTimerWithBuffer(const rclcpp::NodeOptions & options) + : TildeNode("relay_timer_with_buffer", options) + { + const std::string TIMER_MS = "timer_ms"; + const std::string PROC_MS = "proc_ms"; + const std::string UPDATE_STAMP = "update_stamp"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + declare_parameter(PROC_MS, PROC_MS_DEFAULT); + declare_parameter(UPDATE_STAMP, false); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + auto proc_ms = get_parameter(PROC_MS).get_value(); + auto update_stamp = get_parameter(UPDATE_STAMP).get_value(); + + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + sub_pc_ = this->create_tilde_subscription( + "topic_with_stamp", qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + RCLCPP_INFO( + this->get_logger(), "RelayTimer sub PointCloud2 seq: %s stamp: %lu", + msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); + msgs_pc_.push_back(std::move(msg)); + }); + + sub_str_ = this->create_tilde_subscription( + "topic_without_stamp", qos, [this](std_msgs::msg::String::UniquePtr msg) -> void { + RCLCPP_INFO(this->get_logger(), "RelayTimer sub String seq: %s", msg->data.c_str()); + msgs_str_.push_back(std::move(msg)); + }); + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto proc_dur = std::chrono::duration(proc_ms); + auto timer_callback = [this, proc_dur, update_stamp]() -> void { + std::this_thread::sleep_for(proc_dur); + + if (msgs_pc_.size() > 0) { + auto msg = std::move(this->msgs_pc_.back()); + msgs_pc_.pop_back(); + + // We call explicit API in this example unlike RelayTimer + pub_pc_->add_explicit_input_info(this->sub_pc_->get_topic_name(), msg->header.stamp); + + auto stamp = msg->header.stamp; + + if (update_stamp) { + stamp = this->now(); + msg->header.stamp = stamp; + } + + RCLCPP_INFO( + this->get_logger(), "RelayTimer pub PointCloud2 seq: %s stamp: %lu", + msg->header.frame_id.c_str(), nanoseconds(msg->header.stamp)); + + pub_pc_->publish(std::move(msg)); + } + + if (msgs_str_.size() > 0) { + auto msg = std::move(this->msgs_str_.back()); + msgs_str_.pop_back(); + + // We can not call explicit API because header.stamp does not exist + + RCLCPP_INFO(this->get_logger(), "RelayTimer pub String seq: %s", msg->data.c_str()); + + pub_str_->publish(std::move(msg)); + } + }; + + // Create a publisher with a custom Quality of Service profile. + pub_pc_ = + this->create_tilde_publisher("relay_buffer_with_stamp", qos); + pub_str_ = + this->create_tilde_publisher("relay_buffer_without_stamp", qos); + + // Use a timer to schedule periodic message publishing. + auto timer_dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(timer_dur, timer_callback); + } + +private: + std::vector> msgs_pc_; + rclcpp::Subscription::SharedPtr sub_pc_; + tilde::TildePublisher::SharedPtr pub_pc_; + + std::vector> msgs_str_; + rclcpp::Subscription::SharedPtr sub_str_; + tilde::TildePublisher::SharedPtr pub_str_; + + rclcpp::TimerBase::SharedPtr timer_; + + uint64_t nanoseconds(const builtin_interfaces::msg::Time & time_msg) + { + rclcpp::Time time(time_msg); + return time.nanoseconds(); + } +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::RelayTimerWithBuffer) diff --git a/src/tilde_sample/src/sample_publisher.cpp b/src/tilde_sample/src/sample_publisher.cpp new file mode 100644 index 00000000..423d0b9c --- /dev/null +++ b/src/tilde_sample/src/sample_publisher.cpp @@ -0,0 +1,139 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t TIMER_MS_DEFAULT = 1000; + +namespace tilde_sample +{ + +/** + * This class sends messages with header.stamp field. + * see msg.header.frame_id to get the sequence number. + */ +class SamplePublisherWithStamp : public tilde::TildeNode +{ +public: + explicit SamplePublisherWithStamp(const rclcpp::NodeOptions & options) + : TildeNode("talker_with_stamp", options) + { + const std::string TIMER_MS = "timer_ms"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + std::cout << "timer_ms: " << timer_ms << std::endl; + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto publish_message = [this]() -> void { + auto time_now = this->now(); + + msg_pc_ = std::make_unique(); + msg_pc_->header.stamp = time_now; + msg_pc_->header.frame_id = std::to_string(count_); + pub_pc_->publish(std::move(msg_pc_)); + + RCLCPP_INFO( + this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, + time_now.nanoseconds()); + + count_++; + }; + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + pub_pc_ = this->create_tilde_publisher("topic_with_stamp", qos); + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_message); + } + +private: + size_t count_ = 0; + // message with standard header + std::unique_ptr msg_pc_; + tilde::TildePublisher::SharedPtr pub_pc_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +/** + * This class sends messages without header.stamp field. + */ +class SamplePublisherWithoutStamp : public tilde::TildeNode +{ +public: + explicit SamplePublisherWithoutStamp(const rclcpp::NodeOptions & options) + : TildeNode("talker_without_stamp", options) + { + const std::string TIMER_MS = "timer_ms"; + + declare_parameter(TIMER_MS, TIMER_MS_DEFAULT); + auto timer_ms = get_parameter(TIMER_MS).get_value(); + std::cout << "timer_ms: " << timer_ms << std::endl; + + // Create a function for when messages are to be sent. + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + auto publish_message = [this]() -> void { + auto time_now = this->now(); + + msg_string_ = std::make_unique(); + msg_string_->data = std::to_string(count_); + pub_string_->publish(std::move(msg_string_)); + + RCLCPP_INFO( + this->get_logger(), "Publishing String: '%ld' at '%lu'", count_, time_now.nanoseconds()); + + count_++; + }; + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + pub_string_ = this->create_tilde_publisher("topic_without_stamp", qos); + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_message); + } + +private: + size_t count_ = 0; + + // message without standard header, especially header.stamp + std::unique_ptr msg_string_; + tilde::TildePublisher::SharedPtr pub_string_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithStamp) +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SamplePublisherWithoutStamp) diff --git a/src/tilde_sample/src/sample_stee_publisher.cpp b/src/tilde_sample/src/sample_stee_publisher.cpp new file mode 100644 index 00000000..1dbefa5d --- /dev/null +++ b/src/tilde_sample/src/sample_stee_publisher.cpp @@ -0,0 +1,86 @@ +// 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/stee_node.hpp" +#include "tilde/stee_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +const int64_t g_timer_ms_default = 1000; + +namespace tilde_sample +{ + +class SampleSteePublisherNode : public tilde::SteeNode +{ +public: + explicit SampleSteePublisherNode(const rclcpp::NodeOptions & options) + : SteeNode("sample_stee_publisher_node", options) + { + const std::string timer_ms_name = "timer_ms"; + + declare_parameter(timer_ms_name, g_timer_ms_default); + auto timer_ms = get_parameter(timer_ms_name).get_value(); + std::cout << "timer_ms: " << timer_ms << std::endl; + + // Create a function for when messages are to be sent. + setvbuf(stdout, nullptr, _IONBF, BUFSIZ); + + auto publish_message = [this]() -> void { + auto time_now = this->now(); + + msg_pc_ = std::make_unique(); + msg_pc_->header.stamp = time_now; + msg_pc_->header.frame_id = std::to_string(count_); + pub_pc_->publish(std::move(msg_pc_)); + + RCLCPP_INFO( + this->get_logger(), "Publishing PointCloud2: %ld stamp: %lu", count_, + time_now.nanoseconds()); + + count_++; + }; + + // Create a publisher with a custom Quality of Service profile. + rclcpp::QoS qos(rclcpp::KeepLast(7)); + pub_pc_ = this->create_stee_publisher("topic_with_stamp", qos); + + // Use a timer to schedule periodic message publishing. + auto dur = std::chrono::duration(timer_ms); + timer_ = this->create_wall_timer(dur, publish_message); + } + +private: + size_t count_ = 0; + // message with standard header + std::unique_ptr msg_pc_; + tilde::SteePublisher::SharedPtr pub_pc_; + + rclcpp::TimerBase::SharedPtr timer_; +}; + +} // namespace tilde_sample + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_sample::SampleSteePublisherNode) diff --git a/src/tilde_vis/README.md b/src/tilde_vis/README.md new file mode 100644 index 00000000..8c8f5988 --- /dev/null +++ b/src/tilde_vis/README.md @@ -0,0 +1,194 @@ +# tilde_vis + +## Description + +Sample application which uses topic info or MessageTrackingTag. + +## Requirement + +- ros2 galactic +- tilde enabled application + +## Applications + +To use this package, you need the system which uses tilde::TildeNode instead of rclcpp::Node. +Also you should call `create_tilde_subscription` and `create_tilde_publisher`. + +You can use some tools. + +- (1) latency viewer: online latency viewer + - run your application + - run `latency_viewer` +- (2) parse_message_tracking_tag & ipython: analyze latency from rosbag + - run your application and record rosbag2 + - run `parse_message_tracking_tag` to get MessageTrackingTag pkl. + - run ipython and do what you want +- (3) tilde_vis (not well maintained) + - save `/*/message_tracking_tag` to rosbag + - then, run `tilde_vis` +- (4) visualization by CARET + - you can visualize message flows by CARET jupyter notebook + +## (1) latency viewer + +### About + +The latency viewer outputs E2E latency online, just like `vmstat` or `top`. + +### Run + +Run the latency viewer **after** running the target application. +Latency viewer grasps your application topic graph in constructor by discovering and listening `MessageTrackingTag` type topics. + +Latency viewer shows latencies from `target_topic` to source topics. +Here is the example command. + +```bash +# (1) run your application +ros2 launch ... + +# (2) run latency viewer +ros2 run tilde_vis latency_viewer \ + --ros-args \ + -p target_topic:=/localization/pose_twist_fusion_filter/twist_with_covariance +``` + +In default, you see output just like `top`. Internally [curses](https://docs.python.org/3/library/curses.html) is used. +To save the results to a file, use `--batch` option and redirect or `tee` stdout. + +```bash +ros2 run tilde_vis latency_viewer --batch ... +``` + +### Parameters + +#### General parameters + +| name | values | about | +| ------------------------ | ------------------- | ------------------------------------------ | +| `mode` | `one_hot` or `stat` | Calculation rule. See below. | +| `wait_sec_to_init_graph` | positive integer | Initial wait seconds to grasp topic graph. | + +- mode + - one_hot: prints the specific flow latency + - stat: calculates statistics per second + +#### topic graph specific parameters + +You can use topic graph specific options. + +| name | about | +| ----------------- | ------------------------------------------------------------------------------- | +| `excludes_topics` | Topics you don't want to watch, such as `debug` topic. | +| `stops` | Stop traversal if latency viewer reaches these topics to prevent infinite loop. | + +Additionally, you can specify "skip" setting to skip non-TILDE node. +But you need to directly edit `LatencyViewerNode.init_skips()` to do it. + +### Result + +You can see the following lines are displayed repeatedly. +Columns are "topic name", "header stamp", "latency in ROS time", "latency in steady time". + +```text +/control/trajectory_follower/lateral/control_cmd 1618559274.549348133 0 0 + /localization/twist 1618559274.544330488 5 3 + /planning/scenario_planning/trajectory 1618559274.479350019 15 12 + (snip) + /sensing/lidar/concatenated/pointcloud 1618559273.951109000 289 288 + /sensing/lidar/left/outlier_filtered/pointcloud 1618559274.168087000 305 302 + /sensing/lidar/left/mirror_cropped/pointcloud_ex 1618559274.168087000 305 304 + /sensing/lidar/left/self_cropped/pointcloud_ex 1618559274.168087000 305 304 + /sensing/lidar/left/pointcloud_raw_ex* NA NA NA + /sensing/lidar/right/outlier_filtered/pointcloud 1618559274.170810000 300 301 + /sensing/lidar/right/mirror_cropped/pointcloud_ex 1618559274.170810000 305 302 + /sensing/lidar/right/self_cropped/pointcloud_ex 1618559274.170810000 305 303 + /sensing/lidar/right/pointcloud_raw_ex* NA NA NA + /sensing/lidar/top/outlier_filtered/pointcloud 1618559273.951109000 334 333 + /sensing/lidar/top/mirror_cropped/pointcloud_ex 1618559273.951109000 410 407 + /sensing/lidar/top/self_cropped/pointcloud_ex 1618559273.951109000 435 433 + /sensing/lidar/top/pointcloud_raw_ex* NA NA NA + /vehicle/status/twist* NA NA NA +``` + +See [latency_viewer.md](../../doc/latency_viewer.md) in detail (in Japanese). + +### key binding in ncurses view + +In ncurses view, you can use interactive commands. +When you enter "move" or "filter" commands, periodic update of view stops. +Hit + +| keys | about | +| ------------------- | ----------------------------------------------------- | +| down/up arrow, j, k | move lines. | +| d, u | page up/down. | +| g, G | move to top or bottom of lines | +| r, / | filter lines by regex. Hit Enter for periodic update. | +| q, F | stop "move" mode and restart periodic update | + +## (2) parse_message_tracking_tag + +### About + +Read rosbag2 file, and create pkl file. + +### Run + +```bash +ros2 run tilde_vis parse_message_tracking_tag +``` + +`topic_infos.pkl` is created. +You can get `message_tracking_tag.MessageTrackingTags` by reading this pkl file. + +## (3) tilde_vis + +Here are some examples. We use customized autoware.proj logging simulator where we use tilde at `/sensing` module. + +(1) Print subscribers topic info. + +There are five topics, so tilde_vis shows four per-callback latencies(from some subscription callback to the next subscription callback) and e2e latency. + +```bash +$ ros2 run tilde_vis tilde_vis + +/sensing/lidar/top/self_cropped/pointcloud_ex -> /sensing/lidar/top/rectified/pointcloud_ex -> /sensing/lidar/top/outlier_filtered/pointcloud -> /sensing/lidar/concatenated/pointcloud -> /sensing/lidar/measurement_range_cropped/pointcloud +max: 65.2 39.9 15.0 10.0 +avg: 53.0 28.5 9.0 5.5 +min: 40.0 19.9 5.0 0.0 +e2e: 110.0 96.0 80.0 + +max: 60.0 35.0 20.0 10.0 +avg: 55.0 24.0 9.6 5.4 +min: 50.0 19.7 5.0 0.0 +e2e: 110.0 94.0 80.0 +``` + +(2) print publisher topic info + +In the publisher topic info, timestamp means published topic header timestamp. +/sensing nodes conveys the lidar timestamp, we get latency is 0.0. + +```bash +$ ros2 run tilde_vis tilde_vis --ros-args -p watches_pub:=True + +/sensing/lidar/top/self_cropped/pointcloud_ex -> /sensing/lidar/top/mirror_cropped/pointcloud_ex -> /sensing/lidar/top/outlier_filtered/pointcloud -> /sensing/lidar/concatenated/pointcloud -> /sensing/lidar/measurement_range_cropped/pointcloud +max: 0.0 0.0 0.0 0.0 +avg: 0.0 0.0 0.0 0.0 +min: 0.0 0.0 0.0 0.0 +e2e: 0.0 0.0 0.0 + +max: 0.0 0.0 0.0 0.0 +avg: 0.0 0.0 0.0 0.0 +min: 0.0 0.0 0.0 0.0 +e2e: 0.0 0.0 0.0 +``` + +You can set log level by `LOG_LEVEL=DEBUG`. +See source codes for parameters. + +## (4) visualization by CARET + +You can draw message flows from a rosbag file. +Run tilde_vis/caret_vis_tilde in a CARET jupyter notebook. diff --git a/src/tilde_vis/package.xml b/src/tilde_vis/package.xml new file mode 100644 index 00000000..c853da2d --- /dev/null +++ b/src/tilde_vis/package.xml @@ -0,0 +1,20 @@ + + + + tilde_vis + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + rclpy + tilde_msg + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/tilde_vis/resource/tilde_vis b/src/tilde_vis/resource/tilde_vis new file mode 100644 index 00000000..e69de29b diff --git a/src/tilde_vis/setup.cfg b/src/tilde_vis/setup.cfg new file mode 100644 index 00000000..7601a6d4 --- /dev/null +++ b/src/tilde_vis/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/tilde_vis +[install] +install_scripts=$base/lib/tilde_vis diff --git a/src/tilde_vis/setup.py b/src/tilde_vis/setup.py new file mode 100644 index 00000000..dfd93206 --- /dev/null +++ b/src/tilde_vis/setup.py @@ -0,0 +1,30 @@ +"""Visualization tool for TILDE.""" +from setuptools import setup + +package_name = 'tilde_vis' + +setup( + name=package_name, + version='0.0.0', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='y-okumura', + maintainer_email='y-okumura@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'tilde_vis = tilde_vis.tilde_vis:main', + 'latency_viewer = tilde_vis.latency_viewer:main', + 'message_tracking_tag_traverse = tilde_vis.message_tracking_tag_traverse:main', + 'parse_message_tracking_tag = tilde_vis.parse_message_tracking_tag:main', + ], + }, +) diff --git a/src/tilde_vis/test/test_copyright.py b/src/tilde_vis/test/test_copyright.py new file mode 100644 index 00000000..cc8ff03f --- /dev/null +++ b/src/tilde_vis/test/test_copyright.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_copyright.main import main +import pytest + + +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/src/tilde_vis/test/test_data_as_tree.py b/src/tilde_vis/test/test_data_as_tree.py new file mode 100644 index 00000000..5153f470 --- /dev/null +++ b/src/tilde_vis/test/test_data_as_tree.py @@ -0,0 +1,192 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +from collections import deque +import unittest + +from tilde_vis.data_as_tree import TreeNode + + +def dict2tree(dict_): + """ + Convert dictionary to TreeNode. + + Parameters + ---------- + dict_: dictionary. If value is a dictionary, + then sub tree is created. + Otherwise, value are stored on data. + + Return + ------ + TreeNode starts from "root". + + """ + Q = deque() + root = TreeNode('root') + Q.append((root, dict_)) + + while len(Q) > 0: + current_node, sub_dict = Q.pop() + + for k, v in sub_dict.items(): + child = current_node.get_child(k) + if isinstance(v, dict): + Q.append((child, v)) + else: + child.add_data(v) + + return root + + +def get_complex_tree(): + """ + Get complex tree. + + Return: + ------ + see the following `sample` variable. + The name of the top node is "root". + So, call `ret.get_chile("p1")` and so on to access nodes. + + """ + sample = { + 'p1': { + 'c1': 'c11', + 'c2': { + 'c21': { + 'c211': 'c2111', + }, + 'c22': 'c221', + } + } + } + + # (TreeNode, sub dictionary) + Q = deque() + root = TreeNode('root') + Q.append((root, sample)) + + while len(Q) > 0: + current_node, sub_dict = Q.pop() + + if not isinstance(sub_dict, dict): + current_node.get_child(sub_dict) + continue + + for k in sub_dict.keys(): + child_node = current_node.get_child(k) + Q.append((child_node, sub_dict[k])) + + return root + + +class TestTreeNode(unittest.TestCase): + + def test_construct(self): + node = TreeNode('foo') + self.assertTrue(node is not None) + + def test_construct_complex(self): + root = get_complex_tree() + + # check tree structure + self.assertEqual(len(root.children), 1) + p1 = root.name2child['p1'] + self.assertEqual(len(p1.children), 2) + c1 = p1.name2child['c1'] + self.assertEqual(len(c1.children), 1) + c2 = p1.name2child['c2'] + self.assertEqual(len(c2.children), 2) + + def test_apply(self): + root = get_complex_tree() + ret = root.apply(lambda n: n.name) + + self.assertEqual(len(ret), 10) + self.assertEqual(ret, + ['root', + 'p1', + 'c1', + 'c11', + 'c2', + 'c21', + 'c211', + 'c2111', + 'c22', + 'c221']) + + def test_apply_with_depth(self): + root = get_complex_tree() + ret = root.apply_with_depth(lambda n, depth: f'{n.name}_{depth}') + + self.assertEqual(len(ret), 10) + self.assertEqual(ret, + ['root_0', + 'p1_1', + 'c1_2', + 'c11_3', + 'c2_2', + 'c21_3', + 'c211_4', + 'c2111_5', + 'c22_3', + 'c221_4']) + + def test_merge(self): + lhs_dict = { + 'c1': { + 'c11': 1, + 'c13': 2, + }, + 'c2': { + 'c21': 3, + }, + } + rhs_dict = { + 'c1': { + 'c11': 10, + 'c12': { + 'c121': 11, + }, + }, + 'c2': 12, + } + lhs = dict2tree(lhs_dict) + rhs = dict2tree(rhs_dict) + + def to_s(tree_node): + name = tree_node.name + data = tree_node.data + return f'{name}: {data}' + + lhs.apply(lambda x: print(to_s(x))) + lhs.merge(rhs) + lhs.apply(lambda x: print(to_s(x))) + + self.assertEqual(len(lhs.children), 2) + c1 = lhs.get_child('c1') + self.assertEqual(len(c1.children), 3) + self.assertEqual(c1.get_child('c11').data, [1, 10]) + self.assertEqual(c1.get_child('c12').get_child('c121').data, [11]) + self.assertEqual(c1.get_child('c13').data, [2]) + + c2 = lhs.get_child('c2') + self.assertEqual(c2.data, [12]) + self.assertEqual(c2.get_child('c21').data, [3]) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/tilde_vis/test/test_flake8.py b/src/tilde_vis/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/src/tilde_vis/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, 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. + +from ament_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/src/tilde_vis/test/test_latency_viewer.py b/src/tilde_vis/test/test_latency_viewer.py new file mode 100644 index 00000000..f372f701 --- /dev/null +++ b/src/tilde_vis/test/test_latency_viewer.py @@ -0,0 +1,272 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +import unittest + +from builtin_interfaces.msg import Time as TimeMsg +import rclpy +from rclpy.time import Time + +from tilde_msg.msg import ( + MessageTrackingTag as MessageTrackingTagMsg, + ) +from tilde_vis.data_as_tree import TreeNode +from tilde_vis.latency_viewer import ( + calc_stat, + LatencyViewerNode, + update_stat, + ) +from tilde_vis.message_tracking_tag import ( + MessageTrackingTag + ) + + +class TestCalcStat(unittest.TestCase): + + def test_none(self): + root = TreeNode('root') + root.data = [(None, None), (None, None), (None, None)] + ret = calc_stat(root) + + self.assertEqual(len(ret), 1) + self.assertEqual(ret[0]['dur_min'], None) + self.assertEqual(ret[0]['dur_mean'], None) + + def test_simple(self): + root = TreeNode('root') + root.data = [(1, 10), (2, 20), (3, 30)] + ret = calc_stat(root) + + self.assertEqual(len(ret), 1) + self.assertEqual(ret[0]['dur_min'], 1) + self.assertEqual(ret[0]['dur_mean'], 2) + self.assertEqual(ret[0]['dur_max'], 3) + self.assertEqual(ret[0]['dur_min_steady'], 10) + self.assertEqual(ret[0]['dur_mean_steady'], 20) + self.assertEqual(ret[0]['dur_max_steady'], 30) + + +def time_msg(sec, ms): + """Get builtin.msg.Time.""" + return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() + + +def get_solver_result_simple( + t1_pub, + t1_sub, t2_pub, + t2_sub, t3_pub): + """ + Generate solver result. + + Graph: topic1 --> topic2 --> topic3 + + You can specify each pub/sub time. + Assume: + - header_stamp is preserved at topic1 pub time. + - stamp and stamp_steady are same + """ + tree_node3 = TreeNode('topic3') + tree_node2 = tree_node3.get_child('topic2') + tree_node1 = tree_node2.get_child('topic1') + + stamp1 = t1_pub + message_tracking_tag3 = MessageTrackingTag( + 'topic3', + t3_pub, t3_pub, True, stamp1) + message_tracking_tag3.add_input_info( + 'topic2', t2_sub, t2_sub, + True, stamp1) + tree_node3.add_data(message_tracking_tag3) + + message_tracking_tag2 = MessageTrackingTag( + 'topic2', + t2_pub, t2_pub, True, stamp1) + message_tracking_tag2.add_input_info( + 'topic1', t1_sub, t1_sub, + True, stamp1) + tree_node2.add_data(message_tracking_tag2) + + message_tracking_tag1 = MessageTrackingTag( + 'topic1', + t1_pub, t1_pub, True, stamp1) + tree_node1.add_data(message_tracking_tag1) + + return tree_node3 + + +class TestUpdateStat(unittest.TestCase): + + def test_simple(self): + """ + Simple case. + + pub/sub time + (case1) 10 11 12 13 14 + (case2) 20 24 28 30 35 + + t1 means: + topic1 pub at t=10 and subscribed at t=11 + topic2 pub at t=12 and subscribed at t=13 + topic3 pub at t=14 + """ + results_case1 = get_solver_result_simple( + time_msg(0, 10), + time_msg(0, 11), time_msg(0, 12), + time_msg(0, 13), time_msg(0, 14)) + + update_stat_case1 = update_stat(results_case1) + topic3_result_data = update_stat_case1.data + self.assertEqual(topic3_result_data[0], (0, 0)) + + topic2_result_data = update_stat_case1.name2child['topic2'].data + self.assertEqual(topic2_result_data[0], (2, 2)) + + topic1_result_data = \ + update_stat_case1.name2child['topic2'].name2child['topic1'].data + self.assertEqual(topic1_result_data[0], (4, 4)) + + +class TestListenerCallback(unittest.TestCase): + + @classmethod + def setUpClass(cls): + rclpy.init() + + @classmethod + def tearDownClass(cls): + rclpy.shutdown() + + def test_seq(self): + topic_name = 'topic' + node = LatencyViewerNode() + msg = MessageTrackingTagMsg() + msg.output_info.topic_name = topic_name + + msg.output_info.seq = 0 + msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) + node.listener_callback(msg) + self.assertTrue(topic_name in node.topic_seq) + self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 1) + self.assertEqual(node.topic_seq[topic_name], 0) + + msg.output_info.seq = 1 + msg.output_info.header_stamp = TimeMsg(sec=11, nanosec=0) + node.listener_callback(msg) + self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 2) + self.assertEqual(node.topic_seq[topic_name], 1) + + msg.output_info.seq = 3 + msg.output_info.header_stamp = TimeMsg(sec=13, nanosec=0) + node.listener_callback(msg) + self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 3) + self.assertEqual(node.topic_seq[topic_name], 3) + + msg.output_info.seq = 2 + msg.output_info.header_stamp = TimeMsg(sec=12, nanosec=0) + node.listener_callback(msg) + self.assertEqual(len(node.message_tracking_tags.stamps(topic_name)), 4) + self.assertEqual(node.topic_seq[topic_name], 3) + + node.destroy_node() + + def test_seq_non_zero_start(self): + topic_name = 'topic' + node = LatencyViewerNode() + msg = MessageTrackingTagMsg() + msg.output_info.topic_name = topic_name + + msg.output_info.seq = 10 + msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) + node.listener_callback(msg) + self.assertTrue(topic_name in node.topic_seq) + self.assertEqual(node.topic_seq[topic_name], 10) + + msg.output_info.seq = 20 + msg.output_info.header_stamp = TimeMsg(sec=20, nanosec=0) + node.listener_callback(msg) + self.assertTrue(topic_name in node.topic_seq) + self.assertEqual(node.topic_seq[topic_name], 20) + + msg.output_info.seq = 15 + msg.output_info.header_stamp = TimeMsg(sec=15, nanosec=0) + node.listener_callback(msg) + self.assertTrue(topic_name in node.topic_seq) + self.assertEqual(node.topic_seq[topic_name], 20) + + node.destroy_node() + + +class TestTimerCallback(unittest.TestCase): + + def setUp(self): + rclpy.init() + + def tearDown(self): + rclpy.shutdown() + + def test_issues23_1(self): + topic_name = 'topic' + node = LatencyViewerNode() + node.target_topic = topic_name + + msg = MessageTrackingTagMsg() + msg.output_info.topic_name = topic_name + msg.output_info.seq = 0 + msg.output_info.has_header_stamp = False + msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) + + node.listener_callback(msg) + self.assertEqual(len(node.message_tracking_tags.topics()), 1) + self.assertEqual(node.message_tracking_tags.topics()[0], topic_name) + stamps = node.message_tracking_tags.stamps(topic_name) + self.assertEqual(len(stamps), 1) + + node.wait_init = node.wait_sec_to_init_graph + 1 + + try: + node.timer_callback() + self.assertTrue(True) + except AttributeError: + self.fail(msg='timer_callback causes AttributeError') + + def test_issues23_2(self): + topic_name = 'topic' + node = LatencyViewerNode() + + node.target_topic = topic_name + + msg = MessageTrackingTagMsg() + msg.output_info.topic_name = topic_name + msg.output_info.seq = 0 + msg.output_info.has_header_stamp = True + msg.output_info.header_stamp = TimeMsg(sec=10, nanosec=0) + + node.listener_callback(msg) + self.assertEqual(len(node.message_tracking_tags.topics()), 1) + self.assertEqual(node.message_tracking_tags.topics()[0], topic_name) + stamps = node.message_tracking_tags.stamps(topic_name) + self.assertEqual(len(stamps), 1) + + node.wait_init = node.wait_sec_to_init_graph + 1 + + try: + node.timer_callback() + self.assertTrue(True) + except AttributeError: + self.fail(msg='timer_callback causes AttributeError') + + +if __name__ == '__main__': + unittest.main() diff --git a/src/tilde_vis/test/test_message_tracking_tag.py b/src/tilde_vis/test/test_message_tracking_tag.py new file mode 100644 index 00000000..88ad5726 --- /dev/null +++ b/src/tilde_vis/test/test_message_tracking_tag.py @@ -0,0 +1,119 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +import unittest + +from rclpy.time import Time + +from tilde_vis.message_tracking_tag import ( + MessageTrackingTag, + MessageTrackingTags + ) + + +def time_msg(sec, ms): + """Get builtin.msg.Time.""" + return Time(seconds=sec, nanoseconds=ms * 10**6).to_msg() + + +class TestMessageTrackingTags(unittest.TestCase): + + def test_erase_until(self): + infos = MessageTrackingTags() + infos.add(MessageTrackingTag( + 'topic1', + time_msg(8, 0), + time_msg(9, 0), + True, + time_msg(10, 0) + )) + infos.add(MessageTrackingTag( + 'topic2', + time_msg(18, 0), + time_msg(19, 0), + True, + time_msg(20, 0) + )) + infos.add(MessageTrackingTag( + 'topic1', + time_msg(28, 0), + time_msg(29, 0), + True, + time_msg(30, 0) + )) + + self.assertEqual(len(infos.topic_vs_message_tracking_tags.keys()), 2) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) + + infos.erase_until(time_msg(9, 999)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) + + # boundary condition - not deleted + infos.erase_until(time_msg(10, 0)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) + + infos.erase_until(time_msg(10, 1)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 1) + self.assertTrue('30.000000000' in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2'].keys()), 1) + + # topic2 at t=30.0 is deleted, but key is preserved + infos.erase_until(time_msg(20, 1)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags.keys()), 2) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 1) + self.assertTrue('30.000000000' in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic2']), 0) + + def test_erase_until_many(self): + infos = MessageTrackingTags() + for i in range(0, 10): + infos.add(MessageTrackingTag( + 'topic1', + time_msg(i, 0), + time_msg(i, 0), + True, + time_msg(i, 0) + )) + + infos.erase_until(time_msg(3, 1)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 6) + self.assertTrue('0.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('1.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('2.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('3.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + + infos.erase_until(time_msg(7, 1)) + self.assertEqual(len(infos.topic_vs_message_tracking_tags['topic1'].keys()), 2) + self.assertTrue('4.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('5.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('6.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + self.assertTrue('7.000000000' not in + infos.topic_vs_message_tracking_tags['topic1'].keys()) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/tilde_vis/test/test_message_tracking_tag_traverse.py b/src/tilde_vis/test/test_message_tracking_tag_traverse.py new file mode 100644 index 00000000..141bb877 --- /dev/null +++ b/src/tilde_vis/test/test_message_tracking_tag_traverse.py @@ -0,0 +1,558 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +import unittest + +from rclpy.clock import ClockType +from rclpy.duration import Duration +from rclpy.time import Time + +from tilde_vis.latency_viewer import ( + calc_one_hot, + update_stat, + ) +from tilde_vis.message_tracking_tag import MessageTrackingTag, MessageTrackingTags +from tilde_vis.message_tracking_tag_traverse import ( + InputSensorStampSolver, + TopicGraph, + ) + + +def get_scenario1(): + """ + Get the following scenario. + + topic1 --> topic2 --> topic3 + + topic1 is published in 10Hz. + topic2 takes 10ms to execute callback. + + + """ + def get_topic2_times(start_time, start_time_steady): + sub_time = start_time + nw_dur + pub_time = sub_time + cb_dur + + sub_time_steady = start_time_steady + nw_dur + pub_time_steady = sub_time_steady + cb_dur + + return {'sub': sub_time, + 'pub': pub_time, + 'sub_steady': sub_time_steady, + 'pub_steady': pub_time_steady} + + def get_topic3_times(start_time, start_time_steady): + sub_time = start_time + nw_dur + cb_dur + nw_dur + pub_time = sub_time + cb_dur + + sub_time_steady = start_time_steady + nw_dur + cb_dur + nw_dur + pub_time_steady = sub_time_steady + cb_dur + + return {'sub': sub_time, + 'pub': pub_time, + 'sub_steady': sub_time_steady, + 'pub_steady': pub_time_steady} + + t1 = Time(seconds=10, nanoseconds=0) + t1_steady = Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME) + nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] + cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] + + message_tracking_tag_topic1_t1 = \ + MessageTrackingTag( + 'topic1', + t1.to_msg(), + t1_steady.to_msg(), + True, + t1.to_msg()) + + topic2_t1_times = get_topic2_times(t1, t1_steady) + message_tracking_tag_topic2_t1 = \ + MessageTrackingTag( + 'topic2', + topic2_t1_times['pub'].to_msg(), + topic2_t1_times['pub_steady'].to_msg(), + True, + t1.to_msg()) + message_tracking_tag_topic2_t1.add_input_info( + 'topic1', + topic2_t1_times['sub'], + topic2_t1_times['sub_steady'], + True, + t1.to_msg()) + + topic3_t1_times = get_topic3_times(t1, t1_steady) + message_tracking_tag_topic3_t1 = \ + MessageTrackingTag( + 'topic3', + topic3_t1_times['pub'].to_msg(), + topic3_t1_times['pub_steady'].to_msg(), + True, + t1.to_msg()) + message_tracking_tag_topic3_t1.add_input_info( + 'topic2', + topic3_t1_times['sub'], + topic3_t1_times['sub_steady'], + True, + t1.to_msg()) + + return [message_tracking_tag_topic1_t1, + message_tracking_tag_topic2_t1, + message_tracking_tag_topic3_t1] + + +def dur_plus(lhs, rhs): + """Plus of Duration.""" + return Duration(nanoseconds=lhs.nanoseconds + rhs.nanoseconds) + + +def gen_scenario2( + st, st_steady, + nw_dur, cb_dur): + """ + Get the following scenario. + + topic1 --> topic3 --> topic4 + topic2 ---/ + + Args: + st: Time + st_steady: Time + nw_dur: Duration + cb_dur: Duration + + Returns + ------- + list of MessageTrackingTags such as + (message_tracking_tag of topic1, message_tracking_tag of topic2, ...) + + """ + message_tracking_tag_topic1 = \ + MessageTrackingTag( + 'topic1', + st.to_msg(), + st_steady.to_msg(), + True, + st.to_msg()) + + topic2_stamp = st + nw_dur + message_tracking_tag_topic2 = \ + MessageTrackingTag( + 'topic2', + st.to_msg(), + st_steady.to_msg(), + True, + topic2_stamp.to_msg()) + + sub_dur3 = nw_dur + pub_dur3 = dur_plus(sub_dur3, cb_dur) + topic3_stamp = st + pub_dur3 + message_tracking_tag_topic3 = \ + MessageTrackingTag( + 'topic3', + (st + pub_dur3).to_msg(), + (st_steady + pub_dur3).to_msg(), + True, + topic3_stamp.to_msg()) + message_tracking_tag_topic3.add_input_info( + 'topic1', + (st + sub_dur3).to_msg(), + (st_steady + sub_dur3).to_msg(), + True, + st.to_msg() + ) + message_tracking_tag_topic3.add_input_info( + 'topic2', + (st + sub_dur3).to_msg(), + (st_steady + sub_dur3).to_msg(), + True, + topic2_stamp.to_msg()) + + sub_dur4 = dur_plus(pub_dur3, nw_dur) + pub_dur4 = dur_plus(sub_dur4, cb_dur) + topic4_stamp = st + pub_dur4 + message_tracking_tag_topic4 = \ + MessageTrackingTag( + 'topic4', + (st + pub_dur4).to_msg(), + (st_steady + pub_dur4).to_msg(), + True, + topic4_stamp.to_msg()) + message_tracking_tag_topic4.add_input_info( + 'topic3', + (st + sub_dur4).to_msg(), + (st_steady + sub_dur4).to_msg(), + True, + topic3_stamp.to_msg()) + + return [ + message_tracking_tag_topic1, + message_tracking_tag_topic2, + message_tracking_tag_topic3, + message_tracking_tag_topic4, + ] + + +class TestTopicGraph(unittest.TestCase): + + def test_straight(self): + infos = get_scenario1() + message_tracking_tags = MessageTrackingTags() + for info in infos: + message_tracking_tags.add(info) + + graph = TopicGraph(message_tracking_tags, skips={}) + + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') + + edges = graph.topic_edges + self.assertEqual(edges[0], {1}) + self.assertEqual(edges[1], {2}) + self.assertEqual(edges[2], set()) + + rev_edges = graph.rev_edges + self.assertEqual(rev_edges[0], set()) + self.assertEqual(rev_edges[1], {0}) + self.assertEqual(rev_edges[2], {1}) + + def test_scenario2(self): + t0 = Time(seconds=10, nanoseconds=0) + t0_steady = Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME) + nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] + cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] + + infos = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) + message_tracking_tags = MessageTrackingTags() + for info in infos: + message_tracking_tags.add(info) + + graph = TopicGraph(message_tracking_tags, skips={}) + + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') + self.assertEqual(graph.topics[3], 'topic4') + + edges = graph.topic_edges + self.assertEqual(edges[0], {2}) + self.assertEqual(edges[1], {2}) + self.assertEqual(edges[2], {3}) + self.assertEqual(edges[3], set()) + + rev_edges = graph.rev_edges + self.assertEqual(rev_edges[0], set()) + self.assertEqual(rev_edges[1], set()) + self.assertEqual(rev_edges[2], {0, 1}) + self.assertEqual(rev_edges[3], {2}) + + def test_scenario2_with_loss(self): + """ + Graph creation with lossy MessageTrackingTags. + + t0: message_tracking_tag only topic1 and topic3 + t1: only topic2 + t2: only topic4 + """ + t0 = Time(seconds=10, nanoseconds=0) + t0_steady = Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME) + nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] + cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] + period = Duration(seconds=1) + t1, t1_steady = t0 + period, t0_steady + period + t2, t2_steady = t1 + period, t1_steady + period + + infos_t0 = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) + infos_t1 = gen_scenario2(t1, t1_steady, nw_dur, cb_dur) + infos_t2 = gen_scenario2(t2, t2_steady, nw_dur, cb_dur) + message_tracking_tags = MessageTrackingTags() + + message_tracking_tags.add(infos_t0[0]) + message_tracking_tags.add(infos_t0[2]) + message_tracking_tags.add(infos_t1[1]) + message_tracking_tags.add(infos_t2[3]) + + graph = TopicGraph(message_tracking_tags, skips={}) + + self.assertEqual(graph.topics[0], 'topic1') + self.assertEqual(graph.topics[1], 'topic2') + self.assertEqual(graph.topics[2], 'topic3') + self.assertEqual(graph.topics[3], 'topic4') + + edges = graph.topic_edges + self.assertEqual(edges[0], {2}) + self.assertEqual(edges[1], {2}) + self.assertEqual(edges[2], {3}) + self.assertEqual(edges[3], set()) + + rev_edges = graph.rev_edges + self.assertEqual(rev_edges[0], set()) + self.assertEqual(rev_edges[1], set()) + self.assertEqual(rev_edges[2], {0, 1}) + self.assertEqual(rev_edges[3], {2}) + + +class TestTreeNode(unittest.TestCase): + + def test_solve2_straight(self): + infos = get_scenario1() + message_tracking_tags = MessageTrackingTags() + for info in infos: + message_tracking_tags.add(info) + graph = TopicGraph(message_tracking_tags, skips={}) + solver = InputSensorStampSolver(graph) + + tgt_stamp = sorted(message_tracking_tags.stamps('topic3'))[0] + results = solver.solve2( + message_tracking_tags, + 'topic3', + tgt_stamp + ) + + r3 = results + self.assertEqual(r3.name, 'topic3') + self.assertEqual(len(r3.data), 1) + self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) + self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp.sec, 10) + + r2 = r3.name2child['topic2'] + self.assertEqual(r2.name, 'topic2') + self.assertEqual(len(r2.data), 1) + self.assertEqual(len(r2.data[0].in_infos['topic1']), 1) + self.assertEqual(r2.data[0].in_infos['topic1'][0].stamp.sec, 10) + + r1 = r2.name2child['topic1'] + self.assertEqual(r1.name, 'topic1') + self.assertEqual(len(r1.data), 1) + self.assertEqual(len(r1.data[0].in_infos.keys()), 0) + + def test_solve2_branched(self): + t0 = Time(seconds=10, nanoseconds=0) + t0_steady = Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME) + period = Duration(nanoseconds=100 * 10**6) + nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] + cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] + + message_tracking_tags = MessageTrackingTags() + st = t0 + st_steady = t0_steady + + # st: 10.0, 10.1, 10.2, ... + + for i in range(100): + if i == 75: + test_st = Time.from_msg(st.to_msg()) + + infos = gen_scenario2(st, st_steady, nw_dur, cb_dur) + for info in infos: + message_tracking_tags.add(info) + st += period + st_steady += period + + graph = TopicGraph(message_tracking_tags, skips={}) + solver = InputSensorStampSolver(graph) + + sorted_stamps = sorted(message_tracking_tags.stamps('topic4')) + + tgt_stamp = sorted_stamps[75] + results = solver.solve2( + message_tracking_tags, + 'topic4', + tgt_stamp) + + test_stamp1 = test_st + test_stamp2 = test_st + nw_dur + test_stamp3 = test_st + nw_dur + cb_dur + test_stamp4 = test_stamp3 + nw_dur + cb_dur + + r4 = results + self.assertEqual(r4.name, 'topic4') + self.assertEqual(r4.data[0].out_info.stamp, + test_stamp4.to_msg()) + self.assertEqual(len(r4.data), 1) + self.assertEqual(len(r4.data[0].in_infos['topic3']), 1) + self.assertEqual(r4.data[0].in_infos['topic3'][0].stamp, + test_stamp3.to_msg()) + + r3 = r4.name2child['topic3'] + self.assertEqual(r3.name, 'topic3') + self.assertEqual(len(r3.data), 1) + self.assertEqual(len(r3.data[0].in_infos['topic2']), 1) + self.assertEqual(r3.data[0].in_infos['topic2'][0].stamp, + test_stamp2.to_msg()) + self.assertEqual(len(r3.data[0].in_infos['topic1']), 1) + self.assertEqual(r3.data[0].in_infos['topic1'][0].stamp, + test_stamp1.to_msg()) + + one_hot_durations = calc_one_hot(results) + self.assertEqual(len(one_hot_durations), 4) + dur4 = one_hot_durations[0] + self.assertEqual(dur4[1], 'topic4') + self.assertEqual(dur4[3], 0) + + dur3 = one_hot_durations[1] + self.assertEqual(dur3[1], 'topic3') + self.assertEqual(dur3[3], 11) + + dur2 = one_hot_durations[3] + self.assertEqual(dur2[1], 'topic2') + self.assertEqual(dur2[3], 22) + + dur1 = one_hot_durations[2] + self.assertEqual(dur1[1], 'topic1') + self.assertEqual(dur1[3], 22) + + def setup_only_topic4_scenario(self): + """Test lossy scenario.""" + t0 = Time(seconds=10, nanoseconds=0) + t0_steady = Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME) + nw_dur = Duration(nanoseconds=1 * 10**6) # 1 [ms] + cb_dur = Duration(nanoseconds=10 * 10**6) # 10 [ms] + period = Duration(seconds=1) + + def init_solver(): + infos = gen_scenario2(t0, t0_steady, nw_dur, cb_dur) + message_tracking_tags = MessageTrackingTags() + for info in infos: + message_tracking_tags.add(info) + graph = TopicGraph(message_tracking_tags, skips={}) + solver = InputSensorStampSolver(graph) + return solver + + def get_message_tracking_tags_with_only_topic4(): + t1 = t0 + period + t1_steady = t0_steady + period + infos = gen_scenario2(t1, t1_steady, nw_dur, cb_dur) + message_tracking_tags = MessageTrackingTags() + message_tracking_tags.add(infos[-1]) + return message_tracking_tags + + solver = init_solver() + tgt_message_tracking_tags = get_message_tracking_tags_with_only_topic4() + + return (solver, tgt_message_tracking_tags) + + def test_solve_empty(self): + # setup + solver, _ = self.setup_only_topic4_scenario() + tgt_topic = 'topic4' + + results = solver._solve_empty(tgt_topic) + + result_topic4 = results + self.assertEqual(result_topic4.name, 'topic4') + self.assertEqual(len(result_topic4.data), 0) + + result_topic3 = result_topic4.name2child['topic3'] + self.assertEqual(result_topic3.name, 'topic3') + self.assertEqual(len(result_topic3.data), 0) + + result_topic2 = result_topic3 .name2child['topic2'] + self.assertEqual(result_topic2.name, 'topic2') + self.assertEqual(len(result_topic2.data), 0) + + result_topic1 = result_topic3.name2child['topic1'] + self.assertEqual(result_topic1.name, 'topic1') + self.assertEqual(len(result_topic1.data), 0) + + def test_solve2_with_loss(self): + """Solve with lossy MessageTrackingTags.""" + solver, tgt_message_tracking_tags = self.setup_only_topic4_scenario() + + tgt_topic = 'topic4' + tgt_stamps = tgt_message_tracking_tags.stamps(tgt_topic) + + self.assertEqual(len(tgt_stamps), 1) + results = solver.solve2(tgt_message_tracking_tags, tgt_topic, tgt_stamps[0]) + + # is there full graph? + result_topic4 = results + self.assertEqual(result_topic4.name, 'topic4') + self.assertEqual(len(result_topic4.data), 1) + self.assertTrue(isinstance(result_topic4.data[0], MessageTrackingTag)) + + result_topic3 = result_topic4.name2child['topic3'] + self.assertEqual(result_topic3.name, 'topic3') + self.assertEqual(len(result_topic3.data), 0) + + result_topic2 = result_topic3.name2child['topic2'] + self.assertEqual(result_topic2.name, 'topic2') + self.assertEqual(len(result_topic2.data), 0) + + result_topic1 = result_topic3.name2child['topic1'] + self.assertEqual(result_topic1.name, 'topic1') + self.assertEqual(len(result_topic1.data), 0) + + def test_update_stat(self): + t = [] + ts = [] + nw_dur = [] + cb_dur = [] + + t.append(Time(seconds=10, nanoseconds=0)) + ts.append(Time(seconds=0, nanoseconds=1, + clock_type=ClockType.STEADY_TIME)) + nw_dur.append(Duration(nanoseconds=10 * 10**6)) + cb_dur.append(Duration(nanoseconds=1 * 10**6)) + + period1 = Duration(nanoseconds=100 * 10**6) + t.append(t[-1] + period1) + ts.append(ts[-1] + period1) + nw_dur.append(Duration(nanoseconds=20 * 10**6)) + cb_dur.append(Duration(nanoseconds=2 * 10**6)) + + period2 = Duration(nanoseconds=100 * 10**6) + t.append(t[-1] + period2) + ts.append(ts[-1] + period2) + nw_dur.append(Duration(nanoseconds=30 * 10**6)) + cb_dur.append(Duration(nanoseconds=3 * 10**6)) + + message_tracking_tags = MessageTrackingTags() + + for i in range(len(t)): + infos = gen_scenario2(t[i], ts[i], + nw_dur[i], cb_dur[i]) + for info in infos: + message_tracking_tags.add(info) + + graph = TopicGraph(message_tracking_tags, skips={}) + solver = InputSensorStampSolver(graph) + + tgt_stamp = sorted(message_tracking_tags.stamps('topic4'))[-1] + results = solver.solve2( + message_tracking_tags, + 'topic4', + tgt_stamp) + results = update_stat(results) + + self.assertEqual(results.data, [(0, 0)]) + topic3 = results.get_child('topic3') + self.assertEqual(topic3.data, [(33, 33)]) + + topic2 = topic3.get_child('topic2') + self.assertEqual(topic2.data, [(66, 66)]) + + topic1 = topic3.get_child('topic1') + self.assertEqual(topic1.data, [(66, 66)]) + + +if __name__ == '__main__': + unittest.main() diff --git a/src/tilde_vis/test/test_ncurse_printer.py b/src/tilde_vis/test/test_ncurse_printer.py new file mode 100644 index 00000000..c111dc34 --- /dev/null +++ b/src/tilde_vis/test/test_ncurse_printer.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 +# Copyright 2021 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. + +import curses +import sys +import time + +from tilde_vis.printer import ( + NcursesPrinter + ) + + +def main(stdscr, args=None): + printer = NcursesPrinter(stdscr) + while True: + now = time.time() + lines = [f'{now}: {i}' for i in range(100)] + printer.print_(lines) + time.sleep(1) + + +if __name__ == '__main__': + curses.wrapper(main, args=sys.argv) diff --git a/src/tilde_vis/test/test_pep257.py b/src/tilde_vis/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/src/tilde_vis/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, 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. + +from ament_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/src/tilde_vis/tilde_vis/__init__.py b/src/tilde_vis/tilde_vis/__init__.py new file mode 100644 index 00000000..141c8594 --- /dev/null +++ b/src/tilde_vis/tilde_vis/__init__.py @@ -0,0 +1,15 @@ +# Copyright 2021 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. + +"""Package tilde_vis.""" diff --git a/src/tilde_vis/tilde_vis/caret_vis_tilde.py b/src/tilde_vis/tilde_vis/caret_vis_tilde.py new file mode 100755 index 00000000..e7819aaa --- /dev/null +++ b/src/tilde_vis/tilde_vis/caret_vis_tilde.py @@ -0,0 +1,277 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +"""Check CARET vs TILDE results.""" + +from collections import defaultdict + +from bokeh.models import CrosshairTool +from bokeh.plotting import figure, show +import numpy as np +import pandas as pd + +from tilde_vis.message_tracking_tag import ( + MessageTrackingTag, + MessageTrackingTags +) +from tilde_vis.message_tracking_tag_traverse import ( + InputSensorStampSolver, + TopicGraph +) + + +def time2str(t): + """Convert builtin_interfaces.msg.Time to string.""" + return f'{t.sec}.{t.nanosec:09d}' + + +def time2int(t): + """Convert builtin_interfaces.msg.Time to int.""" + return t.sec * 10**9 + t.nanosec + + +def _rosbag_iter(rosbag_path): + from rclpy.serialization import deserialize_message + from rosidl_runtime_py.utilities import get_message + import rosbag2_py + + storage_option = rosbag2_py.StorageOptions(rosbag_path, 'sqlite3') + + converter_option = rosbag2_py.ConverterOptions('cdr', 'cdr') + reader = rosbag2_py.SequentialReader() + + reader.open(storage_option, converter_option) + + topic_types = reader.get_all_topics_and_types() + type_map = {topic_types[i].name: topic_types[i].type + for i in range(len(topic_types))} + + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + yield topic, msg, t + + +def read_msgs(rosbag_path): + """ + Read demo messages from rosbag. + + Returns + ------- + dictionary such as + { + : [] + } + + """ + msg_topics = defaultdict(list) + for topic, msg, t in _rosbag_iter(rosbag_path): + msg_topics[topic].append(msg) + return msg_topics + + +def read_message_tracking_tag(raw_msgs): + """Convert raw messages to MessageTrackingTags.""" + message_tracking_tags = MessageTrackingTags() + for i in range(5): + msgs = raw_msgs[f'/topic{i+1}/message_tracking_tag'] + for msg in msgs: + message_tracking_tags.add(MessageTrackingTag.fromMsg(msg)) + return message_tracking_tags + + +def get_uuid2msg(raw_msgs): + """Convert raw messages to dictionary.""" + hashed_msgs = {} + hash_target_keys = ['/topic1', '/topic2', '/topic3', '/topic4', '/topic5'] + for hash_target_key in hash_target_keys: + hashed_msgs.update({ + msg.uuid: msg for msg in raw_msgs[hash_target_key] + }) + return hashed_msgs + + +def build_latency_table(traces, ds): + """ + Calculate latencies. + + Parameters + ---------- + traces: list + CARET traces + ds: list + list to output + + """ + traces_dict = {trace.uuid: trace for trace in traces} + + from copy import deepcopy + + def search_flow(start_uuid): + flows = [] + + # 後続のトレースを取得 + def get_subsequence(uuid): + return [trace for trace in traces if uuid in trace.used_uuids] + + def search_local(uuid, flow): + flow.append(uuid) + + subsequence = get_subsequence(uuid) + if len(subsequence) == 0: + flows.append(flow) + + for sub in subsequence: + search_local(sub.uuid, deepcopy(flow)) + + search_local(start_uuid, []) + + return flows + + # start_msgs = msgs_topics['/topic1'] + start_msgs = [trace.uuid for trace in traces + if '/topic1' in trace.uuid and trace.trace_type == 'publish'] + + uid_flows = [] + for msg in start_msgs: + uid_flows += search_flow(msg) + + def trace_to_column(trace): + return f'{trace.node_name}_{trace.trace_type}' + + from collections import defaultdict + for uid_flow in uid_flows: + try: + flow = [traces_dict[uid] for uid in uid_flow] + except KeyError: + flow = [] + d = defaultdict() + + d.update({trace_to_column(trace): trace.steady_t for trace in flow}) + + ds.append(d) + + return ds + +############################ +# visualize tilde +############################ + + +class Trace(object): + """Trace data for CARET.""" + + def __init__(self, node_name, uuid, stamp, is_publish, uuids): + """Initialize trace.""" + self.node_name = node_name # "" + self.uuid = uuid # "_" + self.steady_t = stamp + self.trace_type = 'publish' if is_publish else 'callback_start' + self.used_uuids = uuids + + def __repr__(self): + """Print self.""" + return ( + ' ' + f'node_name={self.node_name} uuid={self.uuid} ' + f'steady_t={self.steady_t} trace_type={self.trace_type} ' + f'used_uuids={self.used_uuids}') + + +def vis_tilde(message_tracking_tags): + """Visualize tilde result.""" + tgt_topic = '/topic5' + topic5_stamps = message_tracking_tags.stamps(tgt_topic) + + graph = TopicGraph(message_tracking_tags) + solver = InputSensorStampSolver(graph) + + ds = [] + for stamp in topic5_stamps: + tilde_path = solver.solve2(message_tracking_tags, tgt_topic, stamp) + + traces = [] + + def update_traces(node): + topic = node.name + for d in node.data: + stamp = time2int(d.out_info.stamp) + pub_time = time2int(d.out_info.pubsub_stamp) + uuid = f'{topic}_{stamp}' + uuids = [] + for in_topic, infos in d.in_infos.items(): + for i in infos: + i_stamp = time2int(i.stamp) + sub_time = time2int(i.pubsub_stamp) + i_uuid = f'{in_topic}_{i_stamp}' + uuids.append(i_uuid) + + # append callback_start + trace = Trace(in_topic, i_uuid, sub_time, False, []) + traces.append(trace) + # append publish + traces.append(Trace(topic, uuid, pub_time, True, uuids)) + + tilde_path.apply(update_traces) + + ds = build_latency_table(traces, ds) + + return ds + + +def plot_latency_table(df): + """Plot flows from df.""" + from tqdm import tqdm + from bokeh.palettes import Bokeh8 + + def get_color(i): + cmap = Bokeh8 + i = i % (len(cmap)) + return cmap[i] + + fig = figure( + x_axis_label='Time [s]', + y_axis_label='', + plot_width=1000, + plot_height=400, + active_scroll='wheel_zoom', + ) + fig.add_tools(CrosshairTool(line_alpha=0.4)) + + for i, row in tqdm(df.iterrows()): + row_ = row.dropna() + + y = np.array(list(range(len(row_)))) * -1 + x = list(row_.values) + + fig.line(x, y, line_color=get_color(i), line_alpha=0.4) + show(fig) + + +def main(): + """Run main.""" + bagfile = 'rosbag2_2022_02_16-18_12_46' + raw_msgs = read_msgs(bagfile) + message_tracking_tags = read_message_tracking_tag(raw_msgs) + ds = vis_tilde(message_tracking_tags) + df = pd.DataFrame.from_dict(ds) + + plot_latency_table(df) + + +if __name__ == '__main__': + """Main.""" + main() diff --git a/src/tilde_vis/tilde_vis/data_as_tree.py b/src/tilde_vis/tilde_vis/data_as_tree.py new file mode 100644 index 00000000..0ca7d43d --- /dev/null +++ b/src/tilde_vis/tilde_vis/data_as_tree.py @@ -0,0 +1,128 @@ +# Copyright 2021 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. + +"""Tree like data structure for MessageTrackingTag traversal.""" + + +class TreeNode(object): + """ + Node of DataAsTree. + + This hold data list and children. + + """ + + def __init__(self, name): + """Initialize tree structure.""" + self.name = name + self.children = [] + self.name2child = {} + self.data = [] + self.value = None + + def num_children(self): + """Get the number of children.""" + return len(self.children) + + def get_child(self, child_name): + """ + Get child. + + If not exists, then append. + + """ + children = self.children + name2child = self.name2child + + if child_name in name2child.keys(): + return name2child[child_name] + + c = TreeNode(child_name) + children.append(c) + children.sort(key=lambda x: x.name) + name2child[child_name] = c + + return c + + def add_data(self, d): + """ + Add data. + + :param d: any value or object + + """ + self.data.append(d) + + def apply(self, fn): + """ + Apply fn recursively. + + Internally, pre-order DFS is used. + + :param fn: callable, which takes TreeNode + :return list of fn return + + """ + children = self.children + ret = [] + + ret.append(fn(self)) + for c in children: + ret.extend(c.apply(fn)) + + return ret + + def apply_with_depth(self, fn, depth=0): + """ + Apply with depth option. + + Internally, pre-order DFS is used. + + :param fn: callback such as fn(tree_node_obj, depth) + :return list of fn return + + """ + children = self.children + ret = [] + + ret.append(fn(self, depth)) + for c in children: + ret.extend(c.apply_with_depth(fn, depth + 1)) + + return ret + + def merge(self, rhs): + """ + Merge data of another tree. + + If self does not have some keys which rhs has, + then new nodes are added. + + :param rhs: another TreeNode + + """ + def _merge(lhs, rhs): + """ + Merge data. + + lhs: TreeNode which is updated + rhs: TreeNode which is const + """ + lhs.data.extend(rhs.data) + + for rhs_c in rhs.children: + lhs_c = lhs.get_child(rhs_c.name) + _merge(lhs_c, rhs_c) + + _merge(self, rhs) diff --git a/src/tilde_vis/tilde_vis/latency_viewer.py b/src/tilde_vis/tilde_vis/latency_viewer.py new file mode 100644 index 00000000..1c305f57 --- /dev/null +++ b/src/tilde_vis/tilde_vis/latency_viewer.py @@ -0,0 +1,792 @@ +#!env python3 +# Copyright 2021 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. + +"""Latency viewer node and its main function.""" + +import argparse +import curses +import os +import pickle +from statistics import mean +import sys +import time + +from builtin_interfaces.msg import Time as TimeMsg +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + QoSHistoryPolicy, + QoSProfile, + QoSReliabilityPolicy + ) +from rclpy.time import Time + +from tilde_msg.msg import ( + MessageTrackingTag, + ) +from tilde_vis.message_tracking_tag import ( + MessageTrackingTag as MessageTrackingTagObj, + MessageTrackingTags as MessageTrackingTagsObj, + time2str + ) +from tilde_vis.message_tracking_tag_traverse import ( + InputSensorStampSolver, + TopicGraph + ) +from tilde_vis.printer import ( + NcursesPrinter, + Printer + ) + + +EXCLUDES_TOPICS = [ + '/diagnostics/message_tracking_tag', + '/control/trajectory_follower/mpc_follower/debug/markers/message_tracking_tag', # noqa: #501 + '/control/trajectory_follower/mpc_follower/debug/steering_cmd/message_tracking_tag', # noqa: #501 + '/localization/debug/ellipse_marker/message_tracking_tag', + '/localization/pose_twist_fusion_filter/debug/message_tracking_tag', + '/localization/pose_twist_fusion_filter/debug/measured_pose/message_tracking_tag', # noqa: #501 + '/localization/pose_twist_fusion_filter/debug/stop_flag/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/markers/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/area_with_objects/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/clearance_map/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/object_clearance_map/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/smoothed_points/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/backward_filtered_trajectory/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/forward_filtered_trajectory/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/merged_filtered_trajectory/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_external_velocity_limited/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_lateral_acc_filtered/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_raw/message_tracking_tag', # noqa: #501 + '/planning/scenario_planning/motion_velocity_smoother/debug/trajectory_time_resampled/message_tracking_tag', # noqa: #501 + ] +LEAVES = [ + '/initialpose', + '/map/pointcloud_map', + '/sensing/lidar/top/rectified/pointcloud', + '/sensing/imu/imu_data', + '/vehicle/status/twist', + ] +MESSAGE_TRACKING_TAG = 'topic_infos.pkl' +TIMER_SEC = 1.0 +TARGET_TOPIC = '/sensing/lidar/concatenated/pointcloud' +STOPS = [ + '/localization/pose_twist_fusion_filter/pose_with_covariance_without_yawbias', # noqa: #501 + '/perception/occupancy_grid_map/map', + '/perception/object_recognition/detection/detection_by_tracker/objects', + '/perception/object_recognition/tracking/objects', + '/localization/pose_twist_fusion_filter/biased_pose_with_covariance', + ] +DUMP_DIR = 'dump.d' + + +def truncate(s, left_length=20, n=80): + """ + Truncate string. + + :param s: string + :param left_length: first part length + :param n: total length + :return truncated string such that "abc...edf" + + """ + assert left_length + 5 < n + + if len(s) < n: + return s + + pre = s[:left_length] + post = s[len(s) - n + left_length + 5:] + + return pre + '...' + post + + +class LatencyStat(object): + """Latency statistics.""" + + def __init__(self): + """Initialize data.""" + self.dur_ms_list = [] + self.dur_pub_ms_list = [] + self.dur_pub_ms_steady_list = [] + self.is_leaf_list = [] + + def add(self, r): + """ + Add single result. + + :param r: message_tracking_tag_traverse.SolverResults + + """ + self.dur_ms_list.append(r.dur_ms) + self.dur_pub_ms_list.append(r.dur_pub_ms) + self.dur_pub_ms_steady_list.append(r.dur_pub_ms_steady) + self.is_leaf_list.append(r.is_leaf) + + def report(self): + """Report statistics.""" + dur_ms_list = self.dur_ms_list + is_leaf_list = self.is_leaf_list + dur_pub_ms_list = self.dur_pub_ms_list + dur_pub_ms_steady_list = self.dur_pub_ms_steady_list + + dur_min = float(min(dur_ms_list)) + dur_mean = float(mean(dur_ms_list)) + dur_max = float(max(dur_ms_list)) + + dur_pub_min = float(min(dur_pub_ms_list)) + dur_pub_mean = float(mean(dur_pub_ms_list)) + dur_pub_max = float(max(dur_pub_ms_list)) + + dur_pub_steady_min = float(min(dur_pub_ms_steady_list)) + dur_pub_steady_mean = float(mean(dur_pub_ms_steady_list)) + dur_pub_steady_max = float(max(dur_pub_ms_list)) + + is_all_leaf = all(is_leaf_list) + + return { + 'dur_min': dur_min, + 'dur_mean': dur_mean, + 'dur_max': dur_max, + 'dur_pub_min': dur_pub_min, + 'dur_pub_mean': dur_pub_mean, + 'dur_pub_max': dur_pub_max, + 'dur_pub_steady_min': dur_pub_steady_min, + 'dur_pub_steady_mean': dur_pub_steady_mean, + 'dur_pub_steady_max': dur_pub_steady_max, + 'is_all_leaf': is_all_leaf, + } + + +class PerTopicLatencyStat(object): + """Per topic latency statistics.""" + + def __init__(self): + """Initialize data.""" + self.data = {} + + def add(self, r): + """ + Add single result. + + :param r: message_tracking_tag_traverse.SolverResults + + """ + self.data.setdefault(r.topic, LatencyStat()).add(r) + + def report(self): + """Get report as dictionary.""" + ret = {} + for (topic, stat) in self.data.items(): + ret[topic] = stat.report() + return ret + + def print_report(self, printer): + """ + Print report. + + :param printer: see printer.py + + """ + logs = [] + reports = self.report() + logs.append('{:80} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6} {:>6}'.format( + 'topic', 'dur', 'dur', 'dur', 'e2e', 'e2e', 'e2e', 'e2e_s', 'e2e_s', 'e2e_s' + )) + + def p(v): + if v > 1000: + return ' inf' + else: + return '{:>6.1f}'.format(v) + + for (topic, report) in reports.items(): + s = f'{topic:80} ' + s += f'{p(report["dur_min"])} ' + s += f'{p(report["dur_mean"])} ' + s += f'{p(report["dur_max"])} ' + s += f'{p(report["dur_pub_min"])} ' + s += f'{p(report["dur_pub_mean"])} ' + s += f'{p(report["dur_pub_max"])} ' + s += f'{p(report["dur_pub_steady_min"])} ' + s += f'{p(report["dur_pub_steady_mean"])} ' + s += f'{p(report["dur_pub_steady_max"])} ' + s += f'{report["is_all_leaf"]}' + logs.append(s) + + printer.print_(logs) + + +def calc_one_hot(results): + """ + Calculate one hot result. + + :param results: see InputSensorStampSolver.solve2 + :return list of (depth, topic name, dur, dur_steady, is_leaf) + depth: depth of watched topic + topic_name: watched topic + dur: final topic pub_time - sensor pub_time + dur_steady: steady version of dur + is_leaf: leaf of not [bool] + stamp: target topic output stamp + + """ + last_pub_time = Time.from_msg(results.data[0].out_info.pubsub_stamp) + last_pub_time_steady = Time.from_msg( + results.data[0].out_info.pubsub_stamp_steady) + + def calc(node, depth): + name = node.name + is_leaf = node.num_children() == 0 + + if len(node.data) == 0 or node.data[0] is None: + return (depth, name, None, None, is_leaf, None) + + stamp = node.data[0].out_info.stamp + pub_time = Time.from_msg(node.data[0].out_info.pubsub_stamp) + dur = last_pub_time - pub_time + dur_ms = dur.nanoseconds // (10 ** 6) + + pub_time_steady = Time.from_msg( + node.data[0].out_info.pubsub_stamp_steady) + dur_steady = last_pub_time_steady - pub_time_steady + dur_ms_steady = dur_steady.nanoseconds // (10 ** 6) + + return (depth, name, dur_ms, dur_ms_steady, is_leaf, stamp) + + return results.apply_with_depth(calc) + + +def handle_stat(stamps, message_tracking_tags, target_topic, solver, stops, + dumps=False): + """ + Calculate latency statistics. + + :param stamps: stamp + :param message_tracking_tags: message tracking tag + :param target_topic: target topic + :param solver: InputSensorStampSolver + :param stops: list of stop targets + :param dumps: dump pkl file if True for debug + :return TreeNode. + .data: see calc_stat + + """ + idx = -3 + if len(stamps) == 0: + return + elif len(stamps) < 3: + idx = 1 + + merged = None + print(f'idx: {idx}') + for target_stamp in stamps[:idx]: + results = solver.solve2( + message_tracking_tags, target_topic, target_stamp, + stops=stops) + + if dumps: + pickle.dump(results, + open(f'{DUMP_DIR}/stat_results_{target_stamp}.pkl', + 'wb'), + protocol=pickle.HIGHEST_PROTOCOL) + + results = update_stat(results) + if merged is None: + merged = results + else: + merged.merge(results) + + stats = calc_stat(merged) + return stats + + +def update_stat(results): + """ + Update results to have durations. + + :param results: TreeNode see InputSensorStampSolver.solve2 + :return updated results with + results.data: [(dur_ms, dur_ms_steady)] + results.data_orig = results.data + + """ + last_pub_time = Time.from_msg( + results.data[0].out_info.pubsub_stamp) + last_pub_time_steady = Time.from_msg( + results.data[0].out_info.pubsub_stamp_steady) + + def _update_stat(node): + if len(node.data) == 0 or node.data[0] is None: + node.data_orig = node.data + node.data = [(None, None)] + return + + pub_time = Time.from_msg( + node.data[0].out_info.pubsub_stamp) + dur = last_pub_time - pub_time + dur_ms = dur.nanoseconds // (10 ** 6) + + pub_time_steady = Time.from_msg( + node.data[0].out_info.pubsub_stamp_steady) + dur_steady = last_pub_time_steady - pub_time_steady + dur_ms_steady = dur_steady.nanoseconds // (10 ** 6) + + node.data_orig = node.data + node.data = [(dur_ms, dur_ms_steady)] + + results.apply(_update_stat) + + return results + + +def calc_stat(results): + """ + Calculate statistics. + + :param results: TreeNode which is updated by update_stat + :return list of dictionary whose keys are: + "depth" + "name" + "dur_min" + "dur_mean" + "dur_max" + "dur_min_steady" + "dur_mean_steady" + "dur_max_steady" + "is_leaf" + + """ + def _calc_stat(node, depth): + duration = [] + duration_steady = [] + is_leaf = True if node.num_children() == 0 else False + for d in node.data: + if d[0] is not None: + duration.append(d[0]) + if d[1] is not None: + duration_steady.append(d[1]) + + def _calc(arr, fn): + if len(arr) == 0: + return None + return fn(arr) + + return { + 'depth': depth, + 'name': node.name, + 'dur_min': _calc(duration, min), + 'dur_mean': _calc(duration, mean), + 'dur_max': _calc(duration, max), + 'dur_min_steady': _calc(duration_steady, min), + 'dur_mean_steady': _calc(duration_steady, mean), + 'dur_max_steady': _calc(duration_steady, max), + 'is_leaf': is_leaf + } + + return results.apply_with_depth(_calc_stat) + + +class LatencyViewerNode(Node): + """Latency viewer node.""" + + def __init__(self, stdscr=None): + """ + Initialize Node. + + :param stdscr: if not None, use ncurses + + """ + super().__init__('latency_viewer_node') + self.declare_parameter('excludes_topics', EXCLUDES_TOPICS) + self.declare_parameter('leaves', LEAVES) + self.declare_parameter('graph_pkl', '') + self.declare_parameter('timer_sec', TIMER_SEC) + self.declare_parameter('target_topic', TARGET_TOPIC) + self.declare_parameter('keep_info_sec', 3) + self.declare_parameter('wait_sec_to_init_graph', 10) + self.declare_parameter('mode', 'stat') + self.declare_parameter('stops', STOPS) + # whether to dump solver.solve() result + self.declare_parameter('dumps', False) + + print(stdscr) + if stdscr is not None: + self.printer = NcursesPrinter(stdscr) + else: + self.printer = Printer() + + self.subs = {} + # topic vs the latest sequence numbers + self.topic_seq = {} + + excludes_topic = ( + self.get_parameter('excludes_topics') + .get_parameter_value().string_array_value) + topics = self.get_message_tracking_tag_topics(excludes=excludes_topic) + logs = [] + + qos = QoSProfile( + history=QoSHistoryPolicy.KEEP_LAST, + depth=3, + reliability=QoSReliabilityPolicy.BEST_EFFORT, + ) + for topic in topics: + logs.append(topic) + sub = self.create_subscription( + MessageTrackingTag, + topic, + self.listener_callback, + qos) + self.subs[topic] = sub + + self.solver = None + self.stops = ( + self.get_parameter('stops') + .get_parameter_value().string_array_value) + graph_pkl = ( + self.get_parameter('graph_pkl') + .get_parameter_value().string_value) + if graph_pkl: + graph = pickle.load(open(graph_pkl, 'rb')) + self.solver = InputSensorStampSolver(graph) + + self.message_tracking_tags = MessageTrackingTagsObj() + + timer_sec = ( + self.get_parameter('timer_sec') + .get_parameter_value().double_value) + self.timer = self.create_timer(timer_sec, + self.timer_callback) + + self.target_topic = ( + self.get_parameter('target_topic') + .get_parameter_value().string_value) + self.keep_info_sec = ( + self.get_parameter('keep_info_sec') + .get_parameter_value().integer_value) + self.wait_sec_to_init_graph = ( + self.get_parameter('wait_sec_to_init_graph'). + get_parameter_value().integer_value) + self.wait_init = 0 + + self.init_skips() + + self.printer.print_(logs) + + self.dumps = ( + self.get_parameter('dumps') + .get_parameter_value().bool_value) + if self.dumps: + os.makedirs(DUMP_DIR, exist_ok=True) + + def init_skips(self): + """ + Define skips. + + See TopicGraph.__init__ comment. + """ + skips = {} + RECT_OUT_EX = '/sensing/lidar/{}/rectified/pointcloud_ex' + RECT_OUT = '/sensing/lidar/{}/rectified/pointcloud' + RECT_IN = '/sensing/lidar/{}/mirror_cropped/pointcloud_ex' + + # top + for pos in ['top', 'left', 'right']: + skips[RECT_OUT_EX.format(pos)] = RECT_IN.format(pos) + skips[RECT_OUT.format(pos)] = RECT_IN.format(pos) + + skips['/sensing/lidar/no_ground/pointcloud'] = \ + '/sensing/lidar/concatenated/pointcloud' + + self.skips = skips + + def listener_callback(self, message_tracking_tag_msg): + """Handle MessageTrackingTag message.""" + st = time.time() + output_info = message_tracking_tag_msg.output_info + + topic = output_info.topic_name + stamp = time2str(output_info.header_stamp) + + this_seq = output_info.seq + if topic not in self.topic_seq.keys(): + self.topic_seq[topic] = this_seq + seq = self.topic_seq[topic] + + if this_seq < seq: # skew + s = f'skew topic={topic} ' + \ + f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ + f' saved_seq={seq}' + stamps = self.message_tracking_tags.stamps(topic) + if stamps or len(stamps) > 0: + last_stamp = sorted(stamps)[-1] + s += f'({last_stamp})' + self.get_logger().info(s) + elif seq + 1 < this_seq: # message drop happens + s = f'may drop topic={topic} ' + \ + f'msg_seq={this_seq}({time2str(output_info.header_stamp)})' + \ + f' saved_seq={seq}' + stamps = self.message_tracking_tags.stamps(topic) + if stamps or len(stamps) > 0: + last_stamp = sorted(stamps)[-1] + s += f'({last_stamp})' + self.get_logger().info(s) + self.topic_seq[topic] = this_seq + else: + self.topic_seq[topic] = this_seq + + # add message_tracking_tag + message_tracking_tag = MessageTrackingTagObj( + output_info.topic_name, + output_info.pub_time, + output_info.pub_time_steady, + output_info.has_header_stamp, + output_info.header_stamp) + for input_info in message_tracking_tag_msg.input_infos: + message_tracking_tag.add_input_info( + input_info.topic_name, + input_info.sub_time, + input_info.sub_time_steady, + input_info.has_header_stamp, + input_info.header_stamp) + self.message_tracking_tags.add(message_tracking_tag) + + et = time.time() + + elapsed_ms = (et - st) * 1000 + if elapsed_ms > 1: + self.get_logger().info( + f'sub {topic} at {stamp}@ {elapsed_ms} [ms]') + + def handle_stat(self, stamps): + """ + Report statistics. + + :param stamps: stamps sorted by old-to-new order + :return list of string + + """ + message_tracking_tags = self.message_tracking_tags + target_topic = self.target_topic + solver = self.solver + stops = self.stops + dumps = self.dumps + + stats = handle_stat(stamps, + message_tracking_tags, + target_topic, + solver, + stops, + dumps=dumps) + + logs = [] + + fmt = '{:80} ' + \ + '{:>6} {:>6} {:>6} ' + \ + '{:>6} {:>6} {:>6}' + logs.append(fmt.format( + 'topic', + 'min', 'mean', 'max', + 'min_s', 'mean_s', 'max_s' + )) + + for stat in stats: + name = (' ' * stat['depth'] + + stat['name'] + + ('*' if stat['is_leaf'] else '')) + name = truncate(name) + + def p(v): + if v is None: + s = 'NA' + return f'{s:>6}' + if v > 1000: + s = 'inf' + return f'{s:>6}' + else: + return f'{v:>6.1f}' + + s = f'{name:80} ' + s += f'{p(stat["dur_min"]):>6} ' + s += f'{p(stat["dur_mean"]):>6} ' + s += f'{p(stat["dur_max"]):>6} ' + s += f'{p(stat["dur_min_steady"]):>6} ' + s += f'{p(stat["dur_mean_steady"]):>6} ' + s += f'{p(stat["dur_max_steady"]):>6} ' + + logs.append(s) + + return logs + + def handle_one_hot(self, stamps): + """ + Report only one message. + + :param stamps: stamps sorted by old-to-new order + :return array of strings for log + + """ + message_tracking_tags = self.message_tracking_tags + target_topic = self.target_topic + solver = self.solver + stops = self.stops + + # [-3] has no specific meaning. + # But [-1] may not have full info. So we look somewhat old info. + idx = -3 + if len(stamps) == 0: + return + elif len(stamps) < 3: + idx = 0 + + target_stamp = stamps[idx] + results = solver.solve2(message_tracking_tags, target_topic, + target_stamp, stops=stops) + + if self.dumps: + pickle.dump(results, + open(f'{DUMP_DIR}/one_hot_results_{target_stamp}.pkl', + 'wb'), + protocol=pickle.HIGHEST_PROTOCOL) + + one_hot_durations = calc_one_hot(results) + logs = [] + for e in one_hot_durations: + (depth, name, dur_ms, dur_ms_steady, is_leaf, stamp) = e + name = truncate(' ' * depth + name + ('*' if is_leaf else '')) + if dur_ms is None: + dur_ms = 'NA' + if dur_ms_steady is None: + dur_ms_steady = 'NA' + stamp_s = 'NA' + if stamp: + stamp_s = time2str(stamp) + + s = f'{name:80} {stamp_s:>20} {dur_ms:>6} {dur_ms_steady:>6}' + logs.append(s) + + return logs + + def timer_callback(self): + """Clear old data.""" + st = time.time() + message_tracking_tags = self.message_tracking_tags + target_topic = self.target_topic + + stamps = sorted(message_tracking_tags.stamps(target_topic)) + logs = [] + str_stamp = stamps[0] if len(stamps) > 0 else '' + logs.append(f'stamps: {len(stamps)}, {str_stamp}') + if len(stamps) == 0: + self.printer.print_(logs) + return + + # check header.stamp field existence + if not message_tracking_tags.get(target_topic, stamps[0]).out_info.has_stamp: + logs.append('**WARNING** target topic has no stamp field') + self.printer.print_(logs) + return + + if not self.solver: + if self.wait_init < self.wait_sec_to_init_graph: + self.printer.print_(logs) + self.wait_init += 1 + return + logs.append('init solver') + graph = TopicGraph(message_tracking_tags, skips=self.skips) + self.solver = InputSensorStampSolver(graph) + + mode = self.get_parameter('mode').get_parameter_value().string_value + if mode == 'stat': + logs.extend(self.handle_stat(stamps)) + elif mode == 'one_hot': + logs.extend(self.handle_one_hot(stamps)) + else: + logs.append('unknown mode') + et1 = time.time() + handle_ms = (et1 - st) * 1000 + + # cleanup MessageTrackingTags + (latest_sec, latest_ns) = (int(x) for x in stamps[-1].split('.')) + until_stamp = TimeMsg(sec=latest_sec - self.keep_info_sec, + nanosec=latest_ns) + message_tracking_tags.erase_until(until_stamp) + et2 = time.time() + cleanup_ms = (et2 - et1) * 1000 + + self.printer.print_(logs) + + if handle_ms + cleanup_ms > 30: + s = f'timer handle_ms={handle_ms}' + \ + f' cleanup_ms={cleanup_ms}' + self.get_logger().info(s) + + def get_message_tracking_tag_topics(self, excludes=[]): + """ + Get all topic infos. + + :param excludes: topic names + :return a list of message_tracking_tag topics + + """ + # short sleep between node creation and get_topic_names_and_types + # https://github.com/ros2/ros2/issues/1057 + # We need this sleep with autoware, + # but don't need in dev environment(i.e. MessageTrackingTag in rosbag) + time.sleep(0.5) + + msg_type = 'tilde_msg/msg/MessageTrackingTag' + topic_and_types = self.get_topic_names_and_types() + filtered_topic_and_types = \ + filter(lambda x: msg_type in x[1], topic_and_types) + topics = (x[0] for x in filtered_topic_and_types) + topics = filter(lambda x: x not in excludes, topics) + + return topics + + +def main_curses(stdscr, args=None): + """Wrap main function for ncurses.""" + rclpy.init(args=args) + + node = LatencyViewerNode(stdscr=stdscr) + rclpy.spin(node) + rclpy.shutdown() + + +def main(args=None): + """Run main.""" + parser = argparse.ArgumentParser() + parser.add_argument( + '--batch', action='store_true', + help='Run as batch mode, just like top command `-b` option') + parsed = parser.parse_known_args()[0] + + print(parsed) + + if not parsed.batch: + print('not batch') + curses.wrapper(main_curses, args=sys.argv) + else: + print('batch') + main_curses(None, args=sys.argv) + + +if __name__ == '__main__': + """Main.""" + main(sys.argv) diff --git a/src/tilde_vis/tilde_vis/message_tracking_tag.py b/src/tilde_vis/tilde_vis/message_tracking_tag.py new file mode 100644 index 00000000..a8c46aa5 --- /dev/null +++ b/src/tilde_vis/tilde_vis/message_tracking_tag.py @@ -0,0 +1,269 @@ +# Copyright 2021 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. + +"""Internal data structures for MessageTrackingTag.""" + +from rclpy.time import Time + + +def time2str(t): + """Convert builtin_interfaces.msg.Time to string.""" + return f'{t.sec}.{t.nanosec:09d}' + + +class TopicInfo(object): + """Represent output_info and input_infos of MessageTrackingTag message.""" + + def __init__(self, topic, pubsub_stamp, pubsub_stamp_steady, + has_stamp, stamp): + """ + Hold information similar to MessageTrackingTag in/out info. + + :param topic: target topic [string] + :param pubsub_stamp: + when publish or subscription callback is called + [builtin_interfaces.msg.Time] + :param pubsub_stamp_steady: + same with above but in steady time + [builtin_interfaces.mg.Time] + :param has_stamp: whether main topic has header.stamp or not [bool] + :param stamp: header.stamp [builtin_interfaces.msg.Time] + + """ + self.topic = topic + self.pubsub_stamp = pubsub_stamp + self.pubsub_stamp_steady = pubsub_stamp_steady + self.has_stamp = has_stamp + self.stamp = stamp + + def __str__(self): + """Get string.""" + stamp_s = time2str(self.stamp) if self.has_stamp else 'NA' + return f'TopicInfo(topic={self.topic}, stamp={stamp_s})' + + +class MessageTrackingTag(object): + """ + Hold information similar to MessageTrackingTagMsg. + + TODO(y-okumura-isp): Can we use MessageTrackingTagMsg directly? + + """ + + def __init__(self, out_topic, pub_time, pub_time_steady, + has_stamp, out_stamp): + """ + Initialize data. + + :param out_topic: topic name + :param pub_time: when publish main topic [builtin_interfaces.msg.Time] + :param has_stamp: whether main topic has header.stamp [bool] + :param out_stamp: main topic header.stamp [builtin_interfaces.msg.Time] + + """ + self.out_info = TopicInfo(out_topic, pub_time, pub_time_steady, + has_stamp, out_stamp) + # topic name vs TopicInfo + self.in_infos = {} + + def add_input_info(self, in_topic, sub_stamp, sub_stamp_steady, + has_stamp, stamp): + """ + Add input info. + + :param in_topic: topic string + :param has_stamp: bool + :param stamp: header stamp [builtin_interfaces.msg.Time] + + """ + info = TopicInfo(in_topic, sub_stamp, sub_stamp_steady, + has_stamp, stamp) + self.in_infos.setdefault(in_topic, []).append(info) + + def __str__(self): + """Get string.""" + s = 'MessageTrackingTag: \n' + s += f' out_info={self.out_info}\n' + for _, infos in self.in_infos.items(): + for info in infos: + s += f' in_infos={info}\n' + return s + + @property + def out_topic(self): + """Get output topic.""" + return self.out_info.topic + + @staticmethod + def fromMsg(message_tracking_tag_msg): + """ + Convert MessageTrackingTagMsg to MessageTrackingTag. + + :param message_tracking_tag_msg: MessageTrackingTagMsg + :return MessageTrackingTag + + """ + output_info = message_tracking_tag_msg.output_info + message_tracking_tag = MessageTrackingTag( + output_info.topic_name, + output_info.pub_time, + output_info.pub_time_steady, + output_info.has_header_stamp, + output_info.header_stamp) + for input_info in message_tracking_tag_msg.input_infos: + message_tracking_tag.add_input_info( + input_info.topic_name, + input_info.sub_time, + input_info.sub_time_steady, + input_info.has_header_stamp, + input_info.header_stamp) + return message_tracking_tag + + +class MessageTrackingTags(object): + """ + Hold topic vs MessageTrackingTag. + + We have double-key dictionary internally, i.e. + we can get MessageTrackingTag by + topic_vs_message_tracking_tag[topic_name][stamp]. + + """ + + def __init__(self): + """Initialize data.""" + # {topic => {stamp_str => MessageTrackingTag}} + self.topic_vs_message_tracking_tags = {} + + def add(self, message_tracking_tag): + """Add a MessageTrackingTag.""" + out_topic = message_tracking_tag.out_info.topic + out_stamp = time2str(message_tracking_tag.out_info.stamp) + if out_topic not in self.topic_vs_message_tracking_tags.keys(): + self.topic_vs_message_tracking_tags[out_topic] = {} + infos = self.topic_vs_message_tracking_tags[out_topic] + + if out_stamp not in infos.keys(): + infos[out_stamp] = {} + + infos[out_stamp] = message_tracking_tag + + def erase_until(self, stamp): + """ + Erase added message_tracking_tag where out_stamp < stamp. + + :param stamp: builtin_interfaces.msg.Time + + """ + def time_ge(lhs, rhs): + """ + Compare time. + + Helper function to compare stamps i.e. + self.topic_vs_message_tracking_tags[*].keys(). + + As stamps are string, it is not appropriate to compare as string. + Builtin_msg.msg.Time does not implement `<=>`, we use rclpy.Time, + although clock_type has no meaning. + + """ + lhs_sec, lhs_nano_sec = (int(x) for x in lhs.split('.')) + rhs_sec, rhs_nano_sec = (int(x) for x in rhs.split('.')) + lhs_time = Time(seconds=lhs_sec, nanoseconds=lhs_nano_sec) + rhs_time = Time(seconds=rhs_sec, nanoseconds=rhs_nano_sec) + return lhs_time <= rhs_time + + thres_stamp = time2str(stamp) + erases = {} + for (topic, infos) in self.topic_vs_message_tracking_tags.items(): + for stamp in infos.keys(): + if time_ge(thres_stamp, stamp): + continue + erases.setdefault(topic, []).append(stamp) + + for (topic, stamps) in erases.items(): + for stamp in stamps: + del self.topic_vs_message_tracking_tags[topic][stamp] + + def topics(self): + """Get topic names which has registered MessageTrackingTag.""" + return list(self.topic_vs_message_tracking_tags.keys()) + + def stamps(self, topic): + """Get List[stamps].""" + if topic not in self.topic_vs_message_tracking_tags.keys(): + return [] + + return list(self.topic_vs_message_tracking_tags[topic].keys()) + + def get(self, topic, stamp=None, idx=None): + """ + Get a MessageTrackingTag. + + :param topic: str + :param stamp: str such as 1618559286.884563157 + :return MessageTrackingTag or None + + """ + ret = None + if topic not in self.topic_vs_message_tracking_tags.keys(): + return None + + if stamp is None and idx is None: + return list(self.topic_vs_message_tracking_tags[topic].values())[0] + + infos = self.topic_vs_message_tracking_tags[topic] + + if stamp in infos.keys(): + ret = infos[stamp] + + if idx is not None: + if len(infos.keys()) <= idx: + print('args.idx too large, should be < {len(info.kens())}') + else: + key = sorted(infos.keys())[idx] + ret = infos[key] + + return ret + + def in_topics(self, topic): + """ + Gather input topics by ignoring stamps. + + :param topic: topic name + :return set of topic name + + """ + ret = set() + + if topic not in self.topic_vs_message_tracking_tags.keys(): + return ret + + for message_tracking_tag in self.topic_vs_message_tracking_tags[topic].values(): + for t in message_tracking_tag.in_infos.keys(): + ret.add(t) + return ret + + def all_topics(self): + """Get all topics which are used as input or output.""" + out = set() + + lhs = self.topics() + for t in lhs: + out.add(t) + + rhs = self.in_topics(t) + for t in rhs: + out.add(t) + return out diff --git a/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py b/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py new file mode 100755 index 00000000..68da39bc --- /dev/null +++ b/src/tilde_vis/tilde_vis/message_tracking_tag_traverse.py @@ -0,0 +1,462 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +"""MessageTrackingTag Traverser.""" + +import argparse +from collections import defaultdict, deque +import json +import pickle +import time + +from rclpy.time import Time + +from tilde_vis.data_as_tree import TreeNode +from tilde_vis.message_tracking_tag import time2str + + +def str_stamp2time(str_stamp): + """Convert string time to Time.""" + sec, nanosec = str_stamp.split('.') + return Time(seconds=int(sec), nanoseconds=int(nanosec)) + + +class SolverResult(object): + """SolverResult.""" + + def __init__(self, topic, stamp, dur_ms, + dur_pub_ms, dur_pub_ms_steady, + is_leaf, parent): + """ + Initialize data. + + :param topic: topic [string] + :param stamp: header.stamp [rclpy.Time] + :param dur_ms: duration of header.stamp in ms [double] + :param dur_pub_ms: duration of output.pub_time in ms [double] + :param dur_pub_ms_steady: steady clock ms [double] + :param is_leaf: bool + :param parent: parent topic [string] + + """ + self.topic = topic + self.stamp = stamp + self.dur_ms = dur_ms + self.dur_pub_ms = dur_pub_ms + self.dur_pub_ms_steady = dur_pub_ms_steady + self.is_leaf = is_leaf + self.parent = parent + + +class SolverResultsPrinter(object): + """SolverResultsPrinter.""" + + @classmethod + def as_tree(cls, results): + """ + Construct array of string to print. + + This method returns tree command like expression + from output topic to source topics. + Multiple input topics are shown by indented listing. + + Example: + ------- + topic dur e2e + out_topic: + in_topic1 0.1 0.1 + in_topic1_1 0.2 0.2 + in_topic2 0.3 0.3 + + :param results: SolverResults + :return array of string + + """ + pass + + +class SolverResults(object): + """SolverResults.""" + + def __init__(self): + """Initialize data.""" + self.data = [] # list of Result + + def add(self, *args): + """ + Register Result. + + Parameters + ---------- + args: completely forward. See Result.__init__. + + """ + self.data.append(SolverResult(*args)) + + +class InputSensorStampSolver(object): + """InputSensorStampSolver.""" + + def __init__(self, graph): + """Initialize solver.""" + # {topic: {stamp: {sensor_topic: [stamps]}}} + self.topic_stamp_to_sensor_stamp = {} + self.graph = graph + self.skips = graph.skips + self.empty_results = {} + + def solve(self, message_tracking_tags, tgt_topic, tgt_stamp, + stops=[]): + """ + Traverse from (tgt_topic, tgt_stamp)-message. + + :param message_tracking_tags: MessageTrackingTags + :param tgt_topic: target topic + :param tgt_stamp: target stamp(str) + :param stops: list of stop topics to prevent loop + :return SolverResults + + Calculate topic_stamp_to_sensor_stamp internally. + + """ + graph = self.graph + path_bfs = graph.bfs_rev(tgt_topic) + is_leaf = {t: b for (t, b) in path_bfs} + skips = self.skips + + stamp = tgt_stamp + + # dists[topic][stamp] + dists = defaultdict(lambda: defaultdict(lambda: -1)) + queue = deque() + parentQ = deque() + + start = str_stamp2time(tgt_stamp) + start_message_tracking_tag = message_tracking_tags.get(tgt_topic, tgt_stamp) + start_pub_time = start_message_tracking_tag.out_info.pubsub_stamp + start_pub_time_steady = start_message_tracking_tag.out_info.pubsub_stamp_steady + + dists[tgt_topic][tgt_stamp] = 1 + queue.append((tgt_topic, tgt_stamp, + start_pub_time, start_pub_time_steady)) + parentQ.append('') + + ret = SolverResults() + while len(queue) != 0: + topic, stamp, sub_time, sub_time_steady = queue.popleft() + parent = parentQ.popleft() + + dur = start - str_stamp2time(stamp) + dur_ms = dur.nanoseconds // 10**6 + + dur_pub = Time.from_msg(start_pub_time) - Time.from_msg(sub_time) + dur_pub_ms = dur_pub.nanoseconds // 10**6 + + dur_pub_steady = \ + Time.from_msg(start_pub_time_steady) - \ + Time.from_msg(sub_time_steady) + dur_pub_ms_steady = dur_pub_steady.nanoseconds // 10**6 + + ret.add(topic, stamp, dur_ms, + dur_pub_ms, dur_pub_ms_steady, + is_leaf[topic], parent) + + # NDT-EKF has loop, so stop + if topic in stops: + continue + + # get next edges + next_message_tracking_tag = message_tracking_tags.get(topic, stamp) + if not next_message_tracking_tag: + continue + + for in_infos in next_message_tracking_tag.in_infos.values(): + for in_info in in_infos: + nx_topic = in_info.topic + if nx_topic in skips: + nx_topic = skips[nx_topic] + nx_stamp = time2str(in_info.stamp) + if dists[nx_topic][nx_stamp] > 0: + continue + dists[nx_topic][nx_stamp] = dists[topic][stamp] + 1 + queue.append((nx_topic, nx_stamp, + in_info.pubsub_stamp, + in_info.pubsub_stamp_steady)) + parentQ.append(topic) + + return ret + + def solve2(self, message_tracking_tags, tgt_topic, tgt_stamp, + stops=[]): + """ + Traverse DAG from output to input. + + :param message_tracking_tags: message_tracking_tags + :param tgt_topic: output topic [string] + :param tgt_stamp: output header stamp [string] + :param stops: stop list + :return TreeNode + - Tree structure represents TopicGraph. + This means that even if some MessageTrackingTags loss + in some timing, + returned TreeNode preserve entire graph. + - .name means topic + - .data is MessageTrackingTag of the topic. + [] whn MessageTrackingTag loss + + """ + skips = self.skips + stamp = tgt_stamp + key = tgt_topic + '.'.join(stops) + if key not in self.empty_results: + self.empty_results[key] = self._solve_empty(tgt_topic, stops=stops) + empty_results = self.empty_results[key] + + queue = deque() + + root_results = TreeNode(tgt_topic) + root_results.merge(empty_results) # setup entire graph + + queue.append((tgt_topic, tgt_stamp, root_results)) + while len(queue) != 0: + topic, stamp, current_result = queue.popleft() + + next_message_tracking_tag = message_tracking_tags.get(topic, stamp) + if next_message_tracking_tag is not None: + current_result.add_data(next_message_tracking_tag) + else: + # print(f"message_tracking_tag of {topic} {stamp} not found") + continue + + # NDT-EKF has loop, so stop + if topic in stops: + continue + + for in_infos in next_message_tracking_tag.in_infos.values(): + for in_info in in_infos: + nx_topic = in_info.topic + if nx_topic in skips: + nx_topic = skips[nx_topic] + nx_stamp = time2str(in_info.stamp) + next_result = current_result.get_child(nx_topic) + + queue.append((nx_topic, nx_stamp, + next_result)) + + return root_results + + def append(self, topic, stamp, sensor_topic, sensor_stamp): + """Append results to topic_stamp_to_sensor_stamp.""" + dic = self.topic_stamp_to_sensor_stamp + if topic not in dic.keys(): + dic[topic] = {} + if stamp not in dic[topic].keys(): + dic[topic][stamp] = {} + if sensor_topic not in dic[topic][stamp].keys(): + dic[topic][stamp][sensor_topic] = [] + + dic[topic][stamp][sensor_topic].append(sensor_stamp) + + def _solve_empty(self, tgt_topic, + stops=[]): + """ + Get empty results to know graph. + + :param tgt_topic: output topic [string] + :param stops: stop list + :return TreeNode + + """ + skips = self.skips + graph = self.graph + + queue = deque() + root_results = TreeNode(tgt_topic) + + queue.append((tgt_topic, root_results)) + while len(queue) != 0: + topic, current_result = queue.popleft() + + if topic in stops: + continue + + rev_topics = graph.rev_topics(topic) + for nx_topic in rev_topics: + if nx_topic in skips: + nx_topic = skips[nx_topic] + + next_result = current_result.get_child(nx_topic) + queue.append((nx_topic, next_result)) + + return root_results + + +class TopicGraph(object): + """Construct topic graph by ignoring stamps.""" + + def __init__(self, message_tracking_tags, skips={}): + """ + Initialize graph. + + :param message_tracking_tags: MessageTrackingTags + :param skips: skip topics. {downstream: upstream} + by input-to-output order + ex) {"/sensing/lidar/top/rectified/pointcloud_ex": + "/sensing/lidar/top/mirror_cropped/pointcloud_ex"} + + """ + self.topics = sorted(message_tracking_tags.all_topics()) + self.t2i = {t: i for i, t in enumerate(self.topics)} + self.skips = skips + n = len(self.topics) + + # edges from publisher to subscription + self.topic_edges = [set() for _ in range(n)] + # edges from subscription to publisher + self.rev_edges = [set() for _ in range(n)] + for out_topic in self.topics: + in_topics = message_tracking_tags.in_topics(out_topic) + + out_id = self.t2i[out_topic] + for in_topic in in_topics: + if in_topic in skips.keys(): + in_topic = skips[in_topic] + in_id = self.t2i[in_topic] + self.topic_edges[in_id].add(out_id) + self.rev_edges[out_id].add(in_id) + + def dump(self, fname): + """Dump.""" + out = {} + out['topics'] = self.topics + out['topic2id'] = self.t2i + out['topic_edges'] = [list(edge) for edge in self.topic_edges] + out['rev_edges'] = [list(edge) for edge in self.rev_edges] + + json.dump(out, open(fname, 'wt')) + + def rev_topics(self, topic): + """ + Get input topics of the target topic. + + :param topic: topic name + :return List[Topic] + + """ + out_topic_idx = self.t2i[topic] + return [self.topics[i] for i in self.rev_edges[out_topic_idx]] + + def dfs_rev(self, start_topic): + """ + Do depth first search from the topic. + + :param start_topic: topic name + :return topic names in appearance order + + """ + edges = self.rev_edges + n = len(edges) + seen = [False for _ in range(n)] + sid = self.t2i[start_topic] + + ret = [] + + def dfs(v): + ret.append(self.topics[v]) + seen[v] = True + for nxt in edges[v]: + if seen[nxt]: + continue + dfs(nxt) + dfs(sid) + + return ret + + def bfs_rev(self, start_topic): + """ + Do breadth first search from the topic. + + :param start_topic: topic name + :return topic names in appearance order + + """ + edges = self.rev_edges + n = len(edges) + dist = [-1 for _ in range(n)] + queue = deque() + + sid = self.t2i[start_topic] + dist[sid] = 0 + queue.append(sid) + paths = [] + + while len(queue) != 0: + v = queue.popleft() + paths.append((self.topics[v], len(edges[v]) == 0)) + for nv in edges[v]: + if dist[nv] >= 0: + continue + dist[nv] = dist[v] + 1 + queue.append(nv) + + return paths + + +def run(args): + """Run.""" + pickle_file = args.pickle_file + message_tracking_tags = pickle.load(open(pickle_file, 'rb')) + + tgt_topic = args.topic + tgt_stamp = sorted(message_tracking_tags.stamps(tgt_topic))[args.stamp_index] + + graph = TopicGraph(message_tracking_tags) + + print('dump') + graph.dump('graph.json') + pickle.dump(graph, open('graph.pkl', 'wb'), + protocol=pickle.HIGHEST_PROTOCOL) + + # bfs_path = graph.bfs_rev(tgt_topic) + # print(f"BFS from {tgt_topic}") + # for p, is_leaf in bfs_path: + # print(f" {p} {is_leaf}") + # print("") + + st = time.time() + solver = InputSensorStampSolver(graph) + solver.solve(message_tracking_tags, tgt_topic, tgt_stamp) + et = time.time() + + print(f'solve {(et-st) * 1000} [ms]') + + +def main(): + """Run main.""" + parser = argparse.ArgumentParser() + parser.add_argument('pickle_file') + parser.add_argument('stamp_index', type=int, default=0, + help='header stamp index') + parser.add_argument('topic', + default='/sensing/lidar/no_ground/pointcloud', + nargs='?') + + args = parser.parse_args() + + run(args) + + +if __name__ == '__main__': + main() diff --git a/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py b/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py new file mode 100755 index 00000000..0addd0b8 --- /dev/null +++ b/src/tilde_vis/tilde_vis/parse_message_tracking_tag.py @@ -0,0 +1,99 @@ +#!/usr/bin/python3 +# Copyright 2021 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. + +"""Convert rosbag of MessageTrackingTag to pkl file.""" + +import argparse +import pickle + +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message + +from tilde_vis.message_tracking_tag import MessageTrackingTag, MessageTrackingTags + + +def get_rosbag_options(path, serialization_format='cdr'): + """Get rosbag options.""" + storage_options = rosbag2_py.StorageOptions(uri=path, storage_id='sqlite3') + + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + return storage_options, converter_options + + +def run(args): + """Do main loop.""" + bag_path = args.bag_path + + storage_options, converter_options = get_rosbag_options(bag_path) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + # Create a map for quicker lookup + type_map = {topic_types[i].name: topic_types[i].type + for i in range(len(topic_types))} + + # topic => list of record + out_per_topic = MessageTrackingTags() + + skip_topic_vs_count = {} + + cnt = 0 + while reader.has_next(): + if 0 <= args.cnt and args.cnt < cnt: + break + + (topic, data, t) = reader.read_next() + # TODO: need more accurate check + if '/message_tracking_tag' not in topic: + continue + + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + + message_tracking_tag = MessageTrackingTag.fromMsg(msg) + out_per_topic.add(message_tracking_tag) + + if 0 < cnt and cnt % 1000 == 0: + print(cnt) + cnt += 1 + + pickle.dump(out_per_topic, open('topic_infos.pkl', 'wb'), + protocol=pickle.HIGHEST_PROTOCOL) + + for topic, count in skip_topic_vs_count.items(): + print(f'skipped {topic} {count} times') + + +def main(): + """Run main.""" + parser = argparse.ArgumentParser() + parser.add_argument('bag_path') + parser.add_argument( + '--cnt', type=int, default=-1, + help='number of messages to dump (whole */message_tracking_tag, not per topic)') + args = parser.parse_args() + + run(args) + + +if __name__ == '__main__': + """Main.""" + main() diff --git a/src/tilde_vis/tilde_vis/pathnode_vis.py b/src/tilde_vis/tilde_vis/pathnode_vis.py new file mode 100644 index 00000000..4bb4fa6f --- /dev/null +++ b/src/tilde_vis/tilde_vis/pathnode_vis.py @@ -0,0 +1,202 @@ +#!env python3 +# Copyright 2021 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. + +"""LatencyViewer PoC version. Deprecated.""" + +from collections import defaultdict +from logging import basicConfig, getLogger +import os + +import numpy as np + +import rclpy +from rclpy.node import Node +from rclpy.time import Time + +from tilde_msg.msg import TopicInfo + +basicConfig(level=os.getenv('LOG_LEVEL', 'INFO')) +logger = getLogger(__name__) + +# for subscriber demo +LIDAR_PREPROCESS = [ + '/sensing/lidar/top/self_cropped/pointcloud_ex', + # subscriber is /sensing/lidar/top/velodyne_interpolate_node + # which is not autoware node + # '/sensing/lidar/top/mirror_cropped/pointcloud_ex', + '/sensing/lidar/top/rectified/pointcloud_ex', + '/sensing/lidar/top/outlier_filtered/pointcloud', + '/sensing/lidar/concatenated/pointcloud', + '/sensing/lidar/measurement_range_cropped/pointcloud', +] + +# for publisher demo +LIDAR_PREPROCESS_PUB = [ + '/sensing/lidar/top/self_cropped/pointcloud_ex', + '/sensing/lidar/top/mirror_cropped/pointcloud_ex', + # publisher is /sensing/lidar/top/velodyne_interpolate_node + # which is not autoware node + # '/sensing/lidar/top/rectified/pointcloud_ex', + '/sensing/lidar/top/outlier_filtered/pointcloud', + '/sensing/lidar/concatenated/pointcloud', + '/sensing/lidar/measurement_range_cropped/pointcloud', +] + + +class TopicInfoStatistics(object): + """TopicInfo statistics.""" + + def __init__(self, topics, max_rows=10): + """Initialize statistics data.""" + self.topics = topics + self.t2i = {topic: i for i, topic in enumerate(topics)} + self.seq2time = defaultdict( + lambda: - np.ones(len(self.t2i), dtype=np.float64)) + # (n_messages, n_sub_callbacks), nanoseconds + self.data = - np.ones((max_rows, len(topics)), dtype=np.float) + self.data_idx = 0 + self.max_rows = max_rows + self.num_dump = -1 + + s = '' + for t in topics: + s += f'{t} ' + s = s.rstrip() + s = s.replace(' ', ' -> ') + print(s) + + def register(self, seq, topic, callback_start_time): + """Register result.""" + vec = self.seq2time[seq] + i = self.t2i[topic] + vec[i] = callback_start_time + logger.debug('seq {}: i: {} non zero: {}'.format( + seq, i, np.count_nonzero(vec > 0))) + if (np.count_nonzero(vec > 0) == len(self.t2i) and + self.data_idx < self.max_rows): + self.data[self.data_idx, ...] = vec[...] + del self.seq2time[seq] + self.data_idx += 1 + + # TODO: delete too old seq key (now leaks) + + def is_filled(self): + """Check data.""" + return self.data_idx == self.max_rows + + def dump_and_clear(self, dumps=False): + """Dump and clear results.""" + if dumps: + self.num_dump += 1 + fname = '{}.npy'.format(self.num_dump) + np.save(fname, self.data) + + # TODO: ignore nan(-1) field + data = self.data[self.data.min(axis=1) > 0] + + nano2msec = 1000 * 1000 + data /= nano2msec + + diff = data[:, 1:] - data[:, :-1] + e2e = data[:, -1] - data[:, 0] + + max_time = diff.max(axis=0) + avg_time = diff.mean(axis=0) + min_time = diff.min(axis=0) + max_e2e = e2e.max() + avg_e2e = e2e.mean() + min_e2e = e2e.min() + + def fmt(vec): + s = '' + for v in vec: + s += f'{v:5.1f} ' + return s.rstrip() + + print('max: ' + fmt(max_time)) + print('avg: ' + fmt(avg_time)) + print('min: ' + fmt(min_time)) + print('e2e: ' + fmt([max_e2e, avg_e2e, min_e2e])) + print('') + + self.data[...] = -1.0 + self.data_idx = 0 + + +class PathVisNode(Node): + """Path visualization node.""" + + def __init__(self): + """Initialize.""" + super().__init__('path_vis_node') + self.declare_parameter('topics', LIDAR_PREPROCESS) + self.declare_parameter('window', 10) + self.declare_parameter('dump', False) + self.declare_parameter('watches_pub', False) + + topics = self.get_parameter('topics') \ + .get_parameter_value() \ + .string_array_value + window = self.get_parameter('window') \ + .get_parameter_value() \ + .integer_value + watches_pub = self.get_parameter('watches_pub') \ + .get_parameter_value() \ + .bool_value + info_name = '/info/sub' + if watches_pub: + info_name = '/message_tracking_tag' + topics = LIDAR_PREPROCESS_PUB + + self.statistics = TopicInfoStatistics(topics, window) + self.subs = [] + + logger.debug('info_name: {}'.format(info_name)) + + for topic in topics: + sub = self.create_subscription( + TopicInfo, + topic + info_name, + self.listener_callback, + 1) + + self.subs.append(sub) + + def listener_callback(self, topic_info): + """Store data and print results.""" + seq = topic_info.seq + topic = topic_info.topic_name + start_time = Time.from_msg(topic_info.callback_start).nanoseconds + dump = self.get_parameter('dump').get_parameter_value().bool_value + + logger.debug('{} {}'.format(seq, topic)) + self.statistics.register(seq, topic, start_time) + if self.statistics.is_filled(): + self.statistics.dump_and_clear(dump) + + +def main(args=None): + """Run PathVisNode.""" + rclpy.init(args=args) + node = PathVisNode() + + rclpy.spin(node) + + rclpy.shutdown() + + +if __name__ == '__main__': + """Main.""" + main() diff --git a/src/tilde_vis/tilde_vis/printer.py b/src/tilde_vis/tilde_vis/printer.py new file mode 100644 index 00000000..ff0c5519 --- /dev/null +++ b/src/tilde_vis/tilde_vis/printer.py @@ -0,0 +1,215 @@ +# Copyright 2021 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. + +"""Printer for stdout w/wo interactions.""" + +import curses +import datetime +import re + + +class Printer(object): + """Print messages to stdout.""" + + def print_(self, lines): + """Print logs.""" + for s in lines: + print(s) + + +class NcursesPrinter(object): + """Print messages with interaction.""" + + MODE_STOP = 0 + MODE_RUN = 1 + MODE_REGEXP = 2 + + UP = curses.KEY_UP + DOWN = curses.KEY_DOWN + PPAGE = curses.KEY_PPAGE + NPAGE = curses.KEY_NPAGE + + def __init__(self, stdscr): + """Initialize.""" + stdscr.scrollok(True) + stdscr.timeout(0) # set non-blocking + self.stdscr = stdscr + + size = stdscr.getmaxyx() + self.y_max = size[0] + self.x_max = size[1] + + # mode line colors + curses.init_pair(1, curses.COLOR_BLACK, curses.COLOR_CYAN) + curses.init_pair(2, curses.COLOR_BLACK, curses.COLOR_WHITE) + self.CYAN = curses.color_pair(1) + self.WHITE = curses.color_pair(2) + + self.mode = self.MODE_RUN + + # showing lines + self.lines = [] + self.start_line = 0 + + # regexp mode + self.regexp_ptn = '' + self.search = None + + self.adjust_keys = ( + self.DOWN, + ord('j'), + self.NPAGE, + ord('d'), + + self.UP, + ord('k'), + self.PPAGE, + ord('u'), + + ord('g'), + ord('G'), + ) + + self.run_keys = ( + ord('q'), + ord('F') + ) + + self.regex_keys = ( + ord('r'), + ord('/') + ) + + def adjust_lines(self, k, lines): + """Move lines by interaction.""" + step = 0 + if k in (self.DOWN, ord('j')): + step = 1 + elif k in (self.NPAGE, ord('d')): + step = 10 + elif k in (self.UP, ord('k')): + step = -1 + elif k in (self.PPAGE, ord('u')): + step = -10 + + self.start_line += step + + if k == ord('g'): + self.start_line = 0 + elif k == ord('G'): + self.start_line = len(lines) - self.y_max + 2 + + if self.start_line < 0: + self.start_line = 0 + elif len(lines) <= self.start_line: + self.start_line = len(lines) - 1 + + def enter_regex_mode(self): + """Change mode.""" + self.mode = self.MODE_REGEXP + + def print_(self, input_lines): + """Print logs.""" + stdscr = self.stdscr + lines = self.lines + + keys = [] + k = stdscr.getch() + while k >= 0: + keys.append(k) + k = stdscr.getch() + + if len(keys) == 0 and self.mode == self.MODE_RUN: + self.lines = input_lines + lines = self.lines + + if len(self.regexp_ptn) > 0: + lines = list(filter( + lambda x: self.search.search(x), + lines)) + + for k in keys: + if self.mode == self.MODE_STOP: + if k in self.run_keys: + self.mode = self.MODE_RUN + self.lines = input_lines + elif k in self.adjust_keys: + self.adjust_lines(k, lines) + elif k in self.regex_keys: + self.enter_regex_mode() + else: + return + elif self.mode == self.MODE_RUN: + if k in self.adjust_keys: + self.adjust_lines(k, lines) + self.mode = self.MODE_STOP + elif k in self.regex_keys: + self.enter_regex_mode() + else: + # update displayed lines + self.lines = input_lines + lines = self.lines + elif self.mode == self.MODE_REGEXP: + if ord(' ') <= k <= ord('~'): + self.regexp_ptn += chr(k) + self.search = re.compile(self.regexp_ptn) + self.start_line = 0 + elif k == curses.KEY_BACKSPACE: + self.regexp_ptn = self.regexp_ptn[:-1] + self.search = re.compile(self.regexp_ptn) + self.start_line = 0 + elif k in (curses.KEY_ENTER, ord('\n')): + self.mode = self.MODE_RUN + else: + raise Exception('unknown mode') + + stdscr.clear() + + self.print_mode() + for i, s in enumerate(lines[self.start_line:]): + if i + 1 == self.y_max - 1: + break + + if s[-1] != '\n': + s += '\n' + lineno = self.start_line + i + stdscr.addstr(i+1, 0, f'{lineno:<3}| ') + stdscr.addstr(i+1, 5, s) + stdscr.refresh() + + def print_mode(self): + """Print mode line.""" + stdscr = self.stdscr + color = self.CYAN + + t = datetime.datetime.now().strftime('%Y/%m/%d %H:%M:%S') + + mode_str = '' + if self.mode == self.MODE_RUN: + mode_str = \ + 'press UP/DOWN/PgUp/PgDown/j/k/d/u/g/G to scroll,' + \ + 'r or "/" to filter' + color = self.WHITE + elif self.mode == self.MODE_STOP: + mode_str = 'scrolling... Press q/F to periodically update' + color = self.CYAN + elif self.mode == self.MODE_REGEXP: + mode_str = 'Regexp: ' + self.regexp_ptn + '_' + color = self.CYAN + + s = mode_str + s += ' ' * (self.x_max - len(mode_str) - len(t)) + s += t + + stdscr.addstr(0, 0, s, color) diff --git a/src/tilde_vis_test/CMakeLists.txt b/src/tilde_vis_test/CMakeLists.txt new file mode 100644 index 00000000..53329ab0 --- /dev/null +++ b/src/tilde_vis_test/CMakeLists.txt @@ -0,0 +1,61 @@ +cmake_minimum_required(VERSION 3.8) +project(tilde_vis_test) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_components REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(tilde REQUIRED) + +add_library(tilde_vis_test SHARED + src/tilde_vis_test.cpp) +ament_target_dependencies(tilde_vis_test + "rclcpp" + "rclcpp_components" + "std_msgs" + "tilde" + "sensor_msgs") + +rclcpp_components_register_node(tilde_vis_test + PLUGIN "tilde_vis_test::Sensor" + EXECUTABLE sensor) +rclcpp_components_register_node(tilde_vis_test + PLUGIN "tilde_vis_test::Relay" + EXECUTABLE relay) +rclcpp_components_register_node(tilde_vis_test + PLUGIN "tilde_vis_test::Filter" + EXECUTABLE filter) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install( + TARGETS tilde_vis_test EXPORT tilde_vis_test + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME} +) + +ament_package() diff --git a/src/tilde_vis_test/README.md b/src/tilde_vis_test/README.md new file mode 100644 index 00000000..6c64356e --- /dev/null +++ b/src/tilde_vis_test/README.md @@ -0,0 +1,31 @@ +# tilde_vis_test + +## Description + +Supplementary files to test `tilde_vis` package. + +## issues_23_1.launch.py + +See #23. It's OK if the program runs with no error. + +```bash +$ ros2 launch tilde_vis_test issues_23_1.launch.py +[filter-1] [INFO] [1654561227.800326321] [filter]: Filter get message cnt_ = 0 +[filter-1] [INFO] [1654561228.300162503] [filter]: Filter get message cnt_ = 1 +[filter-1] [INFO] [1654561228.800174003] [filter]: Filter get message cnt_ = 2 +[filter-1] [INFO] [1654561229.300167408] [filter]: Filter get message cnt_ = 3 +[filter-1] [INFO] [1654561229.800184144] [filter]: Filter get message cnt_ = 4 +``` + +## issues_23_2.launch.py + +See #23. It's OK if the program runs with no error. +Be aware that FPS is very slow to reproduce bug. + +```bash +$ ros2 launch tilde_vis_test issues_23_2.launch.py + +[relay-1] [INFO] [1654561285.390170828] [relay]: Relay get message cnt_ = 0 +[relay-1] [INFO] [1654561290.390119277] [relay]: Relay get message cnt_ = 1 +[relay-1] [INFO] [1654561295.390336942] [relay]: Relay get message cnt_ = 2 +``` diff --git a/src/tilde_vis_test/launch/issues_23_1.launch.py b/src/tilde_vis_test/launch/issues_23_1.launch.py new file mode 100644 index 00000000..84918018 --- /dev/null +++ b/src/tilde_vis_test/launch/issues_23_1.launch.py @@ -0,0 +1,36 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription + +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + name='filter', + package='tilde_vis_test', + executable='filter', + output='screen'), + Node( + package='tilde_vis_test', + executable='sensor', + output='screen', + parameters=[{ + 'timer_us': 500 * 1000, + }]), + ]) diff --git a/src/tilde_vis_test/launch/issues_23_2.launch.py b/src/tilde_vis_test/launch/issues_23_2.launch.py new file mode 100644 index 00000000..e78adbda --- /dev/null +++ b/src/tilde_vis_test/launch/issues_23_2.launch.py @@ -0,0 +1,35 @@ +# 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. + +"""Launch a publisher and a relay.""" + +from launch import LaunchDescription + +from launch_ros.actions import Node + + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='tilde_vis_test', + executable='relay', + output='screen'), + Node( + package='tilde_vis_test', + executable='sensor', + output='screen', + parameters=[{ + 'timer_us': 5 * 1000 * 1000, + }]), + ]) diff --git a/src/tilde_vis_test/package.xml b/src/tilde_vis_test/package.xml new file mode 100644 index 00000000..196482c0 --- /dev/null +++ b/src/tilde_vis_test/package.xml @@ -0,0 +1,24 @@ + + + + tilde_vis_test + 0.0.0 + TODO: Package description + y-okumura + TODO: License declaration + + ament_cmake + + rclcpp + rclcpp_components + sensor_msgs + std_msgs + tilde + + ament_lint_auto + caret_lint_common + + + ament_cmake + + diff --git a/src/tilde_vis_test/src/tilde_vis_test.cpp b/src/tilde_vis_test/src/tilde_vis_test.cpp new file mode 100644 index 00000000..b9e3e0f9 --- /dev/null +++ b/src/tilde_vis_test/src/tilde_vis_test.cpp @@ -0,0 +1,123 @@ +// Copyright 2021 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 "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "tilde/tilde_node.hpp" +#include "tilde/tilde_publisher.hpp" + +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "std_msgs/msg/string.hpp" + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; // NOLINT + +/** + * Sensor -> TildeNode -> + * PC2 String + */ +namespace tilde_vis_test +{ + +const char in_topic[] = "in"; +const char out_topic[] = "out"; + +class Sensor : public rclcpp::Node +{ +public: + explicit Sensor(const rclcpp::NodeOptions & options) : Node("sensor", options) + { + const std::string TIMER_DUR = "timer_us"; + const int64_t TIMER_DUR_DEFAULT_NS = 100 * 1000; + declare_parameter(TIMER_DUR, TIMER_DUR_DEFAULT_NS); + + auto timer_dur = get_parameter(TIMER_DUR).get_value(); + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + pub_pc_ = this->create_publisher(in_topic, qos); + + auto timer_callback = [this]() -> void { + auto msg = std::make_unique(); + msg->header.stamp = this->now(); + pub_pc_->publish(std::move(msg)); + }; + auto dur = std::chrono::duration(timer_dur); + timer_ = this->create_wall_timer(dur, timer_callback); + } + +private: + rclcpp::Publisher::SharedPtr pub_pc_; + rclcpp::TimerBase::SharedPtr timer_; +}; + +class Relay : public tilde::TildeNode +{ +public: + explicit Relay(const rclcpp::NodeOptions & options) : TildeNode("relay", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + pub_pc_ = this->create_tilde_publisher(out_topic, qos); + + sub_pc_ = this->create_tilde_subscription( + in_topic, qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + (void)msg; + RCLCPP_INFO(this->get_logger(), "Relay get message cnt_ = %d", cnt_); + pub_pc_->publish(std::move(msg)); + cnt_++; + }); + } + +private: + rclcpp::Subscription::SharedPtr sub_pc_; + tilde::TildePublisher::SharedPtr pub_pc_; + int cnt_{0}; +}; + +class Filter : public tilde::TildeNode +{ +public: + explicit Filter(const rclcpp::NodeOptions & options) : TildeNode("filter", options) + { + rclcpp::QoS qos(rclcpp::KeepLast(7)); + + pub_str_ = this->create_tilde_publisher(out_topic, qos); + + sub_pc_ = this->create_tilde_subscription( + in_topic, qos, [this](sensor_msgs::msg::PointCloud2::UniquePtr msg) -> void { + (void)msg; + RCLCPP_INFO(this->get_logger(), "Filter get message cnt_ = %d", cnt_); + auto out_msg = std::make_unique(); + out_msg->data = "hello"; + pub_str_->publish(std::move(out_msg)); + cnt_++; + }); + } + +private: + rclcpp::Subscription::SharedPtr sub_pc_; + tilde::TildePublisher::SharedPtr pub_str_; + int cnt_{0}; +}; + +} // namespace tilde_vis_test + +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Sensor) +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Relay) +RCLCPP_COMPONENTS_REGISTER_NODE(tilde_vis_test::Filter) From 85c7fc9b53b6d558a3cfa8341ed4c8edc8cc1d3e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 28 Mar 2023 06:42:03 +0000 Subject: [PATCH 12/16] ci(pre-commit): autofix --- src/tilde/src/tilde_node.cpp | 4 +- src/tilde_early_deadline_detector/README.md | 2 +- .../autoware_sensors.yaml | 6 +- .../forward_estimator.hpp | 2 +- .../tilde_early_deadline_detector_node.hpp | 13 +- src/tilde_early_deadline_detector/package.xml | 2 +- .../src/forward_estimator.cpp | 2 +- .../tilde_early_deadline_detector_node.cpp | 132 ++++++++++-------- 8 files changed, 85 insertions(+), 78 deletions(-) diff --git a/src/tilde/src/tilde_node.cpp b/src/tilde/src/tilde_node.cpp index a14de72d..d63f19a6 100644 --- a/src/tilde/src/tilde_node.cpp +++ b/src/tilde/src/tilde_node.cpp @@ -14,8 +14,8 @@ #include "tilde/tilde_node.hpp" -#include #include +#include using tilde::TildeNode; @@ -39,7 +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; + 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/README.md b/src/tilde_early_deadline_detector/README.md index cef7ed70..844b45de 100644 --- a/src/tilde_early_deadline_detector/README.md +++ b/src/tilde_early_deadline_detector/README.md @@ -23,7 +23,7 @@ The default path is shown below. 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/TILDE/tree/master/doc](https://github.com/tier4/caret)). +- 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/TILDE/tree/master/doc](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. diff --git a/src/tilde_early_deadline_detector/autoware_sensors.yaml b/src/tilde_early_deadline_detector/autoware_sensors.yaml index 1086bc92..7d8af5c9 100644 --- a/src/tilde_early_deadline_detector/autoware_sensors.yaml +++ b/src/tilde_early_deadline_detector/autoware_sensors.yaml @@ -1,9 +1,7 @@ tilde_early_deadline_detector_node: ros__parameters: # input of e2e - sensor_topics: [ - "/sensing/lidar/top/self_cropped/pointcloud_ex" - ] + 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", @@ -23,7 +21,7 @@ tilde_early_deadline_detector_node: # 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] + deadline_ms: [553, 553, 553, 553, 553, 553, 553, 553, 553] # parameters of debug messages print_report: true show_performance: true 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 index e9ef26a5..e61f884e 100644 --- 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 @@ -135,6 +135,6 @@ class ForwardEstimator void update_pending(std::shared_ptr message_tracking_tag); }; -} // namespace tilde_deadline_detector +} // 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 index fb0f85e9..16e4ca1f 100644 --- 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 @@ -31,7 +31,6 @@ // map header #include - namespace tilde_early_deadline_detector { struct PerformanceCounter @@ -87,21 +86,21 @@ class TildeEarlyDeadlineDetectorNode : public rclcpp::Node rclcpp::Publisher::SharedPtr notification_pub_; - PerformanceCounter message_tracking_tag_callback_counter_; - PerformanceCounter timer_callback_counter_; + 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; +// using MessageTrackingTagSubscription = +// rclcpp::Subscription; // public: // // constructors @@ -152,6 +151,6 @@ class TildeEarlyDeadlineDetectorNode : public rclcpp::Node // void message_tracking_tag_callback(tilde_msg::msg::MessageTrackingTag::UniquePtr msg); // }; -} // namespace tilde_deadline_detector +} // namespace tilde_early_deadline_detector -#endif // TILDE_EARLY_DEADLINE_DETECTOR__TILDE_DEADLINE_DETECTOR_NODE_HPP_ +#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 index f2c1e58a..aa9b70f7 100644 --- a/src/tilde_early_deadline_detector/package.xml +++ b/src/tilde_early_deadline_detector/package.xml @@ -16,8 +16,8 @@ rclcpp_components sensor_msgs tilde - tilde_msg 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 index 651211ed..71948c6a 100644 --- a/src/tilde_early_deadline_detector/src/forward_estimator.cpp +++ b/src/tilde_early_deadline_detector/src/forward_estimator.cpp @@ -293,4 +293,4 @@ std::map ForwardEstimator::get_pending_mess return ret; } -} // namespace tilde_deadline_detector +} // 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 index 2074c5f7..c71ae5b6 100644 --- 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 @@ -13,6 +13,7 @@ // 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" @@ -34,8 +35,8 @@ #include // container -#include -#include +#include +#include using std::chrono::milliseconds; using tilde_msg::msg::MessageTrackingTag; @@ -44,24 +45,26 @@ using tilde_msg::msg::MessageTrackingTag; // 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; +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 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{ +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){ +double estimate_latency(std::string topic_name) +{ // 99percentile std::map map{ // {"/sensing/lidar/top/self_cropped/pointcloud_ex", 552.49}, @@ -76,8 +79,7 @@ double estimate_latency(std::string topic_name){ {"/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} - }; + {"/control/trajectory_follower/control_cmd", 0.8}}; // // 95percentile // std::map map{ @@ -191,7 +193,7 @@ void TildeEarlyDeadlineDetectorNode::init() sensor_topics_.insert(tmp_sensor_topics.begin(), tmp_sensor_topics.end()); auto tmp_target_topics = - declare_parameter>("target_topics", std::vector{}); + 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{}); @@ -219,7 +221,8 @@ void TildeEarlyDeadlineDetectorNode::init() 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) + // 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; @@ -263,7 +266,7 @@ void TildeEarlyDeadlineDetectorNode::init() }; // print topic names subscribed by early_deadline_detector - for(auto itr = topics.begin(); itr != topics.end(); ++itr) { + for (auto itr = topics.begin(); itr != topics.end(); ++itr) { std::cout << *itr << "\n"; } @@ -287,7 +290,8 @@ void TildeEarlyDeadlineDetectorNode::init() auto sub = create_subscription( topic, qos, std::bind( - &TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback, this, std::placeholders::_1)); + &TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback, this, + std::placeholders::_1)); subs_.push_back(sub); } @@ -316,7 +320,8 @@ void TildeEarlyDeadlineDetectorNode::init() << " max: " << message_tracking_tag_callback_counter_.max << "\n" << "timer_callback: " << " avg: " << timer_callback_counter_.avg << "\n" - << " max: " << timer_callback_counter_.max << "\n" + << " max: " << timer_callback_counter_.max + << "\n" // debug // << " processed_num: " << processed_num << "\n" // << " deadline_miss_num: " << deadline_miss_num << "\n" @@ -344,17 +349,17 @@ void print_report( } 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::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; } @@ -371,7 +376,7 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // measure e2e latency builtin_interfaces::msg::Time pub_time_steady_e2e; - if (message_tracking_tag->output_info.topic_name=="/control/trajectory_follower/control_cmd"){ + 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; } @@ -381,7 +386,7 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( 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; } @@ -428,30 +433,33 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // // 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); + 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; + 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))) { + // 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 + 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 @@ -459,27 +467,27 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( 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 + 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) + if (flag_deadline_miss != 0 && flag_deadline_miss_true != 0) tp++; - else if(flag_deadline_miss==0 && flag_deadline_miss_true==0) + else if (flag_deadline_miss == 0 && flag_deadline_miss_true == 0) tn++; - else if(flag_deadline_miss!=0 && flag_deadline_miss_true==0) + else if (flag_deadline_miss != 0 && flag_deadline_miss_true == 0) fp++; - else if(flag_deadline_miss==0 && flag_deadline_miss_true!=0) + 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!=0 || fp!=0 || fn!=0){ + if (deadline_miss_true_num != 0 && (processed_num - deadline_miss_true_num) != 0) { + if (tp != 0 || fp != 0 || fn != 0) { accuracy = (tp + tn) / (tp + tn + fp + fn); precision = tp / (tp + fp); recall = tp / (tp + fn); - if(precision != 0 || recall != 0){ + if (precision != 0 || recall != 0) { f_measure = 2 * precision * recall / (precision + recall); } } @@ -490,21 +498,23 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // 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" + 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" + << " 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" + << " 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; } From 4824bc1d2d61024f8809f617a67d17fdf17cf16c Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Tue, 28 Mar 2023 16:38:40 +0900 Subject: [PATCH 13/16] Update tilde_early_deadline_detector_node.cpp --- .../src/tilde_early_deadline_detector_node.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) 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 index c71ae5b6..6ae22066 100644 --- 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 @@ -32,7 +32,6 @@ // map #include -#include // container #include @@ -483,14 +482,10 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // calculate accuracy, precision, recall, f_measure if (deadline_miss_true_num != 0 && (processed_num - deadline_miss_true_num) != 0) { - if (tp != 0 || fp != 0 || fn != 0) { - accuracy = (tp + tn) / (tp + tn + fp + fn); - precision = tp / (tp + fp); - recall = tp / (tp + fn); - if (precision != 0 || recall != 0) { - f_measure = 2 * precision * recall / (precision + recall); - } - } + 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);} } } From 7904422dd5b35096c4204022dc351c95430f1a56 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 28 Mar 2023 07:39:10 +0000 Subject: [PATCH 14/16] ci(pre-commit): autofix --- .../src/tilde_early_deadline_detector_node.cpp | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) 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 index 6ae22066..3b599437 100644 --- 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 @@ -482,10 +482,18 @@ void TildeEarlyDeadlineDetectorNode::message_tracking_tag_callback( // 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 (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); + } } } From e3ef3795be0c7528153edc3bb09e45c76d41cba3 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Tue, 28 Mar 2023 16:42:53 +0900 Subject: [PATCH 15/16] Update README.md --- src/tilde_vis/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From 86ea49f25147ff00caad1d1efb9cb9b24224cf84 Mon Sep 17 00:00:00 2001 From: hsato-saitama <119160378+hsato-saitama@users.noreply.github.com> Date: Tue, 28 Mar 2023 16:48:57 +0900 Subject: [PATCH 16/16] Update README.md --- src/tilde_early_deadline_detector/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/tilde_early_deadline_detector/README.md b/src/tilde_early_deadline_detector/README.md index 844b45de..998b13ac 100644 --- a/src/tilde_early_deadline_detector/README.md +++ b/src/tilde_early_deadline_detector/README.md @@ -23,7 +23,7 @@ The default path is shown below. 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/TILDE/tree/master/doc](https://github.com/tier4/caret)>). +- 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.