18#include <rclcpp/rclcpp.hpp>
19#include <rclcpp_components/register_node_macro.hpp>
20#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
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>
39namespace mot = multiple_object_tracking;
43 switch (numeric_value) {
45 return mot::SemanticClass::kUnknown;
47 return mot::SemanticClass::kSmallVehicle;
49 return mot::SemanticClass::kLargeVehicle;
51 return mot::SemanticClass::kPedestrian;
53 return mot::SemanticClass::kMotorcycle;
56 return mot::SemanticClass::kUnknown;
61 switch (semantic_class) {
62 case mot::SemanticClass::kUnknown:
64 case mot::SemanticClass::kSmallVehicle:
66 case mot::SemanticClass::kLargeVehicle:
68 case mot::SemanticClass::kPedestrian:
70 case mot::SemanticClass::kMotorcycle:
81 units::time::second_t{
static_cast<double>(msg.header.stamp.sec)} +
82 units::time::nanosecond_t{
static_cast<double>(msg.header.stamp.nanosec)}};
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);
94 tf2::Matrix3x3 matrix{orientation};
95 matrix.getRPY(roll, pitch, yaw);
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}};
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);
111 return mot::CtrvDetection{
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)}};
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);
132 tf2::Matrix3x3 matrix{orientation};
133 matrix.getRPY(roll, pitch, yaw);
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}};
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);
151 return mot::CtraDetection{
158 switch (msg.motion_model) {
159 case msg.MOTION_MODEL_CTRV:
162 case msg.MOTION_MODEL_CTRA:
165 case msg.MOTION_MODEL_CV:
166 throw std::runtime_error(
"unsupported motion model type '3: constant velocity (CV)'");
169 throw std::runtime_error(
"unkown motion model type '" +
std::to_string(msg.motion_model) +
"'");
178 units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
179 msg.header.frame_id =
"map";
181 msg.id = track.uuid.value();
182 msg.motion_model = msg.MOTION_MODEL_CTRA;
186 tf2::Quaternion orientation;
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();
209 units::time::nanosecond_t{units::math::fmod(track.timestamp, units::time::second_t{1.0})});
210 msg.header.frame_id =
"map";
212 msg.id = track.uuid.value();
213 msg.motion_model = msg.MOTION_MODEL_CTRV;
217 tf2::Quaternion orientation;
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();
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); },
239 throw std::runtime_error{
"cannot make ROS 2 message from track type"};
242 return std::visit(visitor, track);
246: CarmaLifecycleNode{options}
251 const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn
253 RCLCPP_INFO(get_logger(),
"Lifecycle transition: configuring");
255 track_list_pub_ = create_publisher<carma_cooperative_perception_interfaces::msg::TrackList>(
256 "output/track_list", 1);
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);
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());
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";
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";
287 (
"Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
291 this->execution_period_ = 1 / units::frequency::hertz_t{parameter.as_double()};
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";
300 (
"Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
304 if (const auto value{parameter.as_int()}; value < 0) {
305 result.successful =
false;
306 result.reason =
"parameter must be nonnegative";
308 this->track_manager_.set_promotion_threshold_and_update(
309 mot::PromotionThreshold{
static_cast<std::size_t
>(value)});
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";
319 (
"Cannot change parameter 'execution_frequency_hz': " + result.reason).c_str());
323 if (
const auto value{parameter.as_int()}; value < 0) {
324 result.successful =
false;
325 result.reason =
"parameter must be nonnegative";
327 this->track_manager_.set_removal_threshold_and_update(
328 mot::RemovalThreshold{
static_cast<std::size_t
>(value)});
332 result.successful =
false;
333 result.reason =
"Unexpected parameter name '" + parameter.get_name() +
'\'';
341 "execution_frequency_hz",
mot::remove_units(units::frequency::hertz_t{1 / execution_period_}));
344 "track_promotion_threshold",
static_cast<int>(track_manager_.get_promotion_threshold().value));
347 "track_removal_threshold",
static_cast<int>(track_manager_.get_promotion_threshold().value));
349 RCLCPP_INFO(get_logger(),
"Lifecycle transition: successfully configured");
351 return carma_ros2_utils::CallbackReturn::SUCCESS;
354auto MultipleObjectTrackerNode::handle_on_activate(
355 const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn
357 RCLCPP_INFO(get_logger(),
"Lifecycle transition: activating");
359 if (pipeline_execution_timer_ !=
nullptr) {
361 pipeline_execution_timer_->reset();
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(); });
368 RCLCPP_INFO(get_logger(),
"Lifecycle transition: successfully activated");
370 return carma_ros2_utils::CallbackReturn::SUCCESS;
373auto MultipleObjectTrackerNode::handle_on_deactivate(
374 const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn
376 RCLCPP_INFO(get_logger(),
"Lifecycle transition: deactivating");
380 pipeline_execution_timer_.reset();
382 RCLCPP_INFO(get_logger(),
"Lifecycle transition: successfully deactivated");
384 return carma_ros2_utils::CallbackReturn::SUCCESS;
387auto MultipleObjectTrackerNode::handle_on_cleanup(
388 const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn
390 RCLCPP_INFO(get_logger(),
"Lifecycle transition: cleaning up");
393 detection_list_sub_.reset();
395 undeclare_parameter(
"execution_frequency_hz");
396 remove_on_set_parameters_callback(on_set_parameters_callback_.get());
398 RCLCPP_INFO(get_logger(),
"Lifecycle transition: successfully cleaned up");
400 return carma_ros2_utils::CallbackReturn::SUCCESS;
403auto MultipleObjectTrackerNode::handle_on_shutdown(
404 const rclcpp_lifecycle::State & ) -> carma_ros2_utils::CallbackReturn
406 RCLCPP_INFO(get_logger(),
"Lifecycle transition: shutting down");
409 detection_list_sub_.reset();
411 RCLCPP_INFO(get_logger(),
"Lifecycle transition: successfully shut down");
413 return carma_ros2_utils::CallbackReturn::SUCCESS;
416auto MultipleObjectTrackerNode::store_new_detections(
417 const carma_cooperative_perception_interfaces::msg::DetectionList & msg) ->
void
419 if (std::size(msg.detections) == 0) {
420 RCLCPP_WARN(this->get_logger(),
"Not storing detections: incoming detection list is empty");
424 for (
const auto & detection_msg : msg.detections) {
427 const auto uuid{mot::get_uuid(detection)};
429 if (uuid_index_map_.count(uuid) == 0) {
430 detections_.push_back(std::move(detection));
431 uuid_index_map_[uuid] = std::size(detections_) - 1;
435 "Detection with ID '" << uuid <<
"' already exists. Overwriting its data");
436 detections_.at(uuid_index_map_[uuid]) = detection;
438 }
catch (
const std::runtime_error & error) {
440 this->get_logger(),
"Ignoring detection with ID '%s': %s", detection_msg.id.c_str(),
447 std::vector<Detection> & detections, units::time::second_t end_time) ->
void
449 for (
auto & detection : detections) {
450 mot::propagate_to_time(detection, end_time, mot::default_unscented_transform);
456 for (
auto & track : tracks) {
457 mot::propagate_to_time(track, end_time, mot::default_unscented_transform);
472 template <
typename Track,
typename Detection>
475 if constexpr (std::is_same_v<
decltype(track.state),
decltype(detection.state)>) {
476 const auto dist{two_dimensional_distance(track.state, detection.state)};
481 track.semantic_class == mot::SemanticClass::kUnknown ||
482 detection.semantic_class == mot::SemanticClass::kUnknown) {
486 if (track.semantic_class == detection.semantic_class) {
494 template <
typename... TrackAlternatives,
typename... DetectionAlternatives>
496 const std::variant<TrackAlternatives...> & track,
497 const std::variant<DetectionAlternatives...> & detection)
const -> std::optional<float>
500 [
this](
const auto & t,
const auto & d) {
return (*
this)(t, d); }, track, detection);
507 const auto x_diff_sq{
509 const auto y_diff_sq{
512 return std::sqrt(x_diff_sq + y_diff_sq);
518 const auto x_diff_sq{
520 const auto y_diff_sq{
523 return std::sqrt(x_diff_sq + y_diff_sq);
532 template <
typename Detection>
542 const auto abs_diff{std::abs(p_yaw_rad - d_yaw_rad)};
543 sum += std::min(abs_diff, 2 * 3.14159265359 - abs_diff);
548 template <
typename... Alternatives>
549 auto operator()(mot::Point
point,
const std::variant<Alternatives...> & detection)
const ->
double
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 &) {
556 return std::numeric_limits<double>::max();
559 return std::visit(visitor, std::variant<mot::Point>{
point}, detection);
563auto MultipleObjectTrackerNode::execute_pipeline() ->
void
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)};
569 [](
const mot::CtraDetection & d,
const mot::Uuid & u) {
570 return Track{mot::make_track<mot::CtraTrack>(d, u)};
574 throw std::runtime_error(
"cannot make track from given detection");
578 if (track_manager_.get_all_tracks().empty()) {
580 get_logger(),
"List of tracks is empty. Converting detections to tentative tracks");
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()};
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)));
597 track_list_pub_->publish(carma_cooperative_perception_interfaces::msg::TrackList{});
600 uuid_index_map_.clear();
604 const units::time::second_t current_time{this->now().seconds()};
608 const auto predicted_tracks{
predict_track_states(track_manager_.get_all_tracks(), current_time)};
614 mot::prune_track_and_detection_scores_if(scores, [](
const auto & score) {
return score > 1.0; });
616 const auto associations{
617 mot::associate_detections_to_tracks(scores, mot::gnn_association_visitor)};
619 track_manager_.update_track_lists(associations);
621 std::unordered_map<mot::Uuid, Detection> detection_map;
622 for (
const auto & detection : detections_) {
623 detection_map[mot::get_uuid(detection)] = detection;
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);
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);
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);
662 return min_score < 1.0;
665 unassociated_detections.erase(remove_start, std::end(unassociated_detections));
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()};
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)));
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));
687 track_list_pub_->publish(track_list);
690 uuid_index_map_.clear();
MultipleObjectTrackerNode(const rclcpp::NodeOptions &options)
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
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