21 #ifndef TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_
22 #define TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_
24 #include <autoware_auto_perception_msgs/msg/detected_objects.hpp>
25 #include <autoware_auto_perception_msgs/msg/point_clusters.hpp>
26 #include <autoware_auto_perception_msgs/msg/tracked_objects.hpp>
27 #include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
28 #include <message_filters/cache.h>
29 #include <message_filters/subscriber.h>
30 #include <message_filters/sync_policies/approximate_time.h>
31 #include <message_filters/sync_policies/exact_time.h>
32 #include <message_filters/time_synchronizer.h>
33 #include <mpark_variant_vendor/variant.hpp>
34 #include <nav_msgs/msg/odometry.hpp>
35 #include <rclcpp/rclcpp.hpp>
36 #include <tf2/buffer_core.h>
37 #include <tf2_msgs/msg/tf_message.hpp>
38 #include <tf2_ros/transform_listener.h>
57 using ClassifiedRoiArray = autoware_auto_perception_msgs::msg::ClassifiedRoiArray;
58 using ClustersMsg = autoware_auto_perception_msgs::msg::PointClusters;
60 using PoseMsg = geometry_msgs::msg::PoseWithCovarianceStamped;
61 using OdomCache = message_filters::Cache<OdometryMsg>;
63 template<
class TrackCreationPolicyT>
67 using TrackerVariant = mpark::variant<
78 void TRACKING_NODES_LOCAL clusters_callback(
const ClustersMsg::ConstSharedPtr msg);
79 void TRACKING_NODES_LOCAL odometry_callback(
const OdometryMsg::ConstSharedPtr msg);
80 void TRACKING_NODES_LOCAL pose_callback(
const PoseMsg::ConstSharedPtr msg);
81 void TRACKING_NODES_LOCAL detected_objects_callback(
const DetectedObjects::ConstSharedPtr msg);
82 void TRACKING_NODES_LOCAL classified_roi_callback(
const ClassifiedRoiArray::ConstSharedPtr msg);
87 std::size_t m_history_depth{0UL};
89 tf2::BufferCore m_tf_buffer;
90 tf2_ros::TransformListener m_tf_listener;
93 TrackerVariant m_tracker;
95 rclcpp::Subscription<PoseMsg>::SharedPtr m_pose_subscription{};
96 rclcpp::Subscription<OdometryMsg>::SharedPtr m_odom_subscription{};
97 rclcpp::Subscription<ClustersMsg>::SharedPtr m_clusters_subscription{};
98 rclcpp::Subscription<DetectedObjects>::SharedPtr m_detected_objects_subscription{};
99 std::vector<rclcpp::Subscription<ClassifiedRoiArray>::SharedPtr> m_vision_subscriptions;
102 std::unique_ptr<OdomCache> m_odom_cache{};
105 rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr m_track_publisher
108 rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
109 m_leftover_publisher{};
112 void maybe_visualize(
113 const builtin_interfaces::msg::Time & rois_stamp,
114 DetectedObjects all_objects);
116 bool8_t m_visualize_track_creation =
false;
118 rclcpp::Publisher<DetectedObjects>::SharedPtr m_track_creating_clusters_pub;
124 #endif // TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_