2222
2323#include < autoware/object_recognition_utils/object_classification.hpp>
2424#include < autoware/universe_utils/geometry/geometry.hpp>
25+ #include < autoware/universe_utils/ros/polling_subscriber.hpp>
2526#include < autoware/universe_utils/ros/transform_listener.hpp>
2627#include < autoware/universe_utils/ros/uuid_helper.hpp>
28+ #include < autoware_vehicle_info_utils/vehicle_info_utils.hpp>
2729#include < rclcpp/rclcpp.hpp>
2830
31+ #include < autoware_map_msgs/msg/detail/lanelet_map_bin__struct.hpp>
2932#include < autoware_map_msgs/msg/lanelet_map_bin.hpp>
3033#include < autoware_perception_msgs/msg/object_classification.hpp>
3134#include < autoware_perception_msgs/msg/predicted_object_kinematics.hpp>
4750
4851namespace autoware ::mtr
4952{
50- using autoware_map_msgs::msg::LaneletMapBin;
53+ using HADMapBin = autoware_map_msgs::msg::LaneletMapBin;
54+ using autoware::vehicle_info_utils::VehicleInfo;
5155using autoware_perception_msgs::msg::ObjectClassification;
5256using autoware_perception_msgs::msg::PredictedObject;
5357using autoware_perception_msgs::msg::PredictedObjectKinematics;
@@ -57,17 +61,6 @@ using autoware_perception_msgs::msg::TrackedObject;
5761using autoware_perception_msgs::msg::TrackedObjects;
5862using nav_msgs::msg::Odometry;
5963
60- // TODO(ktro2828): use received ego size topic
61- // wheel_base: between front wheel center and rear wheel center [m]
62- // wheel_tread: between left wheel center and right wheel center [m]
63- // front_overhang: between front wheel center and vehicle front [m]
64- // rear_overhang: between rear wheel center and vehicle rear [m]
65- // left_overhang: between left wheel center and vehicle left [m]
66- // right_overhang: between right wheel center and vehicle right [m]
67- constexpr float EGO_LENGTH = 4 .0f ;
68- constexpr float EGO_WIDTH = 2 .0f ;
69- constexpr float EGO_HEIGHT = 1 .0f ;
70-
7164class PolylineTypeMap
7265{
7366public:
@@ -88,9 +81,9 @@ class PolylineTypeMap
8881 }
8982 }
9083
91- // Return the ID corresponding to the label type. If specified type is not contained in map,
84+ // Return the ID of the corresponding label type. If specified type is not contained in map,
9285 // return `-1`.
93- int type_to_id (const std::string & type) const
86+ [[nodiscard]] int getTypeID (const std::string & type) const
9487 {
9588 return label_map_.count (type) == 0 ? -1 : label_map_.at (type);
9689 }
@@ -112,42 +105,47 @@ class MTRNode : public rclcpp::Node
112105 void callback (const TrackedObjects::ConstSharedPtr object_msg);
113106
114107 // Callback being invoked when the HD map topic is subscribed.
115- void on_map (const LaneletMapBin ::ConstSharedPtr map_msg);
108+ void onMap (const HADMapBin ::ConstSharedPtr map_msg);
116109
117- // Callback being invoked when the Ego's odometry topic is subscribed .
118- void on_ego ( const Odometry::ConstSharedPtr ego_msg );
110+ // Fetch data of Ego's odometry topic.
111+ bool fetchData ( );
119112
120113 // Convert Lanelet to `PolylineData`.
121- bool lanelet_to_polyline ();
114+ bool convertLaneletToPolyline ();
122115
123116 // Remove ancient agent histories.
124- void remove_ancient_history (
117+ void removeAncientAgentHistory (
125118 const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
126119
127120 // Appends new states to history.
128- void update_history (const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
121+ void updateAgentHistory (
122+ const float current_time, const TrackedObjects::ConstSharedPtr objects_msg);
129123
130124 // Extract ego state stored in the buffer which has the nearest timestamp from current timestamp.
131- AgentState lookup_ego_state (const float current_time) const ;
125+ AgentState extractNearestEgo (const float current_time) const ;
126+
127+ [[nodiscard]] TrackedObject makeEgoTrackedObject (const Odometry::ConstSharedPtr ego_msg) const ;
132128
133129 // Extract target agents and return corresponding indices.
134130 // NOTE: Extract targets in order of proximity, closest first.
135- std::vector<
size_t >
extract_target_agents (
const std::vector<AgentHistory> & histories);
131+ std::vector<
size_t >
extractTargetAgent (
const std::vector<AgentHistory> & histories);
136132
137133 // Return the timestamps relative from the first element.Return the timestamps relative from the
138134 // first element.
139- std::vector<float > get_relative_timestamps () const ;
135+ std::vector<float > getRelativeTimestamps () const ;
140136
141137 // Generate `PredictedObject` from `PredictedTrajectory`.
142- PredictedObject to_predicted_object (
138+ PredictedObject generatePredictedObject (
143139 const TrackedObject & object, const PredictedTrajectory & trajectory);
144140
145141 // ROS Publisher and Subscriber
146142 // TODO(ktro2828): add debug publisher
147143 rclcpp::Publisher<PredictedObjects>::SharedPtr pub_objects_;
148144 rclcpp::Subscription<TrackedObjects>::SharedPtr sub_objects_;
149- rclcpp::Subscription<LaneletMapBin>::SharedPtr sub_map_;
150- rclcpp::Subscription<Odometry>::SharedPtr sub_ego_;
145+ rclcpp::Subscription<HADMapBin>::SharedPtr sub_map_;
146+ // polling subscriber
147+ autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_ego_{
148+ this , " /localization/kinematic_state"
151149
152150 // Lanelet map pointers
153151 std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
@@ -157,6 +155,8 @@ class MTRNode : public rclcpp::Node
157155 // Agent history
158156 std::map<std::string, AgentHistory> agent_history_map_;
159157 std::map<std::string, TrackedObject> object_msg_map_;
158+ TrackedObject ego_tracked_object_;
159+ VehicleInfo vehicle_info_;
160160
161161 // Pose transform listener
162162 autoware::universe_utils::TransformListener transform_listener_;
@@ -168,7 +168,7 @@ class MTRNode : public rclcpp::Node
168168 PolylineTypeMap polyline_type_map_;
169169 std::shared_ptr<PolylineData> polyline_ptr_;
170170 std::vector<std::pair<float , AgentState>> ego_states_;
171- std::vector<float > timestamps_;
171+ std::vector<double > timestamps_;
172172}; // class MTRNode
173173} // namespace autoware::mtr
174174#endif // AUTOWARE__MTR__NODE_HPP_
0 commit comments