pacmod3_ros_msg_handler.cpp
Go to the documentation of this file.
1 /*
2 * Unpublished Copyright (c) 2009-2017 AutonomouStuff, LLC, All Rights Reserved.
3 *
4 * This file is part of the PACMod ROS 1.0 driver which is released under the MIT license.
5 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6 */
7 
9 
10 using namespace AS::Drivers::PACMod3;
11 
13  _data(),
14  _data_mut()
15 {
16 }
17 
18 std::vector<unsigned char> LockedData::getData() const
19 {
20  std::lock_guard<std::mutex> lck(_data_mut);
21  return _data;
22 }
23 
24 void LockedData::setData(std::vector<unsigned char> new_data)
25 {
26  std::lock_guard<std::mutex> lck(_data_mut);
27  _data = new_data;
28 }
29 
30 void Pacmod3TxRosMsgHandler::fillAndPublish(const int64_t& can_id,
31  std::string frame_id,
32  ros::Publisher& pub,
33  std::shared_ptr<Pacmod3TxMsg>& parser_class)
34 {
35  if (can_id == HornRptMsg::CAN_ID ||
37  {
38  pacmod_msgs::SystemRptBool new_msg;
39  fillSystemRptBool(parser_class, new_msg, frame_id);
40  pub.publish(new_msg);
41  }
42  else if (can_id == CruiseControlButtonsRptMsg::CAN_ID ||
45  can_id == TurnSignalRptMsg::CAN_ID ||
46  can_id == ShiftRptMsg::CAN_ID ||
47  can_id == HeadlightRptMsg::CAN_ID ||
48  can_id == MediaControlsRptMsg::CAN_ID ||
49  can_id == WiperRptMsg::CAN_ID)
50  {
51  pacmod_msgs::SystemRptInt new_msg;
52  fillSystemRptInt(parser_class, new_msg, frame_id);
53  pub.publish(new_msg);
54  }
55  else if (can_id == AccelRptMsg::CAN_ID ||
56  can_id == BrakeRptMsg::CAN_ID ||
57  can_id == SteerRptMsg::CAN_ID)
58  {
59  pacmod_msgs::SystemRptFloat new_msg;
60  fillSystemRptFloat(parser_class, new_msg, frame_id);
61  pub.publish(new_msg);
62  }
63  else if (can_id == GlobalRptMsg::CAN_ID)
64  {
65  pacmod_msgs::GlobalRpt new_msg;
66  fillGlobalRpt(parser_class, new_msg, frame_id);
67  pub.publish(new_msg);
68  }
69  else if (can_id == BrakeMotorRpt1Msg::CAN_ID ||
70  can_id == SteerMotorRpt1Msg::CAN_ID)
71  {
72  pacmod_msgs::MotorRpt1 new_msg;
73  fillMotorRpt1(parser_class, new_msg, frame_id);
74  pub.publish(new_msg);
75  }
76  else if (can_id == BrakeMotorRpt2Msg::CAN_ID ||
77  can_id == SteerMotorRpt2Msg::CAN_ID)
78  {
79  pacmod_msgs::MotorRpt2 new_msg;
80  fillMotorRpt2(parser_class, new_msg, frame_id);
81  pub.publish(new_msg);
82  }
83  else if (can_id == BrakeMotorRpt3Msg::CAN_ID ||
84  can_id == SteerMotorRpt3Msg::CAN_ID)
85  {
86  pacmod_msgs::MotorRpt3 new_msg;
87  fillMotorRpt3(parser_class, new_msg, frame_id);
88  pub.publish(new_msg);
89  }
90  else if (can_id == AccelAuxRptMsg::CAN_ID)
91  {
92  pacmod_msgs::AccelAuxRpt new_msg;
93  fillAccelAuxRpt(parser_class, new_msg, frame_id);
94  pub.publish(new_msg);
95  }
96  else if (can_id == BrakeAuxRptMsg::CAN_ID)
97  {
98  pacmod_msgs::BrakeAuxRpt new_msg;
99  fillBrakeAuxRpt(parser_class, new_msg, frame_id);
100  pub.publish(new_msg);
101  }
102  else if (can_id == DateTimeRptMsg::CAN_ID)
103  {
104  pacmod_msgs::DateTimeRpt new_msg;
105  fillDateTimeRpt(parser_class, new_msg, frame_id);
106  pub.publish(new_msg);
107  }
108  else if (can_id == DoorRptMsg::CAN_ID)
109  {
110  pacmod_msgs::DoorRpt new_msg;
111  fillDoorRpt(parser_class, new_msg, frame_id);
112  pub.publish(new_msg);
113  }
114  else if (can_id == HeadlightAuxRptMsg::CAN_ID)
115  {
116  pacmod_msgs::HeadlightAuxRpt new_msg;
117  fillHeadlightAuxRpt(parser_class, new_msg, frame_id);
118  pub.publish(new_msg);
119  }
120  else if (can_id == InteriorLightsRptMsg::CAN_ID)
121  {
122  pacmod_msgs::InteriorLightsRpt new_msg;
123  fillInteriorLightsRpt(parser_class, new_msg, frame_id);
124  pub.publish(new_msg);
125  }
126  else if (can_id == LatLonHeadingRptMsg::CAN_ID)
127  {
128  pacmod_msgs::LatLonHeadingRpt new_msg;
129  fillLatLonHeadingRpt(parser_class, new_msg, frame_id);
130  pub.publish(new_msg);
131  }
132  else if (can_id == OccupancyRptMsg::CAN_ID)
133  {
134  pacmod_msgs::OccupancyRpt new_msg;
135  fillOccupancyRpt(parser_class, new_msg, frame_id);
136  pub.publish(new_msg);
137  }
138  else if (can_id == RearLightsRptMsg::CAN_ID)
139  {
140  pacmod_msgs::RearLightsRpt new_msg;
141  fillRearLightsRpt(parser_class, new_msg, frame_id);
142  pub.publish(new_msg);
143  }
144  else if (can_id == ShiftAuxRptMsg::CAN_ID)
145  {
146  pacmod_msgs::ShiftAuxRpt new_msg;
147  fillShiftAuxRpt(parser_class, new_msg, frame_id);
148  pub.publish(new_msg);
149  }
150  else if (can_id == SteerAuxRptMsg::CAN_ID)
151  {
152  pacmod_msgs::SteerAuxRpt new_msg;
153  fillSteerAuxRpt(parser_class, new_msg, frame_id);
154  pub.publish(new_msg);
155  }
156  else if (can_id == SteeringPIDRpt1Msg::CAN_ID)
157  {
158  pacmod_msgs::SteeringPIDRpt1 new_msg;
159  fillSteeringPIDRpt1(parser_class, new_msg, frame_id);
160  pub.publish(new_msg);
161  }
162  else if (can_id == SteeringPIDRpt2Msg::CAN_ID)
163  {
164  pacmod_msgs::SteeringPIDRpt2 new_msg;
165  fillSteeringPIDRpt2(parser_class, new_msg, frame_id);
166  pub.publish(new_msg);
167  }
168  else if (can_id == SteeringPIDRpt3Msg::CAN_ID)
169  {
170  pacmod_msgs::SteeringPIDRpt3 new_msg;
171  fillSteeringPIDRpt3(parser_class, new_msg, frame_id);
172  pub.publish(new_msg);
173  }
174  else if (can_id == SteeringPIDRpt4Msg::CAN_ID)
175  {
176  pacmod_msgs::SteeringPIDRpt4 new_msg;
177  fillSteeringPIDRpt4(parser_class, new_msg, frame_id);
178  pub.publish(new_msg);
179  }
180  else if (can_id == TurnAuxRptMsg::CAN_ID)
181  {
182  pacmod_msgs::TurnAuxRpt new_msg;
183  fillTurnAuxRpt(parser_class, new_msg, frame_id);
184  pub.publish(new_msg);
185  }
186  else if (can_id == YawRateRptMsg::CAN_ID)
187  {
188  pacmod_msgs::YawRateRpt new_msg;
189  fillYawRateRpt(parser_class, new_msg, frame_id);
190  pub.publish(new_msg);
191  }
192  else if (can_id == VehicleSpeedRptMsg::CAN_ID)
193  {
194  pacmod_msgs::VehicleSpeedRpt new_msg;
195  fillVehicleSpeedRpt(parser_class, new_msg, frame_id);
196  pub.publish(new_msg);
197  }
198  else if (can_id == VinRptMsg::CAN_ID)
199  {
200  pacmod_msgs::VinRpt new_msg;
201  fillVinRpt(parser_class, new_msg, frame_id);
202  pub.publish(new_msg);
203  }
204  else if (can_id == WheelSpeedRptMsg::CAN_ID)
205  {
206  pacmod_msgs::WheelSpeedRpt new_msg;
207  fillWheelSpeedRpt(parser_class, new_msg, frame_id);
208  pub.publish(new_msg);
209  }
210  else if (can_id == WiperAuxRptMsg::CAN_ID)
211  {
212  pacmod_msgs::WiperAuxRpt new_msg;
213  fillWiperAuxRpt(parser_class, new_msg, frame_id);
214  pub.publish(new_msg);
215  }
216  else if (can_id == DetectedObjectRptMsg::CAN_ID)
217  {
218  pacmod_msgs::DetectedObjectRpt new_msg;
219  fillDetectedObjectRpt(parser_class, new_msg, frame_id);
220  pub.publish(new_msg);
221  }
222  else if (can_id == VehicleSpecificRpt1Msg::CAN_ID)
223  {
224  pacmod_msgs::VehicleSpecificRpt1 new_msg;
225  fillVehicleSpecificRpt1(parser_class, new_msg, frame_id);
226  pub.publish(new_msg);
227  }
228  else if (can_id == VehicleDynamicsRptMsg::CAN_ID)
229  {
230  pacmod_msgs::VehicleDynamicsRpt new_msg;
231  fillVehicleDynamicsRpt(parser_class, new_msg, frame_id);
232  pub.publish(new_msg);
233  }
234 }
235 
236 // Report messages
237 void Pacmod3TxRosMsgHandler::fillSystemRptBool(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SystemRptBool& new_msg, std::string frame_id)
238 {
239  auto dc_parser = std::dynamic_pointer_cast<SystemRptBoolMsg>(parser_class);
240 
241  new_msg.enabled = dc_parser->enabled;
242  new_msg.override_active = dc_parser->override_active;
243  new_msg.command_output_fault = dc_parser->command_output_fault;
244  new_msg.input_output_fault = dc_parser->input_output_fault;
245  new_msg.output_reported_fault = dc_parser->output_reported_fault;
246  new_msg.pacmod_fault = dc_parser->pacmod_fault;
247  new_msg.vehicle_fault = dc_parser->vehicle_fault;
248 
249  new_msg.manual_input = dc_parser->manual_input;
250  new_msg.command = dc_parser->command;
251  new_msg.output = dc_parser->output;
252 
253  new_msg.header.frame_id = frame_id;
254  new_msg.header.stamp = ros::Time::now();
255 }
256 
257 void Pacmod3TxRosMsgHandler::fillSystemRptInt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SystemRptInt& new_msg, std::string frame_id)
258 {
259  auto dc_parser = std::dynamic_pointer_cast<SystemRptIntMsg>(parser_class);
260 
261  new_msg.enabled = dc_parser->enabled;
262  new_msg.override_active = dc_parser->override_active;
263  new_msg.command_output_fault = dc_parser->command_output_fault;
264  new_msg.input_output_fault = dc_parser->input_output_fault;
265  new_msg.output_reported_fault = dc_parser->output_reported_fault;
266  new_msg.pacmod_fault = dc_parser->pacmod_fault;
267  new_msg.vehicle_fault = dc_parser->vehicle_fault;
268 
269  new_msg.manual_input = dc_parser->manual_input;
270  new_msg.command = dc_parser->command;
271  new_msg.output = dc_parser->output;
272 
273  new_msg.header.frame_id = frame_id;
274  new_msg.header.stamp = ros::Time::now();
275 }
276 
277 void Pacmod3TxRosMsgHandler::fillSystemRptFloat(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SystemRptFloat& new_msg, std::string frame_id)
278 {
279  auto dc_parser = std::dynamic_pointer_cast<SystemRptFloatMsg>(parser_class);
280 
281  new_msg.enabled = dc_parser->enabled;
282  new_msg.override_active = dc_parser->override_active;
283  new_msg.command_output_fault = dc_parser->command_output_fault;
284  new_msg.input_output_fault = dc_parser->input_output_fault;
285  new_msg.output_reported_fault = dc_parser->output_reported_fault;
286  new_msg.pacmod_fault = dc_parser->pacmod_fault;
287  new_msg.vehicle_fault = dc_parser->vehicle_fault;
288 
289  new_msg.manual_input = dc_parser->manual_input;
290  new_msg.command = dc_parser->command;
291  new_msg.output = dc_parser->output;
292 
293  new_msg.header.frame_id = frame_id;
294  new_msg.header.stamp = ros::Time::now();
295 }
296 
297 void Pacmod3TxRosMsgHandler::fillGlobalRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::GlobalRpt& new_msg, std::string frame_id)
298 {
299  auto dc_parser = std::dynamic_pointer_cast<GlobalRptMsg>(parser_class);
300 
301  new_msg.enabled = dc_parser->enabled;
302  new_msg.override_active = dc_parser->override_active;
303  new_msg.fault_active = dc_parser->fault_active;
304  new_msg.config_fault_active = dc_parser->config_fault_active;
305  new_msg.user_can_timeout = dc_parser->user_can_timeout;
306  new_msg.steering_can_timeout = dc_parser->steering_can_timeout;
307  new_msg.brake_can_timeout = dc_parser->brake_can_timeout;
308  new_msg.subsystem_can_timeout = dc_parser->subsystem_can_timeout;
309  new_msg.vehicle_can_timeout = dc_parser->vehicle_can_timeout;
310  new_msg.user_can_read_errors = dc_parser->user_can_read_errors;
311 
312  new_msg.header.frame_id = frame_id;
313  new_msg.header.stamp = ros::Time::now();
314 }
315 
316 void Pacmod3TxRosMsgHandler::fillAccelAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::AccelAuxRpt& new_msg, std::string frame_id)
317 {
318  auto dc_parser = std::dynamic_pointer_cast<AccelAuxRptMsg>(parser_class);
319 
320  new_msg.raw_pedal_pos = dc_parser->raw_pedal_pos;
321  new_msg.raw_pedal_force = dc_parser->raw_pedal_force;
322  new_msg.user_interaction = dc_parser->user_interaction;
323  new_msg.raw_pedal_pos_is_valid = dc_parser->raw_pedal_pos_is_valid;
324  new_msg.raw_pedal_force_is_valid = dc_parser->raw_pedal_force_is_valid;
325  new_msg.user_interaction_is_valid = dc_parser->user_interaction_is_valid;
326 
327  new_msg.header.frame_id = frame_id;
328  new_msg.header.stamp = ros::Time::now();
329 }
330 
331 void Pacmod3TxRosMsgHandler::fillBrakeAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::BrakeAuxRpt& new_msg, std::string frame_id)
332 {
333  auto dc_parser = std::dynamic_pointer_cast<BrakeAuxRptMsg>(parser_class);
334 
335  new_msg.raw_pedal_pos = dc_parser->raw_pedal_pos;
336  new_msg.raw_pedal_force = dc_parser->raw_pedal_force;
337  new_msg.raw_brake_pressure = dc_parser->raw_brake_pressure;
338  new_msg.user_interaction = dc_parser->user_interaction;
339  new_msg.brake_on_off = dc_parser->brake_on_off;
340  new_msg.raw_pedal_pos_is_valid = dc_parser->raw_pedal_pos_is_valid;
341  new_msg.raw_pedal_force_is_valid = dc_parser->raw_pedal_force_is_valid;
342  new_msg.user_interaction_is_valid = dc_parser->user_interaction_is_valid;
343  new_msg.brake_on_off_is_valid = dc_parser->brake_on_off_is_valid;
344 
345  new_msg.header.frame_id = frame_id;
346  new_msg.header.stamp = ros::Time::now();
347 }
348 
349 void Pacmod3TxRosMsgHandler::fillDateTimeRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::DateTimeRpt& new_msg, std::string frame_id)
350 {
351  auto dc_parser = std::dynamic_pointer_cast<DateTimeRptMsg>(parser_class);
352 
353  new_msg.year = dc_parser->year;
354  new_msg.month = dc_parser->month;
355  new_msg.day = dc_parser->day;
356  new_msg.hour = dc_parser->hour;
357  new_msg.minute = dc_parser->minute;
358  new_msg.second = dc_parser->second;
359 
360  new_msg.header.frame_id = frame_id;
361  new_msg.header.stamp = ros::Time::now();
362 }
363 
364 void Pacmod3TxRosMsgHandler::fillDetectedObjectRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::DetectedObjectRpt& new_msg, std::string frame_id)
365 {
366  auto dc_parser = std::dynamic_pointer_cast<DetectedObjectRptMsg>(parser_class);
367 
368  new_msg.front_object_distance_low_res = dc_parser->front_object_distance_low_res;
369  new_msg.front_object_distance_high_res = dc_parser->front_object_distance_high_res;
370 
371  new_msg.header.frame_id = frame_id;
372  new_msg.header.stamp = ros::Time::now();
373 }
374 void Pacmod3TxRosMsgHandler::fillDoorRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::DoorRpt& new_msg, std::string frame_id)
375 {
376  auto dc_parser = std::dynamic_pointer_cast<DoorRptMsg>(parser_class);
377 
378  new_msg.driver_door_open = dc_parser->driver_door_open;
379  new_msg.driver_door_open_is_valid = dc_parser->driver_door_open_is_valid;
380  new_msg.passenger_door_open = dc_parser->passenger_door_open;
381  new_msg.passenger_door_open_is_valid = dc_parser->passenger_door_open_is_valid;
382  new_msg.rear_driver_door_open = dc_parser->rear_driver_door_open;
383  new_msg.rear_driver_door_open_is_valid = dc_parser->rear_driver_door_open_is_valid;
384  new_msg.rear_passenger_door_open = dc_parser->rear_passenger_door_open;
385  new_msg.rear_passenger_door_open_is_valid = dc_parser->rear_passenger_door_open_is_valid;
386  new_msg.hood_open = dc_parser->hood_open;
387  new_msg.hood_open_is_valid = dc_parser->hood_open_is_valid;
388  new_msg.trunk_open = dc_parser->trunk_open;
389  new_msg.trunk_open_is_valid = dc_parser->trunk_open_is_valid;
390  new_msg.fuel_door_open = dc_parser->fuel_door_open;
391  new_msg.fuel_door_open_is_valid = dc_parser->fuel_door_open_is_valid;
392 
393  new_msg.header.frame_id = frame_id;
394  new_msg.header.stamp = ros::Time::now();
395 }
396 
397 void Pacmod3TxRosMsgHandler::fillHeadlightAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::HeadlightAuxRpt& new_msg, std::string frame_id)
398 {
399  auto dc_parser = std::dynamic_pointer_cast<HeadlightAuxRptMsg>(parser_class);
400 
401  new_msg.headlights_on = dc_parser->headlights_on;
402  new_msg.headlights_on_bright = dc_parser->headlights_on_bright;
403  new_msg.fog_lights_on = dc_parser->fog_lights_on;
404  new_msg.headlights_mode = dc_parser->headlights_mode;
405  new_msg.headlights_on_is_valid = dc_parser->headlights_on_is_valid;
406  new_msg.headlights_on_bright_is_valid = dc_parser->headlights_on_bright_is_valid;
407  new_msg.fog_lights_on_is_valid = dc_parser->fog_lights_on_is_valid;
408  new_msg.headlights_mode_is_valid = dc_parser->headlights_mode_is_valid;
409 
410  new_msg.header.frame_id = frame_id;
411  new_msg.header.stamp = ros::Time::now();
412 }
413 
414 void Pacmod3TxRosMsgHandler::fillInteriorLightsRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::InteriorLightsRpt& new_msg, std::string frame_id)
415 {
416  auto dc_parser = std::dynamic_pointer_cast<InteriorLightsRptMsg>(parser_class);
417 
418  new_msg.front_dome_lights_on = dc_parser->front_dome_lights_on;
419  new_msg.front_dome_lights_on_is_valid = dc_parser->front_dome_lights_on_is_valid;
420  new_msg.rear_dome_lights_on = dc_parser->rear_dome_lights_on;
421  new_msg.rear_dome_lights_on_is_valid = dc_parser->rear_dome_lights_on_is_valid;
422  new_msg.mood_lights_on = dc_parser->mood_lights_on;
423  new_msg.mood_lights_on_is_valid = dc_parser->mood_lights_on_is_valid;
424  new_msg.dim_level = dc_parser->dim_level;
425  new_msg.dim_level_is_valid = dc_parser->dim_level_is_valid;
426 
427  new_msg.header.frame_id = frame_id;
428  new_msg.header.stamp = ros::Time::now();
429 }
430 
431 void Pacmod3TxRosMsgHandler::fillLatLonHeadingRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::LatLonHeadingRpt& new_msg, std::string frame_id)
432 {
433  auto dc_parser = std::dynamic_pointer_cast<LatLonHeadingRptMsg>(parser_class);
434 
435  new_msg.latitude_degrees = dc_parser->latitude_degrees;
436  new_msg.latitude_minutes = dc_parser->latitude_minutes;
437  new_msg.latitude_seconds = dc_parser->latitude_seconds;
438  new_msg.longitude_degrees = dc_parser->longitude_degrees;
439  new_msg.longitude_minutes = dc_parser->longitude_minutes;
440  new_msg.longitude_seconds = dc_parser->longitude_seconds;
441  new_msg.heading = dc_parser->heading;
442 
443  new_msg.header.frame_id = frame_id;
444  new_msg.header.stamp = ros::Time::now();
445 }
446 
447 void Pacmod3TxRosMsgHandler::fillMotorRpt1(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::MotorRpt1& new_msg, std::string frame_id)
448 {
449  auto dc_parser = std::dynamic_pointer_cast<MotorRpt1Msg>(parser_class);
450 
451  new_msg.current = dc_parser->current;
452  new_msg.position = dc_parser->position;
453 
454  new_msg.header.frame_id = frame_id;
455  new_msg.header.stamp = ros::Time::now();
456 }
457 
458 void Pacmod3TxRosMsgHandler::fillMotorRpt2(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::MotorRpt2& new_msg, std::string frame_id)
459 {
460  auto dc_parser = std::dynamic_pointer_cast<MotorRpt2Msg>(parser_class);
461 
462  new_msg.encoder_temp = dc_parser->encoder_temp;
463  new_msg.motor_temp = dc_parser->motor_temp;
464  new_msg.angular_velocity = dc_parser->velocity;
465 
466  new_msg.header.frame_id = frame_id;
467  new_msg.header.stamp = ros::Time::now();
468 }
469 
470 void Pacmod3TxRosMsgHandler::fillMotorRpt3(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::MotorRpt3& new_msg, std::string frame_id)
471 {
472  auto dc_parser = std::dynamic_pointer_cast<MotorRpt3Msg>(parser_class);
473 
474  new_msg.torque_output = dc_parser->torque_output;
475  new_msg.torque_input = dc_parser->torque_input;
476 
477  new_msg.header.frame_id = frame_id;
478  new_msg.header.stamp = ros::Time::now();
479 }
480 
481 void Pacmod3TxRosMsgHandler::fillOccupancyRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::OccupancyRpt& new_msg, std::string frame_id)
482 {
483  auto dc_parser = std::dynamic_pointer_cast<OccupancyRptMsg>(parser_class);
484 
485  new_msg.driver_seat_occupied = dc_parser->driver_seat_occupied;
486  new_msg.driver_seat_occupied_is_valid = dc_parser->driver_seat_occupied_is_valid;
487  new_msg.passenger_seat_occupied = dc_parser->passenger_seat_occupied;
488  new_msg.passenger_seat_occupied_is_valid = dc_parser->passenger_seat_occupied_is_valid;
489  new_msg.rear_seat_occupied = dc_parser->rear_seat_occupied;
490  new_msg.rear_seat_occupied_is_valid = dc_parser->rear_seat_occupied_is_valid;
491  new_msg.driver_seatbelt_buckled = dc_parser->driver_seatbelt_buckled;
492  new_msg.driver_seatbelt_buckled_is_valid = dc_parser->driver_seatbelt_buckled_is_valid;
493  new_msg.passenger_seatbelt_buckled = dc_parser->passenger_seatbelt_buckled;
494  new_msg.passenger_seatbelt_buckled_is_valid = dc_parser->passenger_seatbelt_buckled_is_valid;
495  new_msg.rear_seatbelt_buckled = dc_parser->rear_seatbelt_buckled;
496  new_msg.rear_seatbelt_buckled_is_valid = dc_parser->rear_seatbelt_buckled_is_valid;
497 
498  new_msg.header.frame_id = frame_id;
499  new_msg.header.stamp = ros::Time::now();
500 }
501 
502 void Pacmod3TxRosMsgHandler::fillRearLightsRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::RearLightsRpt& new_msg, std::string frame_id)
503 {
504  auto dc_parser = std::dynamic_pointer_cast<RearLightsRptMsg>(parser_class);
505 
506  new_msg.brake_lights_on = dc_parser->brake_lights_on;
507  new_msg.brake_lights_on_is_valid = dc_parser->brake_lights_on_is_valid;
508  new_msg.reverse_lights_on = dc_parser->reverse_lights_on;
509  new_msg.reverse_lights_on_is_valid = dc_parser->reverse_lights_on_is_valid;
510 
511  new_msg.header.frame_id = frame_id;
512  new_msg.header.stamp = ros::Time::now();
513 }
514 
515 void Pacmod3TxRosMsgHandler::fillShiftAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::ShiftAuxRpt& new_msg, std::string frame_id)
516 {
517  auto dc_parser = std::dynamic_pointer_cast<ShiftAuxRptMsg>(parser_class);
518 
519  new_msg.between_gears = dc_parser->between_gears;
520  new_msg.stay_in_neutral_mode = dc_parser->stay_in_neutral_mode;
521  new_msg.brake_interlock_active = dc_parser->brake_interlock_active;
522  new_msg.speed_interlock_active = dc_parser->speed_interlock_active;
523  new_msg.between_gears_is_valid = dc_parser->between_gears_is_valid;
524  new_msg.stay_in_neutral_mode_is_valid = dc_parser->stay_in_neutral_mode_is_valid;
525  new_msg.brake_interlock_active_is_valid = dc_parser->brake_interlock_active_is_valid;
526  new_msg.speed_interlock_active_is_valid = dc_parser->speed_interlock_active_is_valid;
527 
528  new_msg.header.frame_id = frame_id;
529  new_msg.header.stamp = ros::Time::now();
530 }
531 
532 void Pacmod3TxRosMsgHandler::fillSteerAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SteerAuxRpt& new_msg, std::string frame_id)
533 {
534  auto dc_parser = std::dynamic_pointer_cast<SteerAuxRptMsg>(parser_class);
535 
536  new_msg.raw_position = dc_parser->raw_position;
537  new_msg.raw_torque = dc_parser->raw_torque;
538  new_msg.rotation_rate = dc_parser->rotation_rate;
539  new_msg.user_interaction = dc_parser->user_interaction;
540  new_msg.raw_position_is_valid = dc_parser->raw_position_is_valid;
541  new_msg.raw_torque_is_valid = dc_parser->raw_torque_is_valid;
542  new_msg.rotation_rate_is_valid = dc_parser->rotation_rate_is_valid;
543  new_msg.user_interaction_is_valid = dc_parser->user_interaction_is_valid;
544 
545  new_msg.header.frame_id = frame_id;
546  new_msg.header.stamp = ros::Time::now();
547 }
548 
549 void Pacmod3TxRosMsgHandler::fillSteeringPIDRpt1(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SteeringPIDRpt1& new_msg, std::string frame_id)
550 {
551  auto dc_parser = std::dynamic_pointer_cast<SteeringPIDRpt1Msg>(parser_class);
552 
553  new_msg.dt = dc_parser->dt;
554  new_msg.Kp = dc_parser->Kp;
555  new_msg.Ki = dc_parser->Ki;
556  new_msg.Kd = dc_parser->Kd;
557 
558  new_msg.header.frame_id = frame_id;
559  new_msg.header.stamp = ros::Time::now();
560 }
561 
562 void Pacmod3TxRosMsgHandler::fillSteeringPIDRpt2(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SteeringPIDRpt2& new_msg, std::string frame_id)
563 {
564  auto dc_parser = std::dynamic_pointer_cast<SteeringPIDRpt2Msg>(parser_class);
565 
566  new_msg.P_term = dc_parser->P_term;
567  new_msg.I_term = dc_parser->I_term;
568  new_msg.D_term = dc_parser->D_term;
569  new_msg.all_terms = dc_parser->all_terms;
570 
571  new_msg.header.frame_id = frame_id;
572  new_msg.header.stamp = ros::Time::now();
573 }
574 
575 void Pacmod3TxRosMsgHandler::fillSteeringPIDRpt3(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SteeringPIDRpt3& new_msg, std::string frame_id)
576 {
577  auto dc_parser = std::dynamic_pointer_cast<SteeringPIDRpt3Msg>(parser_class);
578 
579  new_msg.new_torque = dc_parser->new_torque;
580  new_msg.str_angle_desired = dc_parser->str_angle_desired;
581  new_msg.str_angle_actual = dc_parser->str_angle_actual;
582  new_msg.error = dc_parser->error;
583 
584  new_msg.header.frame_id = frame_id;
585  new_msg.header.stamp = ros::Time::now();
586 }
587 
588 void Pacmod3TxRosMsgHandler::fillSteeringPIDRpt4(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::SteeringPIDRpt4& new_msg, std::string frame_id)
589 {
590  auto dc_parser = std::dynamic_pointer_cast<SteeringPIDRpt4Msg>(parser_class);
591 
592  new_msg.angular_velocity = dc_parser->angular_velocity;
593  new_msg.angular_acceleration = dc_parser->angular_acceleration;
594 
595  new_msg.header.frame_id = frame_id;
596  new_msg.header.stamp = ros::Time::now();
597 }
598 
599 void Pacmod3TxRosMsgHandler::fillTurnAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::TurnAuxRpt& new_msg, std::string frame_id)
600 {
601  auto dc_parser = std::dynamic_pointer_cast<TurnAuxRptMsg>(parser_class);
602 
603  new_msg.driver_blinker_bulb_on = dc_parser->driver_blinker_bulb_on;
604  new_msg.passenger_blinker_bulb_on = dc_parser->passenger_blinker_bulb_on;
605  new_msg.driver_blinker_bulb_on_is_valid = dc_parser->driver_blinker_bulb_on_is_valid;
606  new_msg.passenger_blinker_bulb_on_is_valid = dc_parser->passenger_blinker_bulb_on_is_valid;
607 
608  new_msg.header.frame_id = frame_id;
609  new_msg.header.stamp = ros::Time::now();
610 }
611 
612 void Pacmod3TxRosMsgHandler::fillVehicleDynamicsRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::VehicleDynamicsRpt& new_msg, std::string frame_id)
613 {
614  auto dc_parser = std::dynamic_pointer_cast<VehicleDynamicsRptMsg>(parser_class);
615 
616  new_msg.g_forces = dc_parser->g_forces;
617  new_msg.brake_torque = dc_parser->brake_torque;
618 
619  new_msg.header.frame_id = frame_id;
620  new_msg.header.stamp = ros::Time::now();
621 }
622 
623 void Pacmod3TxRosMsgHandler::fillVehicleSpecificRpt1(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::VehicleSpecificRpt1& new_msg, std::string frame_id)
624 {
625  auto dc_parser = std::dynamic_pointer_cast<VehicleSpecificRpt1Msg>(parser_class);
626 
627  new_msg.shift_pos_1 = dc_parser->shift_pos_1;
628  new_msg.shift_pos_2 = dc_parser->shift_pos_2;
629 
630  new_msg.header.frame_id = frame_id;
631  new_msg.header.stamp = ros::Time::now();
632 }
633 
634 void Pacmod3TxRosMsgHandler::fillVehicleSpeedRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::VehicleSpeedRpt& new_msg, std::string frame_id)
635 {
636  auto dc_parser = std::dynamic_pointer_cast<VehicleSpeedRptMsg>(parser_class);
637 
638  new_msg.vehicle_speed = dc_parser->vehicle_speed;
639  new_msg.vehicle_speed_valid = dc_parser->vehicle_speed_valid;
640  new_msg.vehicle_speed_raw[0] = dc_parser->vehicle_speed_raw[0];
641  new_msg.vehicle_speed_raw[1] = dc_parser->vehicle_speed_raw[1];
642 
643  new_msg.header.frame_id = frame_id;
644  new_msg.header.stamp = ros::Time::now();
645 }
646 
647 void Pacmod3TxRosMsgHandler::fillVinRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::VinRpt& new_msg, std::string frame_id)
648 {
649  auto dc_parser = std::dynamic_pointer_cast<VinRptMsg>(parser_class);
650 
651  new_msg.mfg_code = dc_parser->mfg_code;
652  new_msg.mfg = dc_parser->mfg;
653  new_msg.model_year_code = dc_parser->model_year_code;
654  new_msg.model_year = dc_parser->model_year;
655  new_msg.serial = dc_parser->serial;
656 
657  new_msg.header.frame_id = frame_id;
658  new_msg.header.stamp = ros::Time::now();
659 }
660 
661 void Pacmod3TxRosMsgHandler::fillWheelSpeedRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::WheelSpeedRpt& new_msg, std::string frame_id)
662 {
663  auto dc_parser = std::dynamic_pointer_cast<WheelSpeedRptMsg>(parser_class);
664 
665  new_msg.front_left_wheel_speed = dc_parser->front_left_wheel_speed;
666  new_msg.front_right_wheel_speed = dc_parser->front_right_wheel_speed;
667  new_msg.rear_left_wheel_speed = dc_parser->rear_left_wheel_speed;
668  new_msg.rear_right_wheel_speed = dc_parser->rear_right_wheel_speed;
669 
670  new_msg.header.frame_id = frame_id;
671  new_msg.header.stamp = ros::Time::now();
672 }
673 
674 void Pacmod3TxRosMsgHandler::fillWiperAuxRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::WiperAuxRpt& new_msg, std::string frame_id)
675 {
676  auto dc_parser = std::dynamic_pointer_cast<WiperAuxRptMsg>(parser_class);
677 
678  new_msg.front_wiping = dc_parser->front_wiping;
679  new_msg.front_spraying = dc_parser->front_spraying;
680  new_msg.rear_wiping = dc_parser->rear_wiping;
681  new_msg.rear_spraying = dc_parser->rear_spraying;
682  new_msg.spray_near_empty = dc_parser->spray_near_empty;
683  new_msg.spray_empty = dc_parser->spray_empty;
684  new_msg.front_wiping_is_valid = dc_parser->front_wiping_is_valid;
685  new_msg.front_spraying_is_valid = dc_parser->front_spraying_is_valid;
686  new_msg.rear_wiping_is_valid = dc_parser->rear_wiping_is_valid;
687  new_msg.rear_spraying_is_valid = dc_parser->rear_spraying_is_valid;
688  new_msg.spray_near_empty_is_valid = dc_parser->spray_near_empty_is_valid;
689  new_msg.spray_empty_is_valid = dc_parser->spray_empty_is_valid;
690 
691  new_msg.header.frame_id = frame_id;
692  new_msg.header.stamp = ros::Time::now();
693 }
694 
695 void Pacmod3TxRosMsgHandler::fillYawRateRpt(std::shared_ptr<Pacmod3TxMsg>& parser_class, pacmod_msgs::YawRateRpt& new_msg, std::string frame_id)
696 {
697  auto dc_parser = std::dynamic_pointer_cast<YawRateRptMsg>(parser_class);
698 
699  new_msg.yaw_rate = dc_parser->yaw_rate;
700 
701  new_msg.header.frame_id = frame_id;
702  new_msg.header.stamp = ros::Time::now();
703 }
704 
705 
706 // Command messages
707 std::vector<uint8_t> Pacmod3RxRosMsgHandler::unpackAndEncode(const int64_t& can_id, const pacmod_msgs::SystemCmdBool::ConstPtr& msg)
708 {
709  if (can_id == HornCmdMsg::CAN_ID)
710  {
711  HornCmdMsg encoder;
712  encoder.encode(msg->enable,
713  msg->ignore_overrides,
714  msg->clear_override,
715  msg->command);
716  return encoder.data;
717  }
718  else if (can_id == ParkingBrakeCmdMsg::CAN_ID)
719  {
720  ParkingBrakeCmdMsg encoder;
721  encoder.encode(msg->enable,
722  msg->ignore_overrides,
723  msg->clear_override,
724  msg->command);
725  return encoder.data;
726  }
727  else
728  {
729  std::vector<uint8_t> bad_id;
730  bad_id.assign(8, 0);
731  ROS_ERROR("A bool system command matching the provided CAN ID could not be found.");
732  return bad_id;
733  }
734 }
735 
736 std::vector<uint8_t> Pacmod3RxRosMsgHandler::unpackAndEncode(const int64_t& can_id, const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
737 {
738  if (can_id == AccelCmdMsg::CAN_ID)
739  {
740  AccelCmdMsg encoder;
741  encoder.encode(msg->enable,
742  msg->ignore_overrides,
743  msg->clear_override,
744  msg->command);
745  return encoder.data;
746  }
747  else if (can_id == BrakeCmdMsg::CAN_ID)
748  {
749  BrakeCmdMsg encoder;
750  encoder.encode(msg->enable,
751  msg->ignore_overrides,
752  msg->clear_override,
753  msg->command);
754  return encoder.data;
755  }
756  else
757  {
758  std::vector<uint8_t> bad_id;
759  bad_id.assign(8, 0);
760  ROS_ERROR("A float system command matching the provided CAN ID could not be found.");
761  return bad_id;
762  }
763 }
764 
765 std::vector<uint8_t> Pacmod3RxRosMsgHandler::unpackAndEncode(const int64_t& can_id, const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
766 {
768  {
770  encoder.encode(msg->enable,
771  msg->ignore_overrides,
772  msg->clear_override,
773  msg->command);
774  return encoder.data;
775  }
776  else if (can_id == DashControlsLeftCmdMsg::CAN_ID)
777  {
778  DashControlsLeftCmdMsg encoder;
779  encoder.encode(msg->enable,
780  msg->ignore_overrides,
781  msg->clear_override,
782  msg->command);
783  return encoder.data;
784  }
785  else if (can_id == DashControlsRightCmdMsg::CAN_ID)
786  {
787  DashControlsRightCmdMsg encoder;
788  encoder.encode(msg->enable,
789  msg->ignore_overrides,
790  msg->clear_override,
791  msg->command);
792  return encoder.data;
793  }
794  else if (can_id == HeadlightCmdMsg::CAN_ID)
795  {
796  HeadlightCmdMsg encoder;
797  encoder.encode(msg->enable,
798  msg->ignore_overrides,
799  msg->clear_override,
800  msg->command);
801  return encoder.data;
802  }
803  else if (can_id == MediaControlsCmdMsg::CAN_ID)
804  {
805  MediaControlsCmdMsg encoder;
806  encoder.encode(msg->enable,
807  msg->ignore_overrides,
808  msg->clear_override,
809  msg->command);
810  return encoder.data;
811  }
812  else if (can_id == ShiftCmdMsg::CAN_ID)
813  {
814  ShiftCmdMsg encoder;
815  encoder.encode(msg->enable,
816  msg->ignore_overrides,
817  msg->clear_override,
818  msg->command);
819  return encoder.data;
820  }
821  else if (can_id == TurnSignalCmdMsg::CAN_ID)
822  {
823  TurnSignalCmdMsg encoder;
824  encoder.encode(msg->enable,
825  msg->ignore_overrides,
826  msg->clear_override,
827  msg->command);
828  return encoder.data;
829  }
830  else if (can_id == WiperCmdMsg::CAN_ID)
831  {
832  WiperCmdMsg encoder;
833  encoder.encode(msg->enable,
834  msg->ignore_overrides,
835  msg->clear_override,
836  msg->command);
837  return encoder.data;
838  }
839  else
840  {
841  std::vector<uint8_t> bad_id;
842  bad_id.assign(8, 0);
843  ROS_ERROR("An enum system command matching the provided CAN ID could not be found.");
844  return bad_id;
845  }
846 }
847 
848 std::vector<uint8_t> Pacmod3RxRosMsgHandler::unpackAndEncode(const int64_t& can_id, const pacmod_msgs::SteerSystemCmd::ConstPtr& msg)
849 {
850  if (can_id == SteerCmdMsg::CAN_ID)
851  {
852  SteerCmdMsg encoder;
853  encoder.encode(msg->enable,
854  msg->ignore_overrides,
855  msg->clear_override,
856  msg->command,
857  msg->rotation_rate);
858  return encoder.data;
859  }
860  else
861  {
862  std::vector<uint8_t> bad_id;
863  bad_id.assign(8, 0);
864  ROS_ERROR("A steering system command matching the provided CAN ID could not be found.");
865  return bad_id;
866  }
867 }
void fillDoorRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::DoorRpt &new_msg, std::string frame_id)
void fillSteeringPIDRpt2(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SteeringPIDRpt2 &new_msg, std::string frame_id)
void encode(bool enable, bool ignore_overrides, bool clear_override, uint8_t cmd)
void fillSystemRptFloat(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SystemRptFloat &new_msg, std::string frame_id)
void fillBrakeAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::BrakeAuxRpt &new_msg, std::string frame_id)
void publish(const boost::shared_ptr< M > &message) const
void fillDetectedObjectRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::DetectedObjectRpt &new_msg, std::string frame_id)
static const int64_t CAN_ID
Definition: pacmod3_core.h:779
void fillRearLightsRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::RearLightsRpt &new_msg, std::string frame_id)
void fillDateTimeRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::DateTimeRpt &new_msg, std::string frame_id)
void fillHeadlightAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::HeadlightAuxRpt &new_msg, std::string frame_id)
void fillGlobalRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::GlobalRpt &new_msg, std::string frame_id)
void fillSteeringPIDRpt4(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SteeringPIDRpt4 &new_msg, std::string frame_id)
std::vector< unsigned char > getData() const
void fillOccupancyRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::OccupancyRpt &new_msg, std::string frame_id)
void fillMotorRpt3(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::MotorRpt3 &new_msg, std::string frame_id)
void fillVinRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::VinRpt &new_msg, std::string frame_id)
void fillMotorRpt2(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::MotorRpt2 &new_msg, std::string frame_id)
void fillSystemRptInt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SystemRptInt &new_msg, std::string frame_id)
void fillAndPublish(const int64_t &can_id, std::string frame_id, ros::Publisher &pub, std::shared_ptr< Pacmod3TxMsg > &parser_class)
void fillVehicleSpeedRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::VehicleSpeedRpt &new_msg, std::string frame_id)
void fillTurnAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::TurnAuxRpt &new_msg, std::string frame_id)
void encode(bool enabled, bool ignore_overrides, bool clear_override, float steer_pos, float steer_spd)
void fillSteeringPIDRpt3(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SteeringPIDRpt3 &new_msg, std::string frame_id)
static const int64_t CAN_ID
Definition: pacmod3_core.h:488
void fillWheelSpeedRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::WheelSpeedRpt &new_msg, std::string frame_id)
void fillMotorRpt1(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::MotorRpt1 &new_msg, std::string frame_id)
static const int64_t CAN_ID
Definition: pacmod3_core.h:617
void fillSteerAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SteerAuxRpt &new_msg, std::string frame_id)
std::vector< unsigned char > _data
void fillShiftAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::ShiftAuxRpt &new_msg, std::string frame_id)
void encode(bool enable, bool ignore_overrides, bool clear_override, float cmd)
void fillVehicleSpecificRpt1(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::VehicleSpecificRpt1 &new_msg, std::string frame_id)
void fillVehicleDynamicsRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::VehicleDynamicsRpt &new_msg, std::string frame_id)
void fillSteeringPIDRpt1(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SteeringPIDRpt1 &new_msg, std::string frame_id)
void fillYawRateRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::YawRateRpt &new_msg, std::string frame_id)
void fillAccelAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::AccelAuxRpt &new_msg, std::string frame_id)
void encode(bool enable, bool ignore_overrides, bool clear_override, bool cmd)
static Time now()
static const int64_t CAN_ID
Definition: pacmod3_core.h:189
std::vector< uint8_t > data
Definition: pacmod3_core.h:692
static std::vector< uint8_t > unpackAndEncode(const int64_t &can_id, const pacmod_msgs::SystemCmdBool::ConstPtr &msg)
void fillInteriorLightsRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::InteriorLightsRpt &new_msg, std::string frame_id)
void fillLatLonHeadingRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::LatLonHeadingRpt &new_msg, std::string frame_id)
void setData(std::vector< unsigned char > new_data)
#define ROS_ERROR(...)
void fillSystemRptBool(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::SystemRptBool &new_msg, std::string frame_id)
void fillWiperAuxRpt(std::shared_ptr< Pacmod3TxMsg > &parser_class, pacmod_msgs::WiperAuxRpt &new_msg, std::string frame_id)


pacmod3
Author(s): Joe Driscoll , Josh Whitley
autogenerated on Wed Jun 5 2019 21:22:44