Autoware.Auto
multi_object_tracker_node.hpp
Go to the documentation of this file.
1 // Copyright 2021 The Autoware Foundation
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 //    http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14 //
15 // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 
20 
21 #ifndef TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_
22 #define TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_
23 
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>
41 
42 #include <memory>
43 #include <string>
44 #include <vector>
45 
46 namespace autoware
47 {
48 namespace tracking_nodes
49 {
50 
54 class TRACKING_NODES_PUBLIC MultiObjectTrackerNode : public rclcpp::Node
55 {
57  using ClassifiedRoiArray = autoware_auto_perception_msgs::msg::ClassifiedRoiArray;
58  using ClustersMsg = autoware_auto_perception_msgs::msg::PointClusters;
59  using OdometryMsg = nav_msgs::msg::Odometry;
60  using PoseMsg = geometry_msgs::msg::PoseWithCovarianceStamped;
61  using OdomCache = message_filters::Cache<OdometryMsg>;
62 
63  template<class TrackCreationPolicyT>
66 
67  using TrackerVariant = mpark::variant<
70 
71 public:
73  explicit MultiObjectTrackerNode(const rclcpp::NodeOptions & options);
74 
75 private:
76  TrackerVariant TRACKING_NODES_LOCAL init_tracker(const common::types::bool8_t use_vision);
77 
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);
83 
84 
85  common::types::bool8_t m_use_ndt{true};
86  common::types::bool8_t m_use_vision{true};
87  std::size_t m_history_depth{0UL};
88 
89  tf2::BufferCore m_tf_buffer;
90  tf2_ros::TransformListener m_tf_listener;
91 
93  TrackerVariant m_tracker;
94 
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;
100 
102  std::unique_ptr<OdomCache> m_odom_cache{};
103 
105  rclcpp::Publisher<autoware_auto_perception_msgs::msg::TrackedObjects>::SharedPtr m_track_publisher
106  {};
108  rclcpp::Publisher<autoware_auto_perception_msgs::msg::DetectedObjects>::SharedPtr
109  m_leftover_publisher{};
110 
111  // Visualization variables & functions
112  void maybe_visualize(
113  const builtin_interfaces::msg::Time & rois_stamp,
114  DetectedObjects all_objects);
115 
116  bool8_t m_visualize_track_creation = false;
117 
118  rclcpp::Publisher<DetectedObjects>::SharedPtr m_track_creating_clusters_pub;
119 };
120 
121 } // namespace tracking_nodes
122 } // namespace autoware
123 
124 #endif // TRACKING_NODES__MULTI_OBJECT_TRACKER_NODE_HPP_
tracking_nodes
Definition: tracking_nodes.launch.py:1
autoware::perception::tracking::MultiObjectTracker
A class for multi-object tracking.
Definition: multi_object_tracker.hpp:109
autoware::tracking_nodes::MultiObjectTrackerNode
ROS 2 Node for tracking. Subscribes to DetectedObjects and Odometry or PoseWithCovairanceStamped (dep...
Definition: multi_object_tracker_node.hpp:54
autoware::common::types::bool8_t
bool bool8_t
Definition: types.hpp:39
autoware
This file defines the lanelet2_map_provider_node class.
Definition: quick_sort.hpp:24
multi_object_tracker.hpp
This file defines the multi_object_tracking class.
visibility_control.hpp
autoware::drivers::vehicle_interface::Odometry
autoware_auto_vehicle_msgs::msg::VehicleOdometry Odometry
Definition: safety_state_machine.hpp:50
autoware::perception::tracking::TrackCreator
Class to create new tracks based on a predefined policy and unassociated detections.
Definition: track_creator.hpp:120
DetectedObjects
autoware_auto_perception_msgs::msg::DetectedObjects DetectedObjects
Definition: euclidean_cluster_node.cpp:39