|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "autoware/pointcloud_preprocessor/concatenate_data/combine_cloud_handler.hpp" |
| 16 | +#include "autoware/pointcloud_preprocessor/concatenate_data/concatenate_and_time_sync_node.hpp" |
| 17 | +#include "autoware/pointcloud_preprocessor/concatenate_data/traits.hpp" |
| 18 | + |
| 19 | +#include <rclcpp/rclcpp.hpp> |
| 20 | + |
| 21 | +#include <memory> |
| 22 | +#include <string> |
| 23 | +#include <unordered_map> |
| 24 | +#include <utility> |
| 25 | + |
| 26 | +namespace autoware::pointcloud_preprocessor |
| 27 | +{ |
| 28 | + |
| 29 | +template <typename MsgTraits> |
| 30 | +CloudCollector<MsgTraits>::CloudCollector( |
| 31 | + std::shared_ptr<PointCloudConcatenateDataSynchronizerComponentTemplated<MsgTraits>> && |
| 32 | + ros2_parent_node, |
| 33 | + std::shared_ptr<CombineCloudHandler<MsgTraits>> & combine_cloud_handler, int num_of_clouds, |
| 34 | + double timeout_sec, bool debug_mode) |
| 35 | +: ros2_parent_node_(std::move(ros2_parent_node)), |
| 36 | + combine_cloud_handler_(combine_cloud_handler), |
| 37 | + num_of_clouds_(num_of_clouds), |
| 38 | + timeout_sec_(timeout_sec), |
| 39 | + status_(CollectorStatus::Idle), |
| 40 | + debug_mode_(debug_mode) |
| 41 | +{ |
| 42 | + const auto period_ns = std::chrono::duration_cast<std::chrono::nanoseconds>( |
| 43 | + std::chrono::duration<double>(timeout_sec_)); |
| 44 | + |
| 45 | + timer_ = |
| 46 | + rclcpp::create_timer(ros2_parent_node_, ros2_parent_node_->get_clock(), period_ns, [this]() { |
| 47 | + if (status_ == CollectorStatus::Finished) return; |
| 48 | + concatenate_callback(); |
| 49 | + }); |
| 50 | + |
| 51 | + timer_->cancel(); |
| 52 | +} |
| 53 | + |
| 54 | +template <typename MsgTraits> |
| 55 | +void CloudCollector<MsgTraits>::set_info(std::shared_ptr<CollectorInfoBase> collector_info) |
| 56 | +{ |
| 57 | + collector_info_ = std::move(collector_info); |
| 58 | +} |
| 59 | + |
| 60 | +template <typename MsgTraits> |
| 61 | +std::shared_ptr<CollectorInfoBase> CloudCollector<MsgTraits>::get_info() const |
| 62 | +{ |
| 63 | + return collector_info_; |
| 64 | +} |
| 65 | + |
| 66 | +template <typename MsgTraits> |
| 67 | +bool CloudCollector<MsgTraits>::topic_exists(const std::string & topic_name) |
| 68 | +{ |
| 69 | + return topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end(); |
| 70 | +} |
| 71 | + |
| 72 | +template <typename MsgTraits> |
| 73 | +bool CloudCollector<MsgTraits>::process_pointcloud( |
| 74 | + const std::string & topic_name, typename MsgTraits::PointCloudMessage::ConstSharedPtr cloud) |
| 75 | +{ |
| 76 | + if (status_ == CollectorStatus::Finished) { |
| 77 | + return false; |
| 78 | + } |
| 79 | + |
| 80 | + if (status_ == CollectorStatus::Idle) { |
| 81 | + // Add first pointcloud to the collector, restart the timer |
| 82 | + status_ = CollectorStatus::Processing; |
| 83 | + timer_->reset(); |
| 84 | + } else if (status_ == CollectorStatus::Processing) { |
| 85 | + // Check if the map already contains an entry for the same topic. This shouldn't happen if the |
| 86 | + // parameter 'lidar_timestamp_noise_window' is set correctly. |
| 87 | + if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) { |
| 88 | + RCLCPP_WARN_STREAM_THROTTLE( |
| 89 | + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), |
| 90 | + std::chrono::milliseconds(10000).count(), |
| 91 | + "Topic '" << topic_name |
| 92 | + << "' already exists in the collector. Check the timestamp of the pointcloud."); |
| 93 | + } |
| 94 | + } |
| 95 | + |
| 96 | + // Check if the map already contains an entry for the same topic. This shouldn't happen if the |
| 97 | + // parameter 'lidar_timestamp_noise_window' is set correctly. |
| 98 | + if (topic_to_cloud_map_.find(topic_name) != topic_to_cloud_map_.end()) { |
| 99 | + RCLCPP_WARN_STREAM_THROTTLE( |
| 100 | + ros2_parent_node_->get_logger(), *ros2_parent_node_->get_clock(), |
| 101 | + std::chrono::milliseconds(10000).count(), |
| 102 | + "Topic '" << topic_name |
| 103 | + << "' already exists in the collector. Check the timestamp of the pointcloud."); |
| 104 | + } |
| 105 | + topic_to_cloud_map_[topic_name] = cloud; |
| 106 | + if (topic_to_cloud_map_.size() == num_of_clouds_) { |
| 107 | + concatenate_callback(); |
| 108 | + } |
| 109 | + |
| 110 | + return true; |
| 111 | +} |
| 112 | + |
| 113 | +template <typename MsgTraits> |
| 114 | +CollectorStatus CloudCollector<MsgTraits>::get_status() const |
| 115 | +{ |
| 116 | + return status_; |
| 117 | +} |
| 118 | + |
| 119 | +template <typename MsgTraits> |
| 120 | +void CloudCollector<MsgTraits>::concatenate_callback() |
| 121 | +{ |
| 122 | + if (debug_mode_) { |
| 123 | + show_debug_message(); |
| 124 | + } |
| 125 | + |
| 126 | + // All pointclouds are received or the timer has timed out, cancel the timer and concatenate the |
| 127 | + // pointclouds in the collector. |
| 128 | + timer_->cancel(); |
| 129 | + |
| 130 | + auto concatenated_cloud_result = concatenate_pointclouds(topic_to_cloud_map_); |
| 131 | + |
| 132 | + ros2_parent_node_->publish_clouds(std::move(concatenated_cloud_result), collector_info_); |
| 133 | + |
| 134 | + combine_cloud_handler_->allocate_pointclouds(); |
| 135 | + |
| 136 | + status_ = CollectorStatus::Finished; |
| 137 | +} |
| 138 | + |
| 139 | +template <typename MsgTraits> |
| 140 | +ConcatenatedCloudResult<MsgTraits> CloudCollector<MsgTraits>::concatenate_pointclouds( |
| 141 | + std::unordered_map<std::string, typename MsgTraits::PointCloudMessage::ConstSharedPtr> |
| 142 | + topic_to_cloud_map) |
| 143 | +{ |
| 144 | + return combine_cloud_handler_->combine_pointclouds(topic_to_cloud_map); |
| 145 | +} |
| 146 | + |
| 147 | +template <typename MsgTraits> |
| 148 | +std::unordered_map<std::string, typename MsgTraits::PointCloudMessage::ConstSharedPtr> |
| 149 | +CloudCollector<MsgTraits>::get_topic_to_cloud_map() |
| 150 | +{ |
| 151 | + return topic_to_cloud_map_; |
| 152 | +} |
| 153 | + |
| 154 | +template <typename MsgTraits> |
| 155 | +void CloudCollector<MsgTraits>::show_debug_message() |
| 156 | +{ |
| 157 | + RCLCPP_WARN(ros2_parent_node_->get_logger(), "start show_debug_message timer with timeout=%f", timeout_sec_); |
| 158 | + auto time_until_trigger = timer_->time_until_trigger(); |
| 159 | + std::stringstream log_stream; |
| 160 | + log_stream << std::fixed << std::setprecision(6); |
| 161 | + log_stream << "Collector's concatenate callback time: " |
| 162 | + << ros2_parent_node_->get_clock()->now().seconds() << " seconds\n"; |
| 163 | + |
| 164 | + if ( auto advanced_info = std::dynamic_pointer_cast<AdvancedCollectorInfo>(collector_info_)) { |
| 165 | + log_stream << "Advanced strategy:\n Collector's reference time min: " |
| 166 | + << advanced_info->timestamp - advanced_info->noise_window |
| 167 | + << " to max: " << advanced_info->timestamp + advanced_info->noise_window |
| 168 | + << " seconds\n"; |
| 169 | + } else if (auto naive_info = std::dynamic_pointer_cast<NaiveCollectorInfo>(collector_info_)) { |
| 170 | + log_stream << "Naive strategy:\n Collector's timestamp: " << naive_info->timestamp |
| 171 | + << " seconds\n"; |
| 172 | + } |
| 173 | + |
| 174 | + log_stream << "Time until trigger: " << (time_until_trigger.count() / 1e9) << " seconds\n"; |
| 175 | + |
| 176 | + log_stream << "Pointclouds: ["; |
| 177 | + std::string separator = ""; |
| 178 | + for (const auto & [topic, cloud] : topic_to_cloud_map_) { |
| 179 | + log_stream << separator; |
| 180 | + log_stream << "[" << topic << ", " << rclcpp::Time(cloud->header.stamp).seconds() << "]"; |
| 181 | + separator = ", "; |
| 182 | + } |
| 183 | + |
| 184 | + log_stream << "]\n"; |
| 185 | + |
| 186 | + RCLCPP_WARN(ros2_parent_node_->get_logger(), "end show_debug_message timer with timeout=%f", timeout_sec_); |
| 187 | + RCLCPP_INFO(ros2_parent_node_->get_logger(), "%s", log_stream.str().c_str()); |
| 188 | +} |
| 189 | + |
| 190 | +template <typename MsgTraits> |
| 191 | +void CloudCollector<MsgTraits>::reset() |
| 192 | +{ |
| 193 | + status_ = CollectorStatus::Idle; // Reset status to Idle |
| 194 | + topic_to_cloud_map_.clear(); |
| 195 | + collector_info_ = nullptr; |
| 196 | + |
| 197 | + if (timer_ && !timer_->is_canceled()) { |
| 198 | + timer_->cancel(); |
| 199 | + } |
| 200 | +} |
| 201 | + |
| 202 | +} // namespace autoware::pointcloud_preprocessor |
0 commit comments