Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
ros2Helpers.cpp
Go to the documentation of this file.
1#include "ros2Helpers.hpp"
2
3LandmarksMarkerWrapper::LandmarksMarkerWrapper(double alpha, std::string nameSpace)
4{
5 this->alpha = alpha;
6 this->nameSpace = nameSpace;
7 this->lastMaxId = 0;
8}
9visualization_msgs::msg::MarkerArray LandmarksMarkerWrapper::markerFromLMs(Track& in, std::string frame, double time)
10{
11 visualization_msgs::msg::MarkerArray out;
12 int id = 0;
13 rclcpp::Time stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
14 // TODO: no copy for lists
15 std::vector<Landmark> timekeepingLms;
16 for (auto& pair : in.time_keeping_gates)
17 {
18 timekeepingLms.push_back(pair.first);
19 timekeepingLms.push_back(pair.second);
20 }
21 std::vector<std::vector<Landmark>> lists { in.left_lane, in.right_lane, in.unknown, timekeepingLms };
22 for (auto& list : lists)
23 {
24 for (Landmark lm : list)
25 {
26 visualization_msgs::msg::Marker marker;
27 marker.header.frame_id = frame;
28 marker.header.stamp = stamp;
29 marker.ns = this->nameSpace;
30 marker.id = id;
31 marker.frame_locked = false;
32 id += 1;
33 marker.type = visualization_msgs::msg::Marker::CYLINDER;
34 marker.action = visualization_msgs::msg::Marker::MODIFY;
35 marker.pose.position.x = lm.position.x();
36 marker.pose.position.y = lm.position.y();
37 marker.pose.orientation.x = 0.0;
38 marker.pose.orientation.y = 0.0;
39 marker.pose.orientation.z = 0.0;
40 marker.pose.orientation.w = 1.0;
41 marker.scale.x = 0.228;
42 marker.scale.y = 0.228;
43 marker.scale.z = 0.325;
44 marker.color.a = this->alpha;
45 marker.color.r = 0.0 / 255.0;
46 marker.color.g = 153.0 / 255.0;
47 marker.color.b = 51.0 / 255.0;
48 switch (lm.type)
49 {
50 case LandmarkType::BLUE:
51 marker.color.r = 51.0 / 255.0;
52 marker.color.g = 102.0 / 255.0;
53 marker.color.b = 255.0 / 255.0;
54 break;
55 case LandmarkType::YELLOW:
56 marker.color.r = 255.0 / 255.0;
57 marker.color.g = 255.0 / 255.0;
58 marker.color.b = 0.0 / 255.0;
59 break;
60 case LandmarkType::ORANGE:
61 marker.color.r = 255.0 / 255.0;
62 marker.color.g = 153.0 / 255.0;
63 marker.color.b = 0.0 / 255.0;
64 break;
65 case LandmarkType::BIG_ORANGE:
66 marker.color.r = 255.0 / 255.0;
67 marker.color.g = 153.0 / 255.0;
68 marker.color.b = 0.0 / 255.0;
69 marker.scale.x = 0.285;
70 marker.scale.y = 0.285;
71 marker.scale.z = 0.5;
72 break;
73 case LandmarkType::TIMEKEEPING:
74 marker.color.r = 204.0 / 255.0;
75 marker.color.g = 51.0 / 255.0;
76 marker.color.b = 153.0 / 255.0;
77 marker.scale.x = 0.285;
78 marker.scale.y = 0.285;
79 marker.scale.z = 0.5;
80 break;
81 case LandmarkType::INVISIBLE:
82 marker.color.r = 99.0 / 255.0;
83 marker.color.g = 100.0 / 255.0;
84 marker.color.b = 102.0 / 255.0;
85 break;
86 case LandmarkType::UNKNOWN:
87 marker.color.r = 0.0 / 255.0;
88 marker.color.g = 153.0 / 255.0;
89 marker.color.b = 51.0 / 255.0;
90 break;
91 }
92
93 marker.pose.position.z = lm.position.z() + marker.scale.z * 0.5;
94 out.markers.push_back(marker);
95 }
96 }
97 // draw lines timekeeping
98 for (int i = 0; i < in.time_keeping_gates.size(); ++i)
99 {
100 auto line = in.time_keeping_gates[i];
101 visualization_msgs::msg::Marker marker;
102 marker.header.frame_id = frame;
103 marker.header.stamp = stamp;
104 marker.ns = this->nameSpace;
105 marker.id = id;
106 marker.frame_locked = false;
107 id += 1;
108 marker.type = visualization_msgs::msg::Marker::LINE_STRIP;
109 marker.action = visualization_msgs::msg::Marker::MODIFY;
110 geometry_msgs::msg::Point p1;
111 p1.x = line.first.position.x();
112 p1.y = line.first.position.y();
113 p1.z = line.first.position.z();
114 marker.points.push_back(p1);
115 geometry_msgs::msg::Point p2;
116 p2.x = line.second.position.x();
117 p2.y = line.second.position.y();
118 p2.z = line.second.position.z();
119 marker.points.push_back(p2);
120 marker.pose.position.x = 0.0;
121 marker.pose.position.y = 0.0;
122 marker.pose.position.z = 0.0;
123 marker.pose.orientation.x = 0.0;
124 marker.pose.orientation.y = 0.0;
125 marker.pose.orientation.z = 0.0;
126 marker.pose.orientation.w = 1.0;
127 marker.scale.x = 0.15;
128 marker.scale.y = 0.0;
129 marker.scale.z = 0.0;
130 marker.color.a = this->alpha;
131 marker.color.r = 153.0 / 255.0;
132 marker.color.g = 0.0 / 255.0;
133 marker.color.b = 153.0 / 255.0;
134 if (i == 0)
135 {
136 // green
137 marker.color.r = 0.0 / 255.0;
138 marker.color.g = 102.0 / 255.0;
139 marker.color.b = 0.0 / 255.0;
140 }
141 else if (i == 1 && (!in.lanesFirstWithLastConnected))
142 {
143 // red
144 marker.color.r = 128.0 / 255.0;
145 marker.color.g = 0.0 / 255.0;
146 marker.color.b = 0.0 / 255.0;
147 }
148 out.markers.push_back(marker);
149 }
150
151 // draw lines left lane
152 visualization_msgs::msg::Marker markerLeftLine;
153 markerLeftLine.header.frame_id = frame;
154 markerLeftLine.header.stamp = stamp;
155 markerLeftLine.ns = this->nameSpace;
156 markerLeftLine.id = id;
157 markerLeftLine.frame_locked = false;
158 id += 1;
159 markerLeftLine.type = visualization_msgs::msg::Marker::LINE_STRIP;
160 markerLeftLine.action = visualization_msgs::msg::Marker::MODIFY;
161 markerLeftLine.pose.position.x = 0.0;
162 markerLeftLine.pose.position.y = 0.0;
163 markerLeftLine.pose.position.z = 0.0;
164 markerLeftLine.pose.orientation.x = 0.0;
165 markerLeftLine.pose.orientation.y = 0.0;
166 markerLeftLine.pose.orientation.z = 0.0;
167 markerLeftLine.pose.orientation.w = 1.0;
168 markerLeftLine.scale.x = 0.05;
169 markerLeftLine.scale.y = 0.0;
170 markerLeftLine.scale.z = 0.0;
171 markerLeftLine.color.a = this->alpha;
172 markerLeftLine.color.r = 51.0 / 255.0;
173 markerLeftLine.color.g = 102.0 / 255.0;
174 markerLeftLine.color.b = 255.0 / 255.0;
175 for (auto& p : in.left_lane)
176 {
177 geometry_msgs::msg::Point point;
178 point.x = p.position.x();
179 point.y = p.position.y();
180 point.z = p.position.z() + 0.5 * markerLeftLine.scale.z;
181 markerLeftLine.points.push_back(point);
182 }
183 if (in.lanesFirstWithLastConnected && (in.left_lane.size() >= 1))
184 {
185 geometry_msgs::msg::Point point;
186 auto p = in.left_lane[0];
187 point.x = p.position.x();
188 point.y = p.position.y();
189 point.z = p.position.z() + 0.5 * markerLeftLine.scale.z;
190 markerLeftLine.points.push_back(point);
191 }
192 out.markers.push_back(markerLeftLine);
193
194 // draw lines right lane
195 visualization_msgs::msg::Marker markerRightLane;
196 markerRightLane.header.frame_id = frame;
197 markerRightLane.header.stamp = stamp;
198 markerRightLane.ns = this->nameSpace;
199 markerRightLane.id = id;
200 markerRightLane.frame_locked = false;
201 id += 1;
202 markerRightLane.type = visualization_msgs::msg::Marker::LINE_STRIP;
203 markerRightLane.action = visualization_msgs::msg::Marker::MODIFY;
204 markerRightLane.pose.position.x = 0.0;
205 markerRightLane.pose.position.y = 0.0;
206 markerRightLane.pose.position.z = 0.0;
207 markerRightLane.pose.orientation.x = 0.0;
208 markerRightLane.pose.orientation.y = 0.0;
209 markerRightLane.pose.orientation.z = 0.0;
210 markerRightLane.pose.orientation.w = 1.0;
211 markerRightLane.scale.x = 0.05;
212 markerRightLane.scale.y = 0.0;
213 markerRightLane.scale.z = 0.0;
214 markerRightLane.color.a = this->alpha;
215 markerRightLane.color.r = 255.0 / 255.0;
216 markerRightLane.color.g = 255.0 / 255.0;
217 markerRightLane.color.b = 0.0 / 255.0;
218 for (auto& p : in.right_lane)
219 {
220 geometry_msgs::msg::Point point;
221 point.x = p.position.x();
222 point.y = p.position.y();
223 point.z = p.position.z() + 0.5 * markerRightLane.scale.z;
224 markerRightLane.points.push_back(point);
225 }
226 if (in.lanesFirstWithLastConnected && (in.right_lane.size() >= 1))
227 {
228 geometry_msgs::msg::Point point;
229 auto p = in.right_lane[0];
230 point.x = p.position.x();
231 point.y = p.position.y();
232 point.z = p.position.z() + 0.5 * markerRightLane.scale.z;
233 markerRightLane.points.push_back(point);
234 }
235 out.markers.push_back(markerRightLane);
236
237 for (int i = id; i < lastMaxId; ++i)
238 {
239 // visualization_msgs::MarkerArray out;
240 visualization_msgs::msg::Marker marker;
241 marker.header.frame_id = frame;
242 marker.header.stamp = stamp;
243 marker.ns = this->nameSpace;
244 marker.id = i;
245 marker.action = visualization_msgs::msg::Marker::DELETE;
246 out.markers.push_back(marker);
247 }
248 lastMaxId = id;
249 return out;
250}
251
252visualization_msgs::msg::MarkerArray LandmarksMarkerWrapper::deleteAllMsg(std::string frame)
253{
254 visualization_msgs::msg::MarkerArray out;
255 visualization_msgs::msg::Marker marker;
256 marker.header.frame_id = frame;
257 marker.header.stamp = rclcpp::Time(static_cast<uint64_t>(0));
258 marker.ns = this->nameSpace;
259 marker.id = 0;
260 marker.action = visualization_msgs::msg::Marker::DELETEALL;
261 out.markers.push_back(marker);
262 return out;
263}
264
265pacsim::msg::PerceptionDetections LandmarkListToRosMessage(
266 const LandmarkList& sensorLms, std::string frameId, double time)
267{
268 pacsim::msg::PerceptionDetections lmsMsg;
269 lmsMsg.header.frame_id = frameId;
270 lmsMsg.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
271 ;
272 for (Landmark lm : sensorLms.list)
273 {
274 pacsim::msg::PerceptionDetection lmMsg;
275 lmMsg.header = lmsMsg.header;
276 lmMsg.pose.pose.position.x = lm.position.x();
277 lmMsg.pose.pose.position.y = lm.position.y();
278 lmMsg.pose.pose.position.z = lm.position.z();
279 lmMsg.pose.covariance[0] = lm.cov(0, 0);
280 lmMsg.pose.covariance[1] = lm.cov(0, 1);
281 lmMsg.pose.covariance[2] = lm.cov(0, 2);
282 lmMsg.pose.covariance[3 + 3] = lm.cov(1, 0);
283 lmMsg.pose.covariance[4 + 3] = lm.cov(1, 1);
284 lmMsg.pose.covariance[5 + 3] = lm.cov(1, 2);
285 lmMsg.pose.covariance[6 + 6] = lm.cov(2, 0);
286 lmMsg.pose.covariance[7 + 6] = lm.cov(2, 1);
287 lmMsg.pose.covariance[8 + 6] = lm.cov(2, 2);
288 // orientation unknown
289 lmMsg.pose.covariance[3 + 3 * 6] = -1;
290 lmMsg.pose.covariance[4 + 4 * 6] = -1;
291 lmMsg.pose.covariance[4 + 5 * 6] = -1;
292
293 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_UNKNOWN]
294 = lm.typeWeights[LandmarkType::UNKNOWN];
295 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_BLUE] = lm.typeWeights[LandmarkType::BLUE];
296 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_YELLOW]
297 = lm.typeWeights[LandmarkType::YELLOW];
298 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_ORANGE]
299 = lm.typeWeights[LandmarkType::ORANGE];
300 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_BIGORANGE]
301 = lm.typeWeights[LandmarkType::BIG_ORANGE];
302 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_TIMEKEEPING]
303 = lm.typeWeights[LandmarkType::TIMEKEEPING];
304 lmMsg.class_probabilities[pacsim::msg::PerceptionDetection::CLASS_INVISIBLE]
305 = lm.typeWeights[LandmarkType::INVISIBLE];
306
307 lmMsg.detection_probability = lm.detection_probability;
308 lmsMsg.detections.push_back(lmMsg);
309 }
310 return lmsMsg;
311}
312
313sensor_msgs::msg::Imu createRosImuMsg(const ImuData& data)
314{
315 sensor_msgs::msg::Imu imuMsg;
316 imuMsg.linear_acceleration.x = data.acc.x();
317 imuMsg.linear_acceleration.y = data.acc.y();
318 imuMsg.linear_acceleration.z = data.acc.z();
319
320 imuMsg.angular_velocity.x = data.rot.x();
321 imuMsg.angular_velocity.y = data.rot.y();
322 imuMsg.angular_velocity.z = data.rot.z();
323
324 imuMsg.orientation_covariance[0] = -1;
325 for (int i = 0; i < 3; ++i)
326 {
327 for (int j = 0; j < 3; ++j)
328 {
329 imuMsg.angular_velocity_covariance[j + i * 3] = data.rot_cov(i, j);
330 imuMsg.linear_acceleration_covariance[j + i * 3] = data.acc_cov(i, j);
331 }
332 }
333
334 imuMsg.header.stamp = rclcpp::Time(static_cast<uint64_t>(data.timestamp * 1e9));
335 imuMsg.header.frame_id = data.frame;
336 return imuMsg;
337}
338
339geometry_msgs::msg::TwistWithCovarianceStamped createRosTwistMsg(
340 const Eigen::Vector3d& vel, const Eigen::Vector3d& rot, const std::string& frame, double time)
341{
342 geometry_msgs::msg::TwistWithCovarianceStamped velMsg;
343 velMsg.twist.twist.linear.x = vel.x();
344 velMsg.twist.twist.linear.y = vel.y();
345 velMsg.twist.twist.linear.z = vel.z();
346
347 velMsg.twist.twist.angular.x = rot.x();
348 velMsg.twist.twist.angular.y = rot.y();
349 velMsg.twist.twist.angular.z = rot.z();
350
351 velMsg.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
352 ;
353 velMsg.header.frame_id = frame;
354 return velMsg;
355}
356
357geometry_msgs::msg::TransformStamped createRosTransformMsg(const Eigen::Vector3d& trans, const Eigen::Vector3d& rot,
358 const std::string& frame, const std::string& child_frame, double time)
359{
360 geometry_msgs::msg::TransformStamped transformStamped;
361 transformStamped.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
362 ;
363 transformStamped.header.frame_id = frame;
364 transformStamped.child_frame_id = child_frame;
365
366 transformStamped.transform.translation.x = trans.x();
367 transformStamped.transform.translation.y = trans.y();
368 transformStamped.transform.translation.z = 0.0;
369
370 tf2::Quaternion q;
371 q.setRPY(0, 0, rot.z());
372 transformStamped.transform.rotation.x = q.x();
373 transformStamped.transform.rotation.y = q.y();
374 transformStamped.transform.rotation.z = q.z();
375 transformStamped.transform.rotation.w = q.w();
376
377 return transformStamped;
378}
379geometry_msgs::msg::TransformStamped createStaticTransform(
380 const std::string& frame, const std::string& child_frame, double time)
381{
382 geometry_msgs::msg::TransformStamped transformStamped;
383 transformStamped.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
384 ;
385 transformStamped.header.frame_id = frame;
386 transformStamped.child_frame_id = child_frame;
387
388 return transformStamped;
389}
390sensor_msgs::msg::JointState createRosJointMsg(
391 const std::vector<std::string>& joint_names, const std::vector<double>& joint_vals, double time)
392{
393 sensor_msgs::msg::JointState jointStamped;
394 jointStamped.header.stamp = rclcpp::Time(static_cast<uint64_t>(time * 1e9));
395 jointStamped.name = joint_names;
396
397 jointStamped.position = joint_vals;
398
399 return jointStamped;
400}
401
402pacsim::msg::GNSS createRosGnssMessage(const GnssData& data)
403{
404 pacsim::msg::GNSS msg;
405 msg.header.stamp = rclcpp::Time(static_cast<uint64_t>(data.timestamp * 1e9));
406 msg.status = data.fix_status;
407 msg.latitude = data.latitude;
408 msg.longitude = data.longitude;
409 msg.altitude = data.altitude;
410
411 for (int i = 0; i < 3; ++i)
412 {
413 for (int j = 0; j < 3; ++j)
414 {
415 msg.position_covariance[3 * i + j] = data.position_covariance(i, j);
416 msg.velocity_covariance[3 * i + j] = data.velocity_covariance(i, j);
417 msg.orientation_covariance[3 * i + j] = data.orientation_covariance(i, j);
418 }
419 }
420
421 msg.velocity_east = data.vel_east;
422 msg.velocity_north = data.vel_north;
423 msg.velocity_up = data.vel_up;
424
425 msg.orientation.w = data.orientation.w;
426 msg.orientation.x = data.orientation.x;
427 msg.orientation.y = data.orientation.y;
428 msg.orientation.z = data.orientation.z;
429
430 return msg;
431}
LandmarksMarkerWrapper::LandmarksMarkerWrapper
LandmarksMarkerWrapper(double alpha, std::string nameSpace)
Definition ros2Helpers.cpp:3
LandmarksMarkerWrapper::markerFromLMs
visualization_msgs::msg::MarkerArray markerFromLMs(Track &in, std::string frame, double time)
Definition ros2Helpers.cpp:9
LandmarksMarkerWrapper::deleteAllMsg
visualization_msgs::msg::MarkerArray deleteAllMsg(std::string frame)
Definition ros2Helpers.cpp:252
createStaticTransform
geometry_msgs::msg::TransformStamped createStaticTransform(const std::string &frame, const std::string &child_frame, double time)
Definition ros2Helpers.cpp:379
createRosTwistMsg
geometry_msgs::msg::TwistWithCovarianceStamped createRosTwistMsg(const Eigen::Vector3d &vel, const Eigen::Vector3d &rot, const std::string &frame, double time)
Definition ros2Helpers.cpp:339
LandmarkListToRosMessage
pacsim::msg::PerceptionDetections LandmarkListToRosMessage(const LandmarkList &sensorLms, std::string frameId, double time)
Definition ros2Helpers.cpp:265
createRosImuMsg
sensor_msgs::msg::Imu createRosImuMsg(const ImuData &data)
Definition ros2Helpers.cpp:313
createRosJointMsg
sensor_msgs::msg::JointState createRosJointMsg(const std::vector< std::string > &joint_names, const std::vector< double > &joint_vals, double time)
Definition ros2Helpers.cpp:390
createRosTransformMsg
geometry_msgs::msg::TransformStamped createRosTransformMsg(const Eigen::Vector3d &trans, const Eigen::Vector3d &rot, const std::string &frame, const std::string &child_frame, double time)
Definition ros2Helpers.cpp:357
createRosGnssMessage
pacsim::msg::GNSS createRosGnssMessage(const GnssData &data)
Definition ros2Helpers.cpp:402
GnssData::vel_up
double vel_up
Definition types.hpp:104
GnssData::velocity_covariance
Eigen::Matrix3d velocity_covariance
Definition types.hpp:105
GnssData::orientation
quaternion orientation
Definition types.hpp:107
GnssData::longitude
double longitude
Definition types.hpp:98
GnssData::timestamp
double timestamp
Definition types.hpp:118
GnssData::vel_north
double vel_north
Definition types.hpp:103
GnssData::latitude
double latitude
Definition types.hpp:97
GnssData::position_covariance
Eigen::Matrix3d position_covariance
Definition types.hpp:100
GnssData::altitude
double altitude
Definition types.hpp:99
GnssData::fix_status
FixStatus fix_status
Definition types.hpp:116
GnssData::orientation_covariance
Eigen::Matrix3d orientation_covariance
Definition types.hpp:108
GnssData::vel_east
double vel_east
Definition types.hpp:102
ImuData::acc_cov
Eigen::Matrix3d acc_cov
Definition types.hpp:88
ImuData::rot
Eigen::Vector3d rot
Definition types.hpp:86
ImuData::timestamp
double timestamp
Definition types.hpp:91
ImuData::acc
Eigen::Vector3d acc
Definition types.hpp:85
ImuData::frame
std::string frame
Definition types.hpp:92
ImuData::rot_cov
Eigen::Matrix3d rot_cov
Definition types.hpp:89
LandmarkList::list
std::vector< Landmark > list
Definition types.hpp:51
Track::time_keeping_gates
std::vector< std::pair< Landmark, Landmark > > time_keeping_gates
Definition types.hpp:45
Track::left_lane
std::vector< Landmark > left_lane
Definition types.hpp:42
Track::lanesFirstWithLastConnected
bool lanesFirstWithLastConnected
Definition types.hpp:41
Track::unknown
std::vector< Landmark > unknown
Definition types.hpp:44
Track::right_lane
std::vector< Landmark > right_lane
Definition types.hpp:43
BLUE
@ BLUE
Definition types.hpp:10
INVISIBLE
@ INVISIBLE
Definition types.hpp:15
BIG_ORANGE
@ BIG_ORANGE
Definition types.hpp:13
TIMEKEEPING
@ TIMEKEEPING
Definition types.hpp:14
UNKNOWN
@ UNKNOWN
Definition types.hpp:17
ORANGE
@ ORANGE
Definition types.hpp:12
YELLOW
@ YELLOW
Definition types.hpp:11