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 560 of file multiple_object_tracker_component.cpp.

561{
562 static constexpr mot::Visitor make_track_visitor{
563 [](const mot::CtrvDetection & d, const mot::Uuid & u) {
564 return Track{mot::make_track<mot::CtrvTrack>(d, u)};
565 },
566 [](const mot::CtraDetection & d, const mot::Uuid & u) {
567 return Track{mot::make_track<mot::CtraTrack>(d, u)};
568 },
569 [](const auto &) {
570 // Currently on support CTRV and CTRA
571 throw std::runtime_error("cannot make track from given detection");
572 },
573 };
574
575 if (track_manager_.get_all_tracks().empty()) {
576 RCLCPP_DEBUG(
577 get_logger(), "List of tracks is empty. Converting detections to tentative tracks");
578
579 // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
580 // current purposes, but there's no reason it couldn't be restricted or loosened.
581 const auto clusters{mot::cluster_detections(detections_, 0.75)};
582 for (const auto & cluster : clusters) {
583 const auto detection{std::cbegin(cluster.get_detections())->second};
584 const auto uuid_str{mot::get_uuid(detection).value()};
585 // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
586 // <track_number>-<carla_actor_id> for easier visual analysis by users.
587 const mot::Uuid new_uuid{
589 uuid_str.substr(std::size(uuid_str) - 4, 4)};
590 track_manager_.add_tentative_track(
591 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
592 }
593
594 track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{});
595
596 detections_.clear();
597 uuid_index_map_.clear();
598 return;
599 }
600
601 const units::time::second_t current_time{this->now().seconds()};
602
604
605 const auto predicted_tracks{predict_track_states(track_manager_.get_all_tracks(), current_time)};
606 auto scores{
607 mot::score_tracks_and_detections(predicted_tracks, detections_, SemanticDistance2dScore{})};
608
609 // This pruning distance is an arbitrarily-chosen heuristic. It is working well for our
610 // current purposes, but there's no reason it couldn't be restricted or loosened.
611 mot::prune_track_and_detection_scores_if(scores, [](const auto & score) { return score > 1.0; });
612
613 const auto associations{
614 mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)};
615
616 track_manager_.update_track_lists(associations);
617
618 std::unordered_map<mot::Uuid, Detection> detection_map;
619 for (const auto & detection : detections_) {
620 detection_map[mot::get_uuid(detection)] = detection;
621 }
622
623 const mot::HasAssociation has_association{associations};
624 for (auto & track : track_manager_.get_all_tracks()) {
625 if (has_association(track)) {
626 const auto detection_uuids{associations.at(get_uuid(track))};
627 const auto first_detection{detection_map[detection_uuids.at(0)]};
628 const auto fused_track{
629 std::visit(mot::covariance_intersection_visitor, track, first_detection)};
630 track_manager_.update_track(mot::get_uuid(track), fused_track);
631 }
632 }
633
634 // Unassociated detections don't influence the tracking pipeline, so we can add
635 // them to the tracker at the end.
636 std::vector<Detection> unassociated_detections;
637 for (const auto & [uuid, detection] : detection_map) {
638 if (!has_association(detection)) {
639 unassociated_detections.push_back(detection);
640 }
641 }
642
643 // We want to remove unassociated tracks that are close enough to existing tracks
644 // to avoid creating duplicates. Duplicate tracks will cause association inconsistencies
645 // (flip flopping associations between the two tracks).
646 auto remove_start{std::remove_if(
647 std::begin(unassociated_detections), std::end(unassociated_detections),
648 [&scores](const auto & detection) {
649 const auto uuid{mot::get_uuid(detection)};
650 auto min_score{std::numeric_limits<float>::infinity()};
651 for (const auto & [uuid_pair, score] : scores) {
652 if (uuid_pair.second == uuid) {
653 min_score = std::min(min_score, score);
654 }
655 }
656
657 // This distance is an arbitrarily-chosen heuristic. It is working well for our
658 // current purposes, but there's no reason it couldn't be restricted or loosened.
659 return min_score < 1.0;
660 })};
661
662 unassociated_detections.erase(remove_start, std::end(unassociated_detections));
663
664 // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
665 // current purposes, but there's no reason it couldn't be restricted or loosened.
666 const auto clusters{mot::cluster_detections(unassociated_detections, 0.75, MetricSe2{})};
667 for (const auto & cluster : clusters) {
668 const auto detection{std::cbegin(cluster.get_detections())->second};
669 const auto uuid_str{mot::get_uuid(detection).value()};
670 // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
671 // <track_number>-<carla_actor_id> for easier visual analysis by users.
672 const mot::Uuid new_uuid{
674 uuid_str.substr(std::size(uuid_str) - 4, 4)};
675 track_manager_.add_tentative_track(
676 std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
677 }
678
679 carma_cooperative_perception_interfaces::msg::TrackList track_list;
680 for (const auto & track : track_manager_.get_confirmed_tracks()) {
681 track_list.tracks.push_back(to_ros_msg(track));
682 }
683
684 track_list_pub_->publish(track_list);
685
686 detections_.clear();
687 uuid_index_map_.clear();
688}
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 351 of file multiple_object_tracker_component.cpp.

353{
354 RCLCPP_INFO(get_logger(), "Lifecycle transition: activating");
355
356 if (pipeline_execution_timer_ != nullptr) {
357 // The user might have changed the timer period since last time the Node was active
359 }
360
361 const std::chrono::duration<double, std::nano> period_ns{mot::remove_units(execution_period_)};
363 rclcpp::create_timer(this, this->get_clock(), period_ns, [this] { execute_pipeline(); });
364
365 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated");
366
367 return carma_ros2_utils::CallbackReturn::SUCCESS;
368}
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 384 of file multiple_object_tracker_component.cpp.

386{
387 RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up");
388
389 // CarmaLifecycleNode does not handle subscriber pointer reseting for us
390 detection_list_sub_.reset();
391
392 undeclare_parameter("execution_frequency_hz");
393 remove_on_set_parameters_callback(on_set_parameters_callback_.get());
394
395 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up");
396
397 return carma_ros2_utils::CallbackReturn::SUCCESS;
398}
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(), "Cannot change parameter 'execution_frequency_hz': " + result.reason);
287
288 break;
289 } else {
290 this->execution_period_ = 1 / units::frequency::hertz_t{parameter.as_double()};
291 }
292 } else if (parameter.get_name() == "track_promotion_threshold") {
293 if (this->get_current_state().label() == "active") {
294 result.successful = false;
295 result.reason = "parameter is read-only while node is in 'Active' state";
296
297 RCLCPP_ERROR(
298 get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason);
299
300 break;
301 } else {
302 if (const auto value{parameter.as_int()}; value < 0) {
303 result.successful = false;
304 result.reason = "parameter must be nonnegative";
305 } else {
306 this->track_manager_.set_promotion_threshold_and_update(
307 mot::PromotionThreshold{static_cast<std::size_t>(value)});
308 }
309 }
310 } else if (parameter.get_name() == "track_removal_threshold") {
311 if (this->get_current_state().label() == "active") {
312 result.successful = false;
313 result.reason = "parameter is read-only while node is in 'Active' state";
314
315 RCLCPP_ERROR(
316 get_logger(), "Cannot change parameter 'execution_frequency_hz': " + result.reason);
317
318 break;
319 } else {
320 if (const auto value{parameter.as_int()}; value < 0) {
321 result.successful = false;
322 result.reason = "parameter must be nonnegative";
323 } else {
324 this->track_manager_.set_removal_threshold_and_update(
325 mot::RemovalThreshold{static_cast<std::size_t>(value)});
326 }
327 }
328 } else {
329 result.successful = false;
330 result.reason = "Unexpected parameter name '" + parameter.get_name() + '\'';
331 }
332 }
333
334 return result;
335 });
336
337 declare_parameter(
338 "execution_frequency_hz", mot::remove_units(units::frequency::hertz_t{1 / execution_period_}));
339
340 declare_parameter(
341 "track_promotion_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
342
343 declare_parameter(
344 "track_removal_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
345
346 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");
347
348 return carma_ros2_utils::CallbackReturn::SUCCESS;
349}
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 370 of file multiple_object_tracker_component.cpp.

372{
373 RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating");
374
375 // There is currently no way to change a timer's period in ROS 2, so we will
376 // have to create a new one in case a user changes the period.
378
379 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");
380
381 return carma_ros2_utils::CallbackReturn::SUCCESS;
382}

◆ handle_on_shutdown()

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

Definition at line 400 of file multiple_object_tracker_component.cpp.

402{
403 RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down");
404
405 // CarmaLifecycleNode does not handle subscriber pointer reseting for us
406 detection_list_sub_.reset();
407
408 RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down");
409
410 return carma_ros2_utils::CallbackReturn::SUCCESS;
411}

◆ store_new_detections()

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

Definition at line 413 of file multiple_object_tracker_component.cpp.

415{
416 if (std::size(msg.detections) == 0) {
417 RCLCPP_WARN(this->get_logger(), "Not storing detections: incoming detection list is empty");
418 return;
419 }
420
421 for (const auto & detection_msg : msg.detections) {
422 try {
423 const auto detection{make_detection(detection_msg)};
424 const auto uuid{mot::get_uuid(detection)};
425
426 if (uuid_index_map_.count(uuid) == 0) {
427 detections_.push_back(std::move(detection));
428 uuid_index_map_[uuid] = std::size(detections_) - 1;
429 } else {
430 RCLCPP_WARN_STREAM(
431 this->get_logger(),
432 "Detection with ID '" << uuid << "' already exists. Overwriting its data");
433 detections_.at(uuid_index_map_[uuid]) = detection;
434 }
435 } catch (const std::runtime_error & error) {
436 RCLCPP_ERROR(
437 this->get_logger(), "Ignoring detection with ID '%s': %s", detection_msg.id.c_str(),
438 error.what());
439 }
440 }
441}
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: