35
35
#include < tf2_ros/buffer.h>
36
36
#include < tf2_ros/transform_listener.h>
37
37
38
+ #include < map>
38
39
#include < memory>
39
40
#include < mutex>
40
41
#include < string>
41
- #include < unordered_map>
42
42
#include < utility>
43
43
#include < variant>
44
44
#include < vector>
@@ -55,23 +55,6 @@ using geometry_msgs::msg::Pose;
55
55
using nav_msgs::msg::Odometry;
56
56
using sensor_msgs::msg::PointCloud2;
57
57
58
- enum class PublisherMessageType {
59
- UNKNOWN = 0 ,
60
- CAMERA_INFO = 1 ,
61
- IMAGE = 2 ,
62
- POINTCLOUD2 = 3 ,
63
- POSE_WITH_COVARIANCE_STAMPED = 4 ,
64
- POSE_STAMPED = 5 ,
65
- ODOMETRY = 6 ,
66
- IMU = 7 ,
67
- CONTROL_MODE_REPORT = 8 ,
68
- GEAR_REPORT = 9 ,
69
- HAZARD_LIGHTS_REPORT = 10 ,
70
- STEERING_REPORT = 11 ,
71
- TURN_INDICATORS_REPORT = 12 ,
72
- VELOCITY_REPORT = 13 ,
73
- };
74
-
75
58
struct TopicPublisherParams
76
59
{
77
60
double dummy_perception_publisher_period; // Only for planning_control mode
@@ -110,68 +93,68 @@ struct PublisherVarAccessor
110
93
{
111
94
// Template struct to check if a type has a header member.
112
95
template <typename T, typename = std::void_t <>>
113
- struct has_header : std::false_type
96
+ struct HasHeader : std::false_type
114
97
{
115
98
};
116
99
117
100
template <typename T>
118
- struct has_header <T, std::void_t <decltype(T::header)>> : std::true_type
101
+ struct HasHeader <T, std::void_t <decltype(T::header)>> : std::true_type
119
102
{
120
103
};
121
104
122
105
// Template struct to check if a type has a stamp member.
123
106
template <typename T, typename = std::void_t <>>
124
- struct has_stamp : std::false_type
107
+ struct HasStamp : std::false_type
125
108
{
126
109
};
127
110
128
111
template <typename T>
129
- struct has_stamp <T, std::void_t <decltype(T::stamp)>> : std::true_type
112
+ struct HasStamp <T, std::void_t <decltype(T::stamp)>> : std::true_type
130
113
{
131
114
};
132
115
133
- template <typename MessageType >
116
+ template <typename T >
134
117
void publish_with_current_time (
135
- const PublisherVariables<MessageType > & publisherVar , const rclcpp::Time & current_time,
118
+ const PublisherVariables<T > & publisher_var , const rclcpp::Time & current_time,
136
119
const bool is_object_spawned) const
137
120
{
138
- std::unique_ptr<MessageType > msg_to_be_published = std::make_unique<MessageType >();
121
+ std::unique_ptr<T > msg_to_be_published = std::make_unique<T >();
139
122
140
123
if (is_object_spawned) {
141
- *msg_to_be_published = *publisherVar .object_spawned_message ;
124
+ *msg_to_be_published = *publisher_var .object_spawned_message ;
142
125
} else {
143
- *msg_to_be_published = *publisherVar .empty_area_message ;
126
+ *msg_to_be_published = *publisher_var .empty_area_message ;
144
127
}
145
- if constexpr (has_header<MessageType >::value) {
128
+ if constexpr (HasHeader<T >::value) {
146
129
msg_to_be_published->header .stamp = current_time;
147
- } else if constexpr (has_stamp<MessageType >::value) {
130
+ } else if constexpr (HasStamp<T >::value) {
148
131
msg_to_be_published->stamp = current_time;
149
132
}
150
- publisherVar .publisher ->publish (std::move (msg_to_be_published));
133
+ publisher_var .publisher ->publish (std::move (msg_to_be_published));
151
134
}
152
135
153
136
template <typename T>
154
- void set_period (T & publisherVar , std::chrono::milliseconds new_period)
137
+ void set_period (T & publisher_var , std::chrono::milliseconds new_period)
155
138
{
156
- publisherVar .period_ms = new_period;
139
+ publisher_var .period_ms = new_period;
157
140
}
158
141
159
142
template <typename T>
160
- std::chrono::milliseconds get_period (const T & publisherVar ) const
143
+ std::chrono::milliseconds get_period (const T & publisher_var ) const
161
144
{
162
- return publisherVar .period_ms ;
145
+ return publisher_var .period_ms ;
163
146
}
164
147
165
148
template <typename T>
166
- std::shared_ptr<void > get_empty_area_message (const T & publisherVar ) const
149
+ std::shared_ptr<void > get_empty_area_message (const T & publisher_var ) const
167
150
{
168
- return std::static_pointer_cast<void >(publisherVar .empty_area_message );
151
+ return std::static_pointer_cast<void >(publisher_var .empty_area_message );
169
152
}
170
153
171
154
template <typename T>
172
- std::shared_ptr<void > get_object_spawned_message (const T & publisherVar ) const
155
+ std::shared_ptr<void > get_object_spawned_message (const T & publisher_var ) const
173
156
{
174
- return std::static_pointer_cast<void >(publisherVar .object_spawned_message );
157
+ return std::static_pointer_cast<void >(publisher_var .object_spawned_message );
175
158
}
176
159
};
177
160
@@ -200,7 +183,6 @@ class TopicPublisher
200
183
std::optional<rclcpp::Time> & spawn_cmd_time, const RunningMode & node_running_mode,
201
184
const EntityParams & entity_params);
202
185
203
- ~TopicPublisher () = default ;
204
186
void reset ();
205
187
206
188
private:
@@ -229,21 +211,25 @@ class TopicPublisher
229
211
230
212
// Variables perception_planning mode
231
213
PointcloudPublisherType pointcloud_publisher_type_;
232
- std::unordered_map <std::string, PublisherVariablesVariant> topic_publisher_map_;
233
- std::unordered_map <std::string, LidarOutputPair>
214
+ std::map <std::string, PublisherVariablesVariant> topic_publisher_map_;
215
+ std::map <std::string, LidarOutputPair>
234
216
lidar_pub_variable_pair_map_; // used to publish pointcloud_raw and pointcloud_raw_ex
235
217
bool is_object_spawned_message_published_{false };
236
218
std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
237
219
238
220
// Functions
239
- void set_message_to_variable_map (
240
- const PublisherMessageType & message_type, const std::string & topic_name,
241
- rosbag2_storage::SerializedBagMessage & bag_message, const bool is_empty_area_message);
242
- void set_period_to_variable_map (
243
- const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
244
- bool set_publishers_and_timers_to_variable_map ();
221
+ template <typename MessageType>
222
+ void set_message (
223
+ const std::string & topic_name, const rosbag2_storage::SerializedBagMessage & bag_message,
224
+ const bool is_empty_area_message);
225
+ void set_period (const std::map<std::string, std::vector<rclcpp::Time>> & time_map);
226
+ std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers ();
227
+ void set_pointcloud_publishers_and_timers (
228
+ const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
245
229
bool check_publishers_initialized_correctly ();
246
230
void init_rosbag_publishers ();
231
+ void init_rosbag_publishers_with_object (const std::string & path_bag_with_object);
232
+ void init_rosbag_publishers_without_object (const std::string & path_bag_without_object);
247
233
void pointcloud_messages_sync_publisher (const PointcloudPublisherType type);
248
234
void pointcloud_messages_async_publisher (
249
235
const std::pair<
@@ -253,7 +239,7 @@ class TopicPublisher
253
239
void dummy_perception_publisher (); // Only for planning_control mode
254
240
255
241
// Timers
256
- std::unordered_map <std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
242
+ std::map <std::string, rclcpp::TimerBase::SharedPtr> pointcloud_publish_timers_map_;
257
243
rclcpp::TimerBase::SharedPtr pointcloud_sync_publish_timer_;
258
244
rclcpp::TimerBase::SharedPtr dummy_perception_timer_;
259
245
};
0 commit comments