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.
carma_cooperative_perception::MultipleObjectTrackerNode Class Reference

#include <multiple_object_tracker_component.hpp>

Inheritance diagram for carma_cooperative_perception::MultipleObjectTrackerNode:
Inheritance graph
Collaboration diagram for carma_cooperative_perception::MultipleObjectTrackerNode:
Collaboration graph

Public Member Functions

 MultipleObjectTrackerNode (const rclcpp::NodeOptions &options)
 
auto handle_on_configure (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
 
auto handle_on_activate (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
 
auto handle_on_deactivate (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
 
auto handle_on_cleanup (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
 
auto handle_on_shutdown (const rclcpp_lifecycle::State &) -> carma_ros2_utils::CallbackReturn override
 
auto store_new_detections (const carma_cooperative_perception_interfaces::msg::DetectionList &msg) -> void
 
auto execute_pipeline () -> void
 

Private Attributes

rclcpp::Subscription< carma_cooperative_perception_interfaces::msg::DetectionList >::SharedPtr detection_list_sub_ {nullptr}
 
rclcpp_lifecycle::LifecyclePublisher< carma_cooperative_perception_interfaces::msg::TrackList >::SharedPtr track_list_pub_ {nullptr}
 
rclcpp::TimerBase::SharedPtr pipeline_execution_timer_ {nullptr}
 
std::vector< Detectiondetections_
 
std::unordered_map< multiple_object_tracking::Uuid, std::size_t > uuid_index_map_
 
multiple_object_tracking::FixedThresholdTrackManager< Tracktrack_manager_
 
units::time::nanosecond_t execution_period_ {1 / units::frequency::hertz_t{2.0}}
 
OnSetParametersCallbackHandle::SharedPtr on_set_parameters_callback_ {nullptr}
 
std::size_t lifetime_generated_track_count_ {0U}
 

Detailed Description

Definition at line 42 of file multiple_object_tracker_component.hpp.

Constructor & Destructor Documentation

◆ MultipleObjectTrackerNode()

carma_cooperative_perception::MultipleObjectTrackerNode::MultipleObjectTrackerNode ( const rclcpp::NodeOptions &  options)
explicit

Definition at line 245 of file multiple_object_tracker_component.cpp.

246: CarmaLifecycleNode{options}
247{
248}

Member Function Documentation

◆ execute_pipeline()

auto carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline ( ) -> void

Definition at line 563 of file multiple_object_tracker_component.cpp.

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{
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
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{
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}
rclcpp_lifecycle::LifecyclePublisher< carma_cooperative_perception_interfaces::msg::TrackList >::SharedPtr track_list_pub_
multiple_object_tracking::FixedThresholdTrackManager< Track > track_manager_
std::unordered_map< multiple_object_tracking::Uuid, std::size_t > uuid_index_map_
static auto temporally_align_detections(std::vector< Detection > &detections, units::time::second_t end_time) -> void
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)
std::variant< multiple_object_tracking::CtrvTrack, multiple_object_tracking::CtraTrack > Track
static auto to_ros_msg(const mot::CtraTrack &track)

References carma_cooperative_perception::predict_track_states(), carma_cooperative_perception::temporally_align_detections(), carma_cooperative_perception::to_ros_msg(), and carma_cooperative_perception::to_string().

Here is the call graph for this function:

◆ handle_on_activate()

auto carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_activate ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn
override

Definition at line 354 of file multiple_object_tracker_component.cpp.

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
362 }
363
364 const std::chrono::duration<double, std::nano> period_ns{mot::remove_units(execution_period_)};
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}
constexpr auto remove_units(const T &value)

References carma_cooperative_perception::remove_units().

Here is the call graph for this function:

◆ handle_on_cleanup()

auto carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_cleanup ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn
override

Definition at line 387 of file multiple_object_tracker_component.cpp.

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}
rclcpp::Subscription< carma_cooperative_perception_interfaces::msg::DetectionList >::SharedPtr detection_list_sub_

◆ handle_on_configure()

auto carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_configure ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn
override

Definition at line 250 of file multiple_object_tracker_component.cpp.

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
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}
auto store_new_detections(const carma_cooperative_perception_interfaces::msg::DetectionList &msg) -> void

◆ handle_on_deactivate()

auto carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_deactivate ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn
override

Definition at line 373 of file multiple_object_tracker_component.cpp.

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.
381
382 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");
383
384 return carma_ros2_utils::CallbackReturn::SUCCESS;
385}

◆ handle_on_shutdown()

auto carma_cooperative_perception::MultipleObjectTrackerNode::handle_on_shutdown ( const rclcpp_lifecycle::State &  ) -> carma_ros2_utils::CallbackReturn
override

Definition at line 403 of file multiple_object_tracker_component.cpp.

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}

◆ store_new_detections()

auto carma_cooperative_perception::MultipleObjectTrackerNode::store_new_detections ( const carma_cooperative_perception_interfaces::msg::DetectionList &  msg) -> void

Definition at line 416 of file multiple_object_tracker_component.cpp.

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}
auto make_detection(const carma_cooperative_perception_interfaces::msg::Detection &msg) -> Detection

References carma_cooperative_perception::make_detection().

Here is the call graph for this function:

Member Data Documentation

◆ detection_list_sub_

rclcpp::Subscription<carma_cooperative_perception_interfaces::msg::DetectionList>::SharedPtr carma_cooperative_perception::MultipleObjectTrackerNode::detection_list_sub_ {nullptr}
private

Definition at line 69 of file multiple_object_tracker_component.hpp.

◆ detections_

std::vector<Detection> carma_cooperative_perception::MultipleObjectTrackerNode::detections_
private

Definition at line 76 of file multiple_object_tracker_component.hpp.

◆ execution_period_

units::time::nanosecond_t carma_cooperative_perception::MultipleObjectTrackerNode::execution_period_ {1 / units::frequency::hertz_t{2.0}}
private

Definition at line 81 of file multiple_object_tracker_component.hpp.

◆ lifetime_generated_track_count_

std::size_t carma_cooperative_perception::MultipleObjectTrackerNode::lifetime_generated_track_count_ {0U}
private

Definition at line 83 of file multiple_object_tracker_component.hpp.

◆ on_set_parameters_callback_

OnSetParametersCallbackHandle::SharedPtr carma_cooperative_perception::MultipleObjectTrackerNode::on_set_parameters_callback_ {nullptr}
private

Definition at line 82 of file multiple_object_tracker_component.hpp.

◆ pipeline_execution_timer_

rclcpp::TimerBase::SharedPtr carma_cooperative_perception::MultipleObjectTrackerNode::pipeline_execution_timer_ {nullptr}
private

Definition at line 74 of file multiple_object_tracker_component.hpp.

◆ track_list_pub_

rclcpp_lifecycle::LifecyclePublisher<carma_cooperative_perception_interfaces::msg::TrackList>::SharedPtr carma_cooperative_perception::MultipleObjectTrackerNode::track_list_pub_ {nullptr}
private

Definition at line 72 of file multiple_object_tracker_component.hpp.

◆ track_manager_

multiple_object_tracking::FixedThresholdTrackManager<Track> carma_cooperative_perception::MultipleObjectTrackerNode::track_manager_
private
Initial value:
{
multiple_object_tracking::PromotionThreshold{3U},
multiple_object_tracking::RemovalThreshold{0U}}

Definition at line 78 of file multiple_object_tracker_component.hpp.

◆ uuid_index_map_

std::unordered_map<multiple_object_tracking::Uuid, std::size_t> carma_cooperative_perception::MultipleObjectTrackerNode::uuid_index_map_
private

Definition at line 77 of file multiple_object_tracker_component.hpp.


The documentation for this class was generated from the following files: