@@ -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,23 +93,23 @@ 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
@@ -142,9 +125,9 @@ struct PublisherVarAccessor
142
125
} else {
143
126
*msg_to_be_published = *publisherVar.empty_area_message ;
144
127
}
145
- if constexpr (has_header <MessageType>::value) {
128
+ if constexpr (HasHeader <MessageType>::value) {
146
129
msg_to_be_published->header .stamp = current_time;
147
- } else if constexpr (has_stamp <MessageType>::value) {
130
+ } else if constexpr (HasStamp <MessageType>::value) {
148
131
msg_to_be_published->stamp = current_time;
149
132
}
150
133
publisherVar.publisher ->publish (std::move (msg_to_be_published));
@@ -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:
@@ -236,12 +218,13 @@ class TopicPublisher
236
218
std::shared_ptr<rclcpp::TimerBase> one_shot_timer_shared_ptr_;
237
219
238
220
// Functions
239
- void set_message_to_variable_map (
221
+ void set_message (
240
222
const PublisherMessageType & message_type, const std::string & topic_name,
241
223
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 ();
224
+ void set_period (const std::unordered_map<std::string, std::vector<rclcpp::Time>> & time_map);
225
+ std::map<std::string, PublisherVariables<PointCloud2>> set_general_publishers_and_timers ();
226
+ void set_pointcloud_publishers_and_timers (
227
+ const std::map<std::string, PublisherVariables<PointCloud2>> & pointcloud_variables_map);
245
228
bool check_publishers_initialized_correctly ();
246
229
void init_rosbag_publishers ();
247
230
void pointcloud_messages_sync_publisher (const PointcloudPublisherType type);
0 commit comments