carma_cooperative_perception::MultipleObjectTrackerNode Class Reference

#include <multiple_object_tracker_component.hpp>

Inheritance diagram for carma_cooperative_perception::MultipleObjectTrackerNode:

Collaboration diagram for carma_cooperative_perception::MultipleObjectTrackerNode:

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 Member Functions
rclcpp::Logger	get_logger () const

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< Detection >	detections_
std::unordered_map< multiple_object_tracking::Uuid, std::size_t >	uuid_index_map_
multiple_object_tracking::FixedThresholdTrackManager< Track >	track_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.

: CarmaLifecycleNode{options}
{
}

Member Function Documentation

execute_pipeline()

auto carma_cooperative_perception::MultipleObjectTrackerNode::execute_pipeline

(

)

-> void

Definition at line 563 of file multiple_object_tracker_component.cpp.

{
  static constexpr mot::Visitor make_track_visitor{
    [](const mot::CtrvDetection & d, const mot::Uuid & u) {
      return Track{mot::make_track<mot::CtrvTrack>(d, u)};
    },
    [](const mot::CtraDetection & d, const mot::Uuid & u) {
      return Track{mot::make_track<mot::CtraTrack>(d, u)};
    },
    [](const auto &) {
      // Currently on support CTRV and CTRA
      throw std::runtime_error("cannot make track from given detection");
    },
  };
 
  if (track_manager_.get_all_tracks().empty()) {
    RCLCPP_DEBUG(
      get_logger(), "List of tracks is empty. Converting detections to tentative tracks");
 
    // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
    // current purposes, but there's no reason it couldn't be restricted or loosened.
    RCLCPP_DEBUG_STREAM(
      get_logger(), "Detection size before clustering: " << detections_.size());
    const auto clusters{mot::cluster_detections(detections_, 0.75)};
    for (const auto & cluster : clusters) {
      const auto detection{std::cbegin(cluster.get_detections())->second};
      const auto uuid_str{mot::get_uuid(detection).value()};
      // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
      // <track_number>-<carla_actor_id> for easier visual analysis by users.
      const mot::Uuid new_uuid{
        std::to_string(lifetime_generated_track_count_++) +
        uuid_str.substr(std::size(uuid_str) - 4, 4)};
      track_manager_.add_tentative_track(
        std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
    }
    RCLCPP_DEBUG_STREAM(
      get_logger(), "Track size after detection clustering: "
      << track_manager_.get_all_tracks().size());
    track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{});
 
    detections_.clear();
    uuid_index_map_.clear();
    return;
  }
 
  const units::time::second_t current_time{this->now().seconds()};
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Starting new cycle, detection size: " << detections_.size());
 
  temporally_align_detections(detections_, current_time);
 
  const auto predicted_tracks{predict_track_states(track_manager_.get_all_tracks(), current_time)};
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Track size after prediction: " << predicted_tracks.size());
  auto scores{
    mot::score_tracks_and_detections(predicted_tracks, detections_, SemanticDistance2dScore{})};
 
  // This pruning distance is an arbitrarily-chosen heuristic. It is working well for our
  // current purposes, but there's no reason it couldn't be restricted or loosened.
  mot::prune_track_and_detection_scores_if(scores, [](const auto & score) { return score > 1.0; });
 
  const auto associations{
    mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)};
 
  track_manager_.update_track_lists(associations);
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Track size after association: " << track_manager_.get_all_tracks().size());
  std::unordered_map<mot::Uuid, Detection> detection_map;
  for (const auto & detection : detections_) {
    detection_map[mot::get_uuid(detection)] = detection;
  }
 
  const mot::HasAssociation has_association{associations};
  for (auto & track : track_manager_.get_all_tracks()) {
    if (has_association(track)) {
      const auto detection_uuids{associations.at(get_uuid(track))};
      const auto first_detection{detection_map[detection_uuids.at(0)]};
      const auto fused_track{
        std::visit(mot::covariance_intersection_visitor, track, first_detection)};
      track_manager_.update_track(mot::get_uuid(track), fused_track);
      RCLCPP_DEBUG_STREAM(
        get_logger(), "Updating track: " << mot::get_uuid(track).value());
    }
  }
 
  // Unassociated detections don't influence the tracking pipeline, so we can add
  // them to the tracker at the end.
  std::vector<Detection> unassociated_detections;
  for (const auto & [uuid, detection] : detection_map) {
    if (!has_association(detection)) {
      unassociated_detections.push_back(detection);
    }
  }
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Unassociated detection size: " << unassociated_detections.size());
 
  // We want to remove unassociated tracks that are close enough to existing tracks
  // to avoid creating duplicates. Duplicate tracks will cause association inconsistencies
  // (flip flopping associations between the two tracks).
  auto remove_start{std::remove_if(
    std::begin(unassociated_detections), std::end(unassociated_detections),
    [&scores](const auto & detection) {
      const auto uuid{mot::get_uuid(detection)};
      auto min_score{std::numeric_limits<float>::infinity()};
      for (const auto & [uuid_pair, score] : scores) {
        if (uuid_pair.second == uuid) {
          min_score = std::min(min_score, score);
        }
      }
 
      // This distance is an arbitrarily-chosen heuristic. It is working well for our
      // current purposes, but there's no reason it couldn't be restricted or loosened.
      return min_score < 1.0;
    })};
 
  unassociated_detections.erase(remove_start, std::end(unassociated_detections));
 
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Unassociated detection size after removal: "
    << unassociated_detections.size());
 
  // This clustering distance is an arbitrarily-chosen heuristic. It is working well for our
  // current purposes, but there's no reason it couldn't be restricted or loosened.
  const auto clusters{mot::cluster_detections(unassociated_detections, 0.75, MetricSe2{})};
  for (const auto & cluster : clusters) {
    const auto detection{std::cbegin(cluster.get_detections())->second};
    const auto uuid_str{mot::get_uuid(detection).value()};
    // CARLA uses three-digit actor identifiers. We want to UUID scheme to be
    // <track_number>-<carla_actor_id> for easier visual analysis by users.
    const mot::Uuid new_uuid{
      std::to_string(lifetime_generated_track_count_++) +
      uuid_str.substr(std::size(uuid_str) - 4, 4)};
    track_manager_.add_tentative_track(
      std::visit(make_track_visitor, detection, std::variant<mot::Uuid>(new_uuid)));
  }
 
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Track size after adding unassociated detections: "
    << track_manager_.get_all_tracks().size());
 
  carma_cooperative_perception_interfaces::msg::TrackList track_list;
  for (const auto & track : track_manager_.get_confirmed_tracks()) {
    track_list.tracks.push_back(to_ros_msg(track));
  }
 
  RCLCPP_DEBUG_STREAM(
    get_logger(), "Confirmed Track size after converting to ROS message: "
    << track_list.tracks.size());
  track_list_pub_->publish(track_list);
 
  detections_.clear();
  uuid_index_map_.clear();
}

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:

get_logger()

rclcpp::Logger carma_cooperative_perception::MultipleObjectTrackerNode::get_logger ( ) const

inlineprivate

Definition at line 85 of file multiple_object_tracker_component.hpp.

  {
    return rclcpp::get_logger("cp_multiple_object_tracker_node");
  }

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.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: activating");
 
  if (pipeline_execution_timer_ != nullptr) {
    // The user might have changed the timer period since last time the Node was active
    pipeline_execution_timer_->reset();
  }
 
  const std::chrono::duration<double, std::nano> period_ns{mot::remove_units(execution_period_)};
  pipeline_execution_timer_ =
    rclcpp::create_timer(this, this->get_clock(), period_ns, [this] { execute_pipeline(); });
 
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully activated");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

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.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: cleaning up");
 
  // CarmaLifecycleNode does not handle subscriber pointer reseting for us
  detection_list_sub_.reset();
 
  undeclare_parameter("execution_frequency_hz");
  remove_on_set_parameters_callback(on_set_parameters_callback_.get());
 
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully cleaned up");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

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.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: configuring");
 
  track_list_pub_ = create_publisher<carma_cooperative_perception_interfaces::msg::TrackList>(
    "output/track_list", 1);
 
  detection_list_sub_ = create_subscription<
    carma_cooperative_perception_interfaces::msg::DetectionList>(
    "input/detection_list", 100,
    [this](const carma_cooperative_perception_interfaces::msg::DetectionList::SharedPtr msg_ptr) {
      if (const auto current_state{this->get_current_state().label()}; current_state == "active") {
        this->store_new_detections(*msg_ptr);
      } else {
        RCLCPP_WARN(
          this->get_logger(),
          "Trying to receive message on the topic '%s', but the containing node is not activated. "
          "Current node state: '%s'",
          this->detection_list_sub_->get_topic_name(), current_state.c_str());
      }
    });
 
  on_set_parameters_callback_ =
    add_on_set_parameters_callback([this](const std::vector<rclcpp::Parameter> & parameters) {
      rcl_interfaces::msg::SetParametersResult result;
      result.successful = true;
      result.reason = "success";
 
      for (const auto & parameter : parameters) {
        if (parameter.get_name() == "execution_frequency_hz") {
          if (this->get_current_state().label() == "active") {
            result.successful = false;
            result.reason = "parameter is read-only while node is in 'Active' state";
 
            RCLCPP_DEBUG(
              get_logger(),
              ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
 
            break;
          } else {
            this->execution_period_ = 1 / units::frequency::hertz_t{parameter.as_double()};
          }
        } else if (parameter.get_name() == "track_promotion_threshold") {
          if (this->get_current_state().label() == "active") {
            result.successful = false;
            result.reason = "parameter is read-only while node is in 'Active' state";
 
            RCLCPP_ERROR(
              get_logger(),
              ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
 
            break;
          } else {
            if (const auto value{parameter.as_int()}; value < 0) {
              result.successful = false;
              result.reason = "parameter must be nonnegative";
            } else {
              this->track_manager_.set_promotion_threshold_and_update(
                mot::PromotionThreshold{static_cast<std::size_t>(value)});
            }
          }
        } else if (parameter.get_name() == "track_removal_threshold") {
          if (this->get_current_state().label() == "active") {
            result.successful = false;
            result.reason = "parameter is read-only while node is in 'Active' state";
 
            RCLCPP_ERROR(
              get_logger(),
              ("Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
 
            break;
          } else {
            if (const auto value{parameter.as_int()}; value < 0) {
              result.successful = false;
              result.reason = "parameter must be nonnegative";
            } else {
              this->track_manager_.set_removal_threshold_and_update(
                mot::RemovalThreshold{static_cast<std::size_t>(value)});
            }
          }
        } else {
          result.successful = false;
          result.reason = "Unexpected parameter name '" + parameter.get_name() + '\'';
        }
      }
 
      return result;
    });
 
  declare_parameter(
    "execution_frequency_hz", mot::remove_units(units::frequency::hertz_t{1 / execution_period_}));
 
  declare_parameter(
    "track_promotion_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
 
  declare_parameter(
    "track_removal_threshold", static_cast<int>(track_manager_.get_promotion_threshold().value));
 
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully configured");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

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.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: deactivating");
 
  // There is currently no way to change a timer's period in ROS 2, so we will
  // have to create a new one in case a user changes the period.
  pipeline_execution_timer_.reset();
 
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully deactivated");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

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.

{
  RCLCPP_INFO(get_logger(), "Lifecycle transition: shutting down");
 
  // CarmaLifecycleNode does not handle subscriber pointer reseting for us
  detection_list_sub_.reset();
 
  RCLCPP_INFO(get_logger(), "Lifecycle transition: successfully shut down");
 
  return carma_ros2_utils::CallbackReturn::SUCCESS;
}

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.

{
  if (std::size(msg.detections) == 0) {
    RCLCPP_WARN(this->get_logger(), "Not storing detections: incoming detection list is empty");
    return;
  }
 
  for (const auto & detection_msg : msg.detections) {
    try {
      const auto detection{make_detection(detection_msg)};
      const auto uuid{mot::get_uuid(detection)};
 
      if (uuid_index_map_.count(uuid) == 0) {
        detections_.push_back(std::move(detection));
        uuid_index_map_[uuid] = std::size(detections_) - 1;
      } else {
        RCLCPP_WARN_STREAM(
          this->get_logger(),
          "Detection with ID '" << uuid << "' already exists. Overwriting its data");
        detections_.at(uuid_index_map_[uuid]) = detection;
      }
    } catch (const std::runtime_error & error) {
      RCLCPP_ERROR(
        this->get_logger(), "Ignoring detection with ID '%s': %s", detection_msg.id.c_str(),
        error.what());
    }
  }
}

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:

• carma_cooperative_perception/include/carma_cooperative_perception/multiple_object_tracker_component.hpp
• carma_cooperative_perception/src/multiple_object_tracker_component.cpp