Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
multiple_object_tracker_component.cpp
Go to the documentation of this file.
1// Copyright 2023 Leidos
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
16
17#include <units.h>
18#include <rclcpp/rclcpp.hpp>
19#include <rclcpp_components/register_node_macro.hpp>
20#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
21
22#include <algorithm>
23#include <chrono>
24#include <limits>
25#include <multiple_object_tracking/clustering.hpp>
26#include <multiple_object_tracking/ctra_model.hpp>
27#include <multiple_object_tracking/ctrv_model.hpp>
28#include <multiple_object_tracking/fusing.hpp>
29#include <multiple_object_tracking/gating.hpp>
30#include <multiple_object_tracking/scoring.hpp>
31#include <multiple_object_tracking/temporal_alignment.hpp>
32#include <unordered_map>
33#include <utility>
34#include <vector>
35
37{
38
39namespace mot = multiple_object_tracking;
40
41auto make_semantic_class(std::size_t numeric_value)
42{
43 switch (numeric_value) {
44 case 0:
45 return mot::SemanticClass::kUnknown;
46 case 1:
47 return mot::SemanticClass::kSmallVehicle;
48 case 2:
49 return mot::SemanticClass::kLargeVehicle;
50 case 3:
51 return mot::SemanticClass::kPedestrian;
52 case 4:
53 return mot::SemanticClass::kMotorcycle;
54 }
55
56 return mot::SemanticClass::kUnknown;
57}
58
59auto semantic_class_to_numeric_value(mot::SemanticClass semantic_class)
60{
61 switch (semantic_class) {
62 case mot::SemanticClass::kUnknown:
63 return 0;
64 case mot::SemanticClass::kSmallVehicle:
65 return 1;
66 case mot::SemanticClass::kLargeVehicle:
67 return 2;
68 case mot::SemanticClass::kPedestrian:
69 return 3;
70 case mot::SemanticClass::kMotorcycle:
71 return 4;
72 }
73
74 return 0;
75}
76
78 -> Detection
79{
80 const auto timestamp{
81 units::time::second_t{static_cast<double>(msg.header.stamp.sec)} +
82 units::time::nanosecond_t{static_cast<double>(msg.header.stamp.nanosec)}};
83
84 tf2::Quaternion orientation;
85 orientation.setX(msg.pose.pose.orientation.x);
86 orientation.setY(msg.pose.pose.orientation.y);
87 orientation.setZ(msg.pose.pose.orientation.z);
88 orientation.setW(msg.pose.pose.orientation.w);
89
90 double roll{0.0};
91 double pitch{0.0};
92 double yaw{0.0};
93
94 tf2::Matrix3x3 matrix{orientation};
95 matrix.getRPY(roll, pitch, yaw);
96
97 const mot::CtrvState state{
98 units::length::meter_t{msg.pose.pose.position.x},
99 units::length::meter_t{msg.pose.pose.position.y},
100 units::velocity::meters_per_second_t{msg.twist.twist.linear.x},
101 mot::Angle{units::angle::radian_t{yaw}},
102 units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z}};
103
104 mot::CtrvStateCovariance covariance = mot::CtrvStateCovariance::Zero();
105 covariance(0, 0) = msg.pose.covariance.at(0);
106 covariance(1, 1) = msg.pose.covariance.at(7);
107 covariance(2, 2) = msg.twist.covariance.at(0);
108 covariance(3, 3) = msg.pose.covariance.at(35);
109 covariance(4, 4) = msg.twist.covariance.at(35);
110
111 return mot::CtrvDetection{
112 timestamp, state, covariance, mot::Uuid{msg.id}, make_semantic_class(msg.semantic_class)};
113}
114
116 -> Detection
117{
118 const auto timestamp{
119 units::time::second_t{static_cast<double>(msg.header.stamp.sec)} +
120 units::time::nanosecond_t{static_cast<double>(msg.header.stamp.nanosec)}};
121
122 tf2::Quaternion orientation;
123 orientation.setX(msg.pose.pose.orientation.x);
124 orientation.setY(msg.pose.pose.orientation.y);
125 orientation.setZ(msg.pose.pose.orientation.z);
126 orientation.setW(msg.pose.pose.orientation.w);
127
128 double roll{0.0};
129 double pitch{0.0};
130 double yaw{0.0};
131
132 tf2::Matrix3x3 matrix{orientation};
133 matrix.getRPY(roll, pitch, yaw);
134
135 const mot::CtraState state{
136 units::length::meter_t{msg.pose.pose.position.x},
137 units::length::meter_t{msg.pose.pose.position.y},
138 units::velocity::meters_per_second_t{msg.twist.twist.linear.x},
139 mot::Angle{units::angle::radian_t{yaw}},
140 units::angular_velocity::radians_per_second_t{msg.twist.twist.angular.z},
141 units::acceleration::meters_per_second_squared_t{msg.accel.accel.linear.x}};
142
143 mot::CtraStateCovariance covariance = mot::CtraStateCovariance::Zero();
144 covariance(0, 0) = msg.pose.covariance.at(0);
145 covariance(1, 1) = msg.pose.covariance.at(7);
146 covariance(2, 2) = msg.twist.covariance.at(0);
147 covariance(3, 3) = msg.pose.covariance.at(35);
148 covariance(4, 4) = msg.twist.covariance.at(35);
149 covariance(5, 5) = msg.accel.covariance.at(0);
150
151 return mot::CtraDetection{
152 timestamp, state, covariance, mot::Uuid{msg.id}, make_semantic_class(msg.semantic_class)};
153}
154
156 -> Detection
157{
158 switch (msg.motion_model) {
159 case msg.MOTION_MODEL_CTRV:
160 return make_ctrv_detection(msg);
161
162 case msg.MOTION_MODEL_CTRA:
163 return make_ctra_detection(msg);
164
165 case msg.MOTION_MODEL_CV:
166 throw std::runtime_error("unsupported motion model type '3: constant velocity (CV)'");
167 }
168
169 throw std::runtime_error("unkown motion model type '" + std::to_string(msg.motion_model) + "'");
170}
171
172static auto to_ros_msg(const mot::CtraTrack & track)
173{
175
176 msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp));
177 msg.header.stamp.nanosec = mot::remove_units(
178 units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
179 msg.header.frame_id = "map";
180
181 msg.id = track.uuid.value();
182 msg.motion_model = msg.MOTION_MODEL_CTRA;
183 msg.pose.pose.position.x = mot::remove_units(track.state.position_x);
184 msg.pose.pose.position.y = mot::remove_units(track.state.position_y);
185
186 tf2::Quaternion orientation;
187 orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle()));
188 msg.pose.pose.orientation.x = orientation.getX();
189 msg.pose.pose.orientation.y = orientation.getY();
190 msg.pose.pose.orientation.z = orientation.getZ();
191 msg.pose.pose.orientation.w = orientation.getW();
192
193 msg.twist.twist.linear.x = mot::remove_units(track.state.velocity);
194 msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate);
195
196 msg.accel.accel.linear.x = mot::remove_units(track.state.acceleration);
197
198 msg.semantic_class = semantic_class_to_numeric_value(mot::get_semantic_class(track));
199
200 return msg;
201}
202
203static auto to_ros_msg(const mot::CtrvTrack & track)
204{
206
207 msg.header.stamp.sec = mot::remove_units(units::math::floor(track.timestamp));
208 msg.header.stamp.nanosec = mot::remove_units(
209 units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
210 msg.header.frame_id = "map";
211
212 msg.id = track.uuid.value();
213 msg.motion_model = msg.MOTION_MODEL_CTRV;
214 msg.pose.pose.position.x = mot::remove_units(track.state.position_x);
215 msg.pose.pose.position.y = mot::remove_units(track.state.position_y);
216
217 tf2::Quaternion orientation;
218 orientation.setRPY(0, 0, mot::remove_units(track.state.yaw.get_angle()));
219 msg.pose.pose.orientation.x = orientation.getX();
220 msg.pose.pose.orientation.y = orientation.getY();
221 msg.pose.pose.orientation.z = orientation.getZ();
222 msg.pose.pose.orientation.w = orientation.getW();
223
224 msg.twist.twist.linear.x = mot::remove_units(track.state.velocity);
225 msg.twist.twist.angular.z = mot::remove_units(track.state.yaw_rate);
226
227 msg.semantic_class = semantic_class_to_numeric_value(mot::get_semantic_class(track));
228
229 return msg;
230}
231
232static auto to_ros_msg(const Track & track)
233{
234 static constexpr mot::Visitor visitor{
235 [](const mot::CtrvTrack & t) { return to_ros_msg(t); },
236 [](const mot::CtraTrack & t) { return to_ros_msg(t); },
237 [](const auto &) {
238 // Currently on support CTRV and CTRA
239 throw std::runtime_error{"cannot make ROS 2 message from track type"};
240 }};
241
242 return std::visit(visitor, track);
243}
244
246: CarmaLifecycleNode{options}
247{
248}
249
251 const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
252{
253 RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring");
254
255 track_list_pub_ = create_publisher<carma_cooperative_perception_interfaces::msg::TrackList>(
256 "output/track_list", 1);
257
258 detection_list_sub_ = create_subscription<
259 carma_cooperative_perception_interfaces::msg::DetectionList>(
260 "input/detection_list", 1,
261 [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) {
262 if (const auto current_state{this->get_current_state().label()}; current_state == "active") {
263 this->store_new_detections(*msg_ptr);
264 } else {
265 RCLCPP_WARN(
266 this->get_logger(),
267 "Trying to receive message on the topic '%s', but the containing node is not activated. "
268 "Current node state: '%s'",
269 this->detection_list_sub_->get_topic_name(), current_state.c_str());
270 }
271 });
272
273 on_set_parameters_callback_ =
274 add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) {
275 rcl_interfaces::msg::SetParametersResult result;
276 result.successful = true;
277 result.reason = "success";
278
279 for (const auto & parameter : parameters) {
280 if (parameter.get_name() == "execution_frequency_hz") {
281 if (this->get_current_state().label() == "active") {
282 result.successful = false;
283 result.reason = "parameter is read-only while node is in 'Active' state";
284
285 RCLCPP_ERROR(
286 get_logger(),
287 ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
288
289 break;
290 } else {
291 this->execution_period_ = 1 / units::frequency::hertz_t{parameter.as_double()};
292 }
293 } else if (parameter.get_name() == "track_promotion_threshold") {
294 if (this->get_current_state().label() == "active") {
295 result.successful = false;
296 result.reason = "parameter is read-only while node is in 'Active' state";
297
298 RCLCPP_ERROR(
299 get_logger(),
300 ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
301
302 break;
303 } else {
304 if (const auto value{parameter.as_int()}; value < 0) {
305 result.successful = false;
306 result.reason = "parameter must be nonnegative";
307 } else {
308 this->track_manager_.set_promotion_threshold_and_update(
309 mot::PromotionThreshold{static_cast<std::size_t>(value)});
310 }
311 }
312 } else if (parameter.get_name() == "track_removal_threshold") {
313 if (this->get_current_state().label() == "active") {
314 result.successful = false;
315 result.reason = "parameter is read-only while node is in 'Active' state";
316
317 RCLCPP_ERROR(
318 get_logger(),
319 ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
320
321 break;
322 } else {
323 if (const auto value{parameter.as_int()}; value < 0) {
324 result.successful = false;
325 result.reason = "parameter must be nonnegative";
326 } else {
327 this->track_manager_.set_removal_threshold_and_update(
328 mot::RemovalThreshold{static_cast<std::size_t>(value)});
329 }
330 }
331 } else {
332 result.successful = false;
333 result.reason = "Unexpected parameter name '" + parameter.get_name() + '\'';
334 }
335 }
336
337 return result;
338 });
339
340 declare_parameter(
341 "execution_frequency_hz", mot::remove_units(units::frequency::hertz_t{1 / execution_period_}));
342
343 declare_parameter(
344 "track_promotion_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
345
346 declare_parameter(
347 "track_removal_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
348
349 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");
350
351 return carma_ros2_utils::CallbackReturn::SUCCESS;
352}
353
354auto MultipleObjectTrackerNode::handle_on_activate(
355 const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
356{
357 RCLCPP_INFO(get_logger(), "Lifecycle transition: activating");
358
359 if (pipeline_execution_timer_ != nullptr) {
360 // The user might have changed the timer period since last time the Node was active
361 pipeline_execution_timer_->reset();
362 }
363
364 const std::chrono::duration<double, std::nano> period_ns{mot::remove_units(execution_period_)};
365 pipeline_execution_timer_ =
366 rclcpp::create_timer(this, this->get_clock(), period_ns, [this] { execute_pipeline(); });
367
368 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated");
369
370 return carma_ros2_utils::CallbackReturn::SUCCESS;
371}
372
373auto MultipleObjectTrackerNode::handle_on_deactivate(
374 const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
375{
376 RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating");
377
378 // There is currently no way to change a timer's period in ROS 2, so we will
379 // have to create a new one in case a user changes the period.
380 pipeline_execution_timer_.reset();
381
382 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");
383
384 return carma_ros2_utils::CallbackReturn::SUCCESS;
385}
386
387auto MultipleObjectTrackerNode::handle_on_cleanup(
388 const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
389{
390 RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up");
391
392 // CarmaLifecycleNode does not handle subscriber pointer reseting for us
393 detection_list_sub_.reset();
394
395 undeclare_parameter("execution_frequency_hz");
396 remove_on_set_parameters_callback(on_set_parameters_callback_.get());
397
398 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up");
399
400 return carma_ros2_utils::CallbackReturn::SUCCESS;
401}
402
403auto MultipleObjectTrackerNode::handle_on_shutdown(
404 const rclcpp_lifecycle::State & /* previous_state */) -> carma_ros2_utils::CallbackReturn
405{
406 RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down");
407
408 // CarmaLifecycleNode does not handle subscriber pointer reseting for us
409 detection_list_sub_.reset();
410
411 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down");
412
413 return carma_ros2_utils::CallbackReturn::SUCCESS;
414}
415
416auto MultipleObjectTrackerNode::store_new_detections(
417 const carma_cooperative_perception_interfaces::msg::DetectionList & msg) -> void
418{
419 if (std::size(msg.detections) == 0) {
420 RCLCPP_WARN(this->get_logger(), "Not storing detections: incoming detection list is empty");
421 return;
422 }
423
424 for (const auto & detection_msg : msg.detections) {
425 try {
426 const auto detection{make_detection(detection_msg)};
427 const auto uuid{mot::get_uuid(detection)};
428
429 if (uuid_index_map_.count(uuid) == 0) {
430 detections_.push_back(std::move(detection));
431 uuid_index_map_[uuid] = std::size(detections_) - 1;
432 } else {
433 RCLCPP_WARN_STREAM(
434 this->get_logger(),
435 "Detection with ID '" << uuid << "' already exists. Overwriting its data");
436 detections_.at(uuid_index_map_[uuid]) = detection;
437 }
438 } catch (const std::runtime_error & error) {
439 RCLCPP_ERROR(
440 this->get_logger(), "Ignoring detection with ID '%s': %s", detection_msg.id.c_str(),
441 error.what());
442 }
443 }
444}
445
447 std::vector<Detection> & detections, units::time::second_t end_time) -> void
448{
449 for (auto & detection : detections) {
450 mot::propagate_to_time(detection, end_time, mot::default_unscented_transform);
451 }
452}
453
454static auto predict_track_states(std::vector<Track> tracks, units::time::second_t end_time)
455{
456 for (auto & track : tracks) {
457 mot::propagate_to_time(track, end_time, mot::default_unscented_transform);
458 }
459
460 return tracks;
461}
462
471{
472 template <typename Track, typename Detection>
473 auto operator()(const Track & track, const Detection & detection) const -> std::optional<float>
474 {
475 if constexpr (std::is_same_v<decltype(track.state), decltype(detection.state)>) {
476 const auto dist{two_dimensional_distance(track.state, detection.state)};
477
478 // Fall back to 2D Euclidean distance if either semantic class if unknown. The unknown
479 // track/detection may actually be the same other track/detection we are scoring against.
480 if (
481 track.semantic_class == mot::SemanticClass::kUnknown ||
482 detection.semantic_class == mot::SemanticClass::kUnknown) {
483 return dist;
484 }
485
486 if (track.semantic_class == detection.semantic_class) {
487 return dist;
488 }
489 }
490
491 return std::nullopt;
492 }
493
494 template <typename... TrackAlternatives, typename... DetectionAlternatives>
496 const std::variant<TrackAlternatives...> & track,
497 const std::variant<DetectionAlternatives...> & detection) const -> std::optional<float>
498 {
499 return std::visit(
500 [this](const auto & t, const auto & d) { return (*this)(t, d); }, track, detection);
501 }
502
503private:
504 static auto two_dimensional_distance(const mot::CtrvState & lhs, const mot::CtrvState & rhs)
505 -> float
506 {
507 const auto x_diff_sq{
508 std::pow(mot::remove_units(lhs.position_x) - mot::remove_units(rhs.position_x), 2)};
509 const auto y_diff_sq{
510 std::pow(mot::remove_units(lhs.position_y) - mot::remove_units(rhs.position_y), 2)};
511
512 return std::sqrt(x_diff_sq + y_diff_sq);
513 }
514
515 static auto two_dimensional_distance(const mot::CtraState & lhs, const mot::CtraState & rhs)
516 -> float
517 {
518 const auto x_diff_sq{
519 std::pow(mot::remove_units(lhs.position_x) - mot::remove_units(rhs.position_x), 2)};
520 const auto y_diff_sq{
521 std::pow(mot::remove_units(lhs.position_y) - mot::remove_units(rhs.position_y), 2)};
522
523 return std::sqrt(x_diff_sq + y_diff_sq);
524 }
525};
526
531{
532 template <typename Detection>
533 auto operator()(const mot::Point & point, const Detection & detection) const -> double
534 {
535 double sum{0};
536
537 sum += std::pow(mot::remove_units(point.position_x - detection.state.position_x), 2);
538 sum += std::pow(mot::remove_units(point.position_y - detection.state.position_y), 2);
539
540 const auto p_yaw_rad{mot::remove_units(point.yaw.get_angle())};
541 const auto d_yaw_rad{mot::remove_units(detection.state.yaw.get_angle())};
542 const auto abs_diff{std::abs(p_yaw_rad - d_yaw_rad)};
543 sum += std::min(abs_diff, 2 * 3.14159265359 - abs_diff);
544
545 return sum;
546 }
547
548 template <typename... Alternatives>
549 auto operator()(mot::Point point, const std::variant<Alternatives...> & detection) const -> double
550 {
551 const mot::Visitor visitor{
552 [this](const mot::Point & p, const mot::CtrvDetection & d) { return this->operator()(p, d); },
553 [this](const mot::Point & p, const mot::CtraDetection & d) { return this->operator()(p, d); },
554 [](const mot::Point &, const auto &) {
555 // Currently on support CTRV and CTRA
556 return std::numeric_limits<double>::max();
557 }};
558
559 return std::visit(visitor, std::variant<mot::Point>{point}, detection);
560 }
561};
562
563auto MultipleObjectTrackerNode::execute_pipeline() -> void
564{
565 static constexpr mot::Visitor make_track_visitor{
566 [](const mot::CtrvDetection & d, const mot::Uuid & u) {
567 return Track{mot::make_track<mot::CtrvTrack>(d, u)};
568 },
569 [](const mot::CtraDetection & d, const mot::Uuid & u) {
570 return Track{mot::make_track<mot::CtraTrack>(d, u)};
571 },
572 [](const auto &) {
573 // Currently on support CTRV and CTRA
574 throw std::runtime_error("cannot make track from given detection");
575 },
576 };
577
578 if (track_manager_.get_all_tracks().empty()) {
579 RCLCPP_DEBUG(
580 get_logger(), "List of tracks is empty. Converting detections to tentative tracks");
581
582 // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
583 // current purposes, but there's no reason it couldn't be restricted or loosened.
584 const auto clusters{mot::cluster_detections(detections_, 0.75)};
585 for (const auto & cluster : clusters) {
586 const auto detection{std::cbegin(cluster.get_detections())->second};
587 const auto uuid_str{mot::get_uuid(detection).value()};
588 // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
589 // <track_number>-<carla_actor_id> for easier visual analysis by users.
590 const mot::Uuid new_uuid{
591 std::to_string(lifetime_generated_track_count_++) +
592 uuid_str.substr(std::size(uuid_str) - 4, 4)};
593 track_manager_.add_tentative_track(
594 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
595 }
596
597 track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{});
598
599 detections_.clear();
600 uuid_index_map_.clear();
601 return;
602 }
603
604 const units::time::second_t current_time{this->now().seconds()};
605
606 temporally_align_detections(detections_, current_time);
607
608 const auto predicted_tracks{predict_track_states(track_manager_.get_all_tracks(), current_time)};
609 auto scores{
610 mot::score_tracks_and_detections(predicted_tracks, detections_, SemanticDistance2dScore{})};
611
612 // This pruning distance is an arbitrarily-chosen heuristic. It is working well for our
613 // current purposes, but there's no reason it couldn't be restricted or loosened.
614 mot::prune_track_and_detection_scores_if(scores, [](const auto & score) { return score > 1.0; });
615
616 const auto associations{
617 mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)};
618
619 track_manager_.update_track_lists(associations);
620
621 std::unordered_map<mot::Uuid, Detection> detection_map;
622 for (const auto & detection : detections_) {
623 detection_map[mot::get_uuid(detection)] = detection;
624 }
625
626 const mot::HasAssociation has_association{associations};
627 for (auto & track : track_manager_.get_all_tracks()) {
628 if (has_association(track)) {
629 const auto detection_uuids{associations.at(get_uuid(track))};
630 const auto first_detection{detection_map[detection_uuids.at(0)]};
631 const auto fused_track{
632 std::visit(mot::covariance_intersection_visitor, track, first_detection)};
633 track_manager_.update_track(mot::get_uuid(track), fused_track);
634 }
635 }
636
637 // Unassociated detections don't influence the tracking pipeline, so we can add
638 // them to the tracker at the end.
639 std::vector<Detection> unassociated_detections;
640 for (const auto & [uuid, detection] : detection_map) {
641 if (!has_association(detection)) {
642 unassociated_detections.push_back(detection);
643 }
644 }
645
646 // We want to remove unassociated tracks that are close enough to existing tracks
647 // to avoid creating duplicates. Duplicate tracks will cause association inconsistencies
648 // (flip flopping associations between the two tracks).
649 auto remove_start{std::remove_if(
650 std::begin(unassociated_detections), std::end(unassociated_detections),
651 [&scores](const auto & detection) {
652 const auto uuid{mot::get_uuid(detection)};
653 auto min_score{std::numeric_limits<float>::infinity()};
654 for (const auto & [uuid_pair, score] : scores) {
655 if (uuid_pair.second == uuid) {
656 min_score = std::min(min_score, score);
657 }
658 }
659
660 // This distance is an arbitrarily-chosen heuristic. It is working well for our
661 // current purposes, but there's no reason it couldn't be restricted or loosened.
662 return min_score < 1.0;
663 })};
664
665 unassociated_detections.erase(remove_start, std::end(unassociated_detections));
666
667 // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
668 // current purposes, but there's no reason it couldn't be restricted or loosened.
669 const auto clusters{mot::cluster_detections(unassociated_detections, 0.75, MetricSe2{})};
670 for (const auto & cluster : clusters) {
671 const auto detection{std::cbegin(cluster.get_detections())->second};
672 const auto uuid_str{mot::get_uuid(detection).value()};
673 // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
674 // <track_number>-<carla_actor_id> for easier visual analysis by users.
675 const mot::Uuid new_uuid{
676 std::to_string(lifetime_generated_track_count_++) +
677 uuid_str.substr(std::size(uuid_str) - 4, 4)};
678 track_manager_.add_tentative_track(
679 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
680 }
681
682 carma_cooperative_perception_interfaces::msg::TrackList track_list;
683 for (const auto & track : track_manager_.get_confirmed_tracks()) {
684 track_list.tracks.push_back(to_ros_msg(track));
685 }
686
687 track_list_pub_->publish(track_list);
688
689 detections_.clear();
690 uuid_index_map_.clear();
691}
692
693} // namespace carma_cooperative_perception
694
695// This is not our macro, so we should not worry about linting it.
696
697RCLCPP_COMPONENTS_REGISTER_NODE(carma_cooperative_perception::MultipleObjectTrackerNode) // NOLINT
auto handle_on_configure(const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
static auto temporally_align_detections(std::vector< Detection > &detections, units::time::second_t end_time) -> void
auto make_ctra_detection(const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection
auto semantic_class_to_numeric_value(mot::SemanticClass semantic_class)
std::variant< multiple_object_tracking::CtrvDetection, multiple_object_tracking::CtraDetection > Detection
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
static auto predict_track_states(std::vector< Track > tracks, units::time::second_t end_time)
static auto to_ros_msg(const Track &track)
auto make_semantic_class(std::size_t numeric_value)
auto make_ctrv_detection(const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection
auto make_detection(const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection
constexpr auto remove_units(const T &value)
std::variant< multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack > Track
static auto to_ros_msg(const mot::CtraTrack &track)
Calculates distance between a point and detection in SE(2) (special Euclidean) space.
auto operator()(const mot::Point &point, const Detection &detection) const -> double
auto operator()(mot::Point point, const std::variant< Alternatives... > &detection) const -> double
Calculate 2D Euclidean distance between track and detection.
static auto two_dimensional_distance(const mot::CtrvState &lhs, const mot::CtrvState &rhs) -> float
auto operator()(const std::variant< TrackAlternatives... > &track, const std::variant< DetectionAlternatives... > &detection) const -> std::optional< float >
auto operator()(const Track &track, const Detection &detection) const -> std::optional< float >
static auto two_dimensional_distance(const mot::CtraState &lhs, const mot::CtraState &rhs) -> float