pacmod3_node.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 PACMod3 v3 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 #include <signal.h>
10 #include <queue>
11 #include <thread>
12 #include <unistd.h>
13 #include <time.h>
14 #include <map>
15 #include <unordered_map>
16 #include <tuple>
17 
18 #include <std_msgs/Bool.h>
19 #include <std_msgs/Float64.h>
20 #include <can_msgs/Frame.h>
21 
22 using namespace AS::Drivers::PACMod3;
23 
24 std::string veh_type_string = "POLARIS_GEM";
26 std::unordered_map<int64_t, ros::Publisher> pub_tx_list;
28 
29 //Vehicle-Specific Publishers
61 
62 // Advertise published messages
80 
81 std::unordered_map<long long, std::shared_ptr<LockedData>> rx_list;
82 std::map<long long, std::tuple<bool, bool, bool>> system_statuses;
83 
84 bool global_keep_going = true;
85 std::mutex keep_going_mut;
86 
87 // Sets the PACMod3 enable flag through CAN.
88 void set_enable(bool val)
89 {
90  for (auto rx_it = rx_list.begin(); rx_it != rx_list.end(); rx_it++)
91  {
92  // This assumes that all data in rx_list are encoded
93  // command messages which means the least significant
94  // bit in their first byte will be the enable flag.
95  std::vector<uint8_t> current_data = rx_it->second->getData();
96 
97  if (val)
98  current_data[0] |= 0x01; // Enable true
99  else
100  current_data[0] &= 0xFE; // Enable false
101 
102  rx_it->second->setData(current_data);
103  }
104 }
105 
106 // Looks up the appropriate LockedData and inserts the command info
107 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdBool::ConstPtr& msg)
108 {
109  auto rx_it = rx_list.find(can_id);
110 
111  if (rx_it != rx_list.end())
112  rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
113  else
114  ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
115 }
116 
117 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
118 {
119  auto rx_it = rx_list.find(can_id);
120 
121  if (rx_it != rx_list.end())
122  rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
123  else
124  ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
125 }
126 
127 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
128 {
129  auto rx_it = rx_list.find(can_id);
130 
131  if (rx_it != rx_list.end())
132  rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
133  else
134  ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
135 }
136 
137 // Listens for incoming requests to change the position of the throttle pedal
138 void callback_accel_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
139 {
141 }
142 
143 // Listens for incoming requests to change the position of the brake pedal
144 void callback_brake_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
145 {
147 }
148 
149 void callback_cruise_control_buttons_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
150 {
152 }
153 
154 void callback_dash_controls_left_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
155 {
157 }
158 
159 void callback_dash_controls_right_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
160 {
162 }
163 
164 //Listens for incoming requests to change the state of the headlights
165 void callback_headlight_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
166 {
168 }
169 
170 //Listens for incoming requests to change the state of the horn
171 void callback_horn_set_cmd(const pacmod_msgs::SystemCmdBool::ConstPtr& msg)
172 {
174 }
175 
176 void callback_media_controls_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
177 {
179 }
180 
181 // Listens for incoming requests to change the gear shifter state
182 void callback_shift_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
183 {
185 }
186 
187 // Listens for incoming requests to change the position of the steering wheel with a speed limit
188 void callback_steer_cmd_sub(const pacmod_msgs::SteerSystemCmd::ConstPtr& msg)
189 {
190  int64_t can_id = SteerCmdMsg::CAN_ID;
191  auto rx_it = rx_list.find(can_id);
192 
193  if (rx_it != rx_list.end())
194  rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
195  else
196  ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
197 }
198 
199 // Listens for incoming requests to change the state of the turn signals
200 void callback_turn_signal_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
201 {
203 }
204 
205 // Listens for incoming requests to change the state of the windshield wipers
206 void callback_wiper_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
207 {
209 }
210 
211 void send_can(long id, const std::vector<unsigned char>& vec)
212 {
213  can_msgs::Frame frame;
214  frame.id = id;
215  frame.is_rtr = false;
216  frame.is_extended = false;
217  frame.is_error = false;
218  frame.dlc = 8;
219  std::copy(vec.begin(), vec.end(), frame.data.begin());
220 
221  frame.header.stamp = ros::Time::now();
222 
223  can_rx_pub.publish(frame);
224 }
225 
226 void can_write()
227 {
228  std::vector<unsigned char> data;
229 
230  const std::chrono::milliseconds loop_pause = std::chrono::milliseconds(33);
231  const std::chrono::milliseconds inter_msg_pause = std::chrono::milliseconds(1);
232  bool keep_going = true;
233 
234  //Set local to global value before looping.
235  keep_going_mut.lock();
236  keep_going = global_keep_going;
237  keep_going_mut.unlock();
238 
239  while (keep_going)
240  {
241  // Write all the data that we have received.
242  for (const auto& element : rx_list)
243  {
244  send_can(element.first, element.second->getData());
245  std::this_thread::sleep_for(inter_msg_pause);
246  }
247 
248  std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
249  next_time += loop_pause;
250  std::this_thread::sleep_until(next_time);
251 
252  //Set local to global immediately before next loop.
253  keep_going_mut.lock();
254  keep_going = global_keep_going;
255  keep_going_mut.unlock();
256  }
257 }
258 
259 void can_read(const can_msgs::Frame::ConstPtr &msg)
260 {
261  auto parser_class = Pacmod3TxMsg::make_message(msg->id);
262  auto pub = pub_tx_list.find(msg->id);
263 
264  // Only parse messages for which we have a parser and a publisher.
265  if (parser_class != NULL && pub != pub_tx_list.end())
266  {
267  parser_class->parse(const_cast<unsigned char *>(&msg->data[0]));
268  handler.fillAndPublish(msg->id, "pacmod", pub->second, parser_class);
269 
270  if (parser_class->isSystem())
271  {
272  auto dc_parser = std::dynamic_pointer_cast<SystemRptMsg>(parser_class);
273 
274  system_statuses[msg->id] = std::make_tuple(dc_parser->enabled,
275  dc_parser->override_active,
276  (dc_parser->command_output_fault |
277  dc_parser->input_output_fault |
278  dc_parser->output_reported_fault |
279  dc_parser->pacmod_fault |
280  dc_parser->vehicle_fault));
281  }
282 
283  if (msg->id == GlobalRptMsg::CAN_ID)
284  {
285  auto dc_parser = std::dynamic_pointer_cast<GlobalRptMsg>(parser_class);
286 
287  std_msgs::Bool bool_pub_msg;
288  bool_pub_msg.data = (dc_parser->enabled);
289  enabled_pub.publish(bool_pub_msg);
290 
291  if (dc_parser->override_active ||
292  dc_parser->fault_active)
293  set_enable(false);
294  }
295  else if (msg->id == VehicleSpeedRptMsg::CAN_ID)
296  {
297  auto dc_parser = std::dynamic_pointer_cast<VehicleSpeedRptMsg>(parser_class);
298 
299  // Now publish by itself
300  std_msgs::Float64 veh_spd_ms_msg;
301  veh_spd_ms_msg.data = (dc_parser->vehicle_speed);
302  vehicle_speed_ms_pub.publish(veh_spd_ms_msg);
303  }
304  }
305 }
306 
307 int main(int argc, char *argv[])
308 {
309  ros::init(argc, argv, "pacmod3");
310  ros::AsyncSpinner spinner(2);
311  ros::NodeHandle n;
312  ros::NodeHandle priv("~");
313  ros::Rate loop_rate(30); //PACMod3 is sending at ~30Hz.
314 
315  //Vehicle-Specific Subscribers
316  std::shared_ptr<ros::Subscriber> wiper_set_cmd_sub,
317  headlight_set_cmd_sub,
318  horn_set_cmd_sub,
319  cruise_control_buttons_set_cmd_sub,
320  dash_controls_left_set_cmd_sub,
321  dash_controls_right_set_cmd_sub,
322  media_controls_set_cmd_sub;
323 
324  // Wait for time to be valid
325  while (ros::Time::now().nsec == 0);
326 
327  // Get and validate parameters
328  if (priv.getParam("vehicle_type", veh_type_string))
329  {
330  ROS_INFO("PACMod3 - Got vehicle type of: %s", veh_type_string.c_str());
331 
332  if (veh_type_string == "POLARIS_GEM")
334  else if (veh_type_string == "POLARIS_RANGER")
336  else if (veh_type_string == "INTERNATIONAL_PROSTAR_122")
338  else if (veh_type_string == "LEXUS_RX_450H")
340  else if (veh_type_string == "VEHICLE_4")
342  else if (veh_type_string == "VEHICLE_5")
344  else
345  {
347  ROS_WARN("PACMod3 - An invalid vehicle type was entered. Assuming POLARIS_GEM.");
348  }
349  }
350 
351  // Advertise published messages
352  can_rx_pub = n.advertise<can_msgs::Frame>("can_rx", 20);
353  global_rpt_pub = n.advertise<pacmod_msgs::GlobalRpt>("parsed_tx/global_rpt", 20);
354  accel_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/accel_rpt", 20);
355  brake_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/brake_rpt", 20);
356  shift_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/shift_rpt", 20);
357  steer_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/steer_rpt", 20);
358  turn_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/turn_rpt", 20);
359  vehicle_speed_pub = n.advertise<pacmod_msgs::VehicleSpeedRpt>("parsed_tx/vehicle_speed_rpt", 20);
360  vin_rpt_pub = n.advertise<pacmod_msgs::VinRpt>("parsed_tx/vin_rpt", 5);
361  accel_aux_rpt_pub = n.advertise<pacmod_msgs::AccelAuxRpt>("parsed_tx/accel_aux_rpt", 20);
362  brake_aux_rpt_pub = n.advertise<pacmod_msgs::BrakeAuxRpt>("parsed_tx/brake_aux_rpt", 20);
363  shift_aux_rpt_pub = n.advertise<pacmod_msgs::ShiftAuxRpt>("parsed_tx/shift_aux_rpt", 20);
364  steer_aux_rpt_pub = n.advertise<pacmod_msgs::SteerAuxRpt>("parsed_tx/steer_aux_rpt", 20);
365  turn_aux_rpt_pub = n.advertise<pacmod_msgs::TurnAuxRpt>("parsed_tx/turn_aux_rpt", 20);
366 
367  enabled_pub = n.advertise<std_msgs::Bool>("as_tx/enabled", 20, true);
368  vehicle_speed_ms_pub = n.advertise<std_msgs::Float64>("as_tx/vehicle_speed", 20);
369  all_system_statuses_pub = n.advertise<pacmod_msgs::AllSystemStatuses>("as_tx/all_system_statuses", 20);
370 
371  std::string frame_id = "pacmod";
372 
373  //Populate handler list
374  pub_tx_list.insert(std::make_pair(GlobalRptMsg::CAN_ID, global_rpt_pub));
375  pub_tx_list.insert(std::make_pair(AccelRptMsg::CAN_ID, accel_rpt_pub));
376  pub_tx_list.insert(std::make_pair(BrakeRptMsg::CAN_ID, brake_rpt_pub));
377  pub_tx_list.insert(std::make_pair(ShiftRptMsg::CAN_ID, shift_rpt_pub));
378  pub_tx_list.insert(std::make_pair(SteerRptMsg::CAN_ID, steer_rpt_pub));
379  pub_tx_list.insert(std::make_pair(TurnSignalRptMsg::CAN_ID, turn_rpt_pub));
380  pub_tx_list.insert(std::make_pair(VehicleSpeedRptMsg::CAN_ID, vehicle_speed_pub));
381  pub_tx_list.insert(std::make_pair(VinRptMsg::CAN_ID, vin_rpt_pub));
382  pub_tx_list.insert(std::make_pair(AccelAuxRptMsg::CAN_ID, accel_aux_rpt_pub));
383  pub_tx_list.insert(std::make_pair(BrakeAuxRptMsg::CAN_ID, brake_aux_rpt_pub));
384  pub_tx_list.insert(std::make_pair(ShiftAuxRptMsg::CAN_ID, shift_aux_rpt_pub));
385  pub_tx_list.insert(std::make_pair(SteerAuxRptMsg::CAN_ID, steer_aux_rpt_pub));
386  pub_tx_list.insert(std::make_pair(TurnAuxRptMsg::CAN_ID, turn_aux_rpt_pub));
387 
388  // Subscribe to messages
389  ros::Subscriber can_tx_sub = n.subscribe("can_tx", 20, can_read);
390 
391  ros::Subscriber accel_cmd_sub = n.subscribe("as_rx/accel_cmd", 20, callback_accel_cmd_sub);
392  ros::Subscriber brake_cmd_sub = n.subscribe("as_rx/brake_cmd", 20, callback_brake_cmd_sub);
393  ros::Subscriber shift_cmd_sub = n.subscribe("as_rx/shift_cmd", 20, callback_shift_set_cmd);
394  ros::Subscriber steer_cmd_sub = n.subscribe("as_rx/steer_cmd", 20, callback_steer_cmd_sub);
395  ros::Subscriber turn_cmd_sub = n.subscribe("as_rx/turn_cmd", 20, callback_turn_signal_set_cmd);
396 
397  // Populate rx list
398  std::shared_ptr<LockedData> accel_data(new LockedData);
399  std::shared_ptr<LockedData> brake_data(new LockedData);
400  std::shared_ptr<LockedData> shift_data(new LockedData);
401  std::shared_ptr<LockedData> steer_data(new LockedData);
402  std::shared_ptr<LockedData> turn_data(new LockedData);
403 
404  rx_list.insert(std::make_pair(AccelCmdMsg::CAN_ID, accel_data));
405  rx_list.insert(std::make_pair(BrakeCmdMsg::CAN_ID, brake_data));
406  rx_list.insert(std::make_pair(ShiftCmdMsg::CAN_ID, shift_data));
407  rx_list.insert(std::make_pair(SteerCmdMsg::CAN_ID, steer_data));
408  rx_list.insert(std::make_pair(TurnSignalCmdMsg::CAN_ID, turn_data));
409 
413  {
414  brake_rpt_detail_1_pub = n.advertise<pacmod_msgs::MotorRpt1>("parsed_tx/brake_rpt_detail_1", 20);
415  brake_rpt_detail_2_pub = n.advertise<pacmod_msgs::MotorRpt2>("parsed_tx/brake_rpt_detail_2", 20);
416  brake_rpt_detail_3_pub = n.advertise<pacmod_msgs::MotorRpt3>("parsed_tx/brake_rpt_detail_3", 20);
417  steering_rpt_detail_1_pub = n.advertise<pacmod_msgs::MotorRpt1>("parsed_tx/steer_rpt_detail_1", 20);
418  steering_rpt_detail_2_pub = n.advertise<pacmod_msgs::MotorRpt2>("parsed_tx/steer_rpt_detail_2", 20);
419  steering_rpt_detail_3_pub = n.advertise<pacmod_msgs::MotorRpt3>("parsed_tx/steer_rpt_detail_3", 20);
420 
421  pub_tx_list.insert(std::make_pair(BrakeMotorRpt1Msg::CAN_ID, brake_rpt_detail_1_pub));
422  pub_tx_list.insert(std::make_pair(BrakeMotorRpt2Msg::CAN_ID, brake_rpt_detail_2_pub));
423  pub_tx_list.insert(std::make_pair(BrakeMotorRpt3Msg::CAN_ID, brake_rpt_detail_3_pub));
424  pub_tx_list.insert(std::make_pair(SteerMotorRpt1Msg::CAN_ID, steering_rpt_detail_1_pub));
425  pub_tx_list.insert(std::make_pair(SteerMotorRpt2Msg::CAN_ID, steering_rpt_detail_2_pub));
426  pub_tx_list.insert(std::make_pair(SteerMotorRpt3Msg::CAN_ID, steering_rpt_detail_3_pub));
427  }
428 
430  {
431  wiper_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/wiper_rpt", 20);
432  wiper_aux_rpt_pub = n.advertise<pacmod_msgs::WiperAuxRpt>("parsed_tx/wiper_aux_rpt", 20);
433 
434  pub_tx_list.insert(std::make_pair(WiperRptMsg::CAN_ID, wiper_rpt_pub));
435  pub_tx_list.insert(std::make_pair(WiperAuxRptMsg::CAN_ID, wiper_aux_rpt_pub));
436 
437  wiper_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/wiper_cmd", 20, callback_wiper_set_cmd)));
438 
439  std::shared_ptr<LockedData> wiper_data(new LockedData);
440  rx_list.insert(std::make_pair(WiperCmdMsg::CAN_ID, wiper_data));
441  }
442 
445  {
446  date_time_rpt_pub = n.advertise<pacmod_msgs::DateTimeRpt>("parsed_tx/date_time_rpt", 20);
447  headlight_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/headlight_rpt", 20);
448  horn_rpt_pub = n.advertise<pacmod_msgs::SystemRptBool>("parsed_tx/horn_rpt", 20);
449  lat_lon_heading_rpt_pub = n.advertise<pacmod_msgs::LatLonHeadingRpt>("parsed_tx/lat_lon_heading_rpt", 20);
450  parking_brake_rpt_pub = n.advertise<pacmod_msgs::SystemRptBool>("parsed_tx/parking_brake_status_rpt", 20);
451  wheel_speed_rpt_pub = n.advertise<pacmod_msgs::WheelSpeedRpt>("parsed_tx/wheel_speed_rpt", 20);
452  yaw_rate_rpt_pub = n.advertise<pacmod_msgs::YawRateRpt>("parsed_tx/yaw_rate_rpt", 20);
453  headlight_aux_rpt_pub = n.advertise<pacmod_msgs::HeadlightAuxRpt>("parsed_tx/headlight_aux_rpt", 20);
454 
455  pub_tx_list.insert(std::make_pair(DateTimeRptMsg::CAN_ID, date_time_rpt_pub));
456  pub_tx_list.insert(std::make_pair(HeadlightRptMsg::CAN_ID, headlight_rpt_pub));
457  pub_tx_list.insert(std::make_pair(HornRptMsg::CAN_ID, horn_rpt_pub));
458  pub_tx_list.insert(std::make_pair(LatLonHeadingRptMsg::CAN_ID, lat_lon_heading_rpt_pub));
459  pub_tx_list.insert(std::make_pair(ParkingBrakeRptMsg::CAN_ID, parking_brake_rpt_pub));
460  pub_tx_list.insert(std::make_pair(WheelSpeedRptMsg::CAN_ID, wheel_speed_rpt_pub));
461  pub_tx_list.insert(std::make_pair(YawRateRptMsg::CAN_ID, yaw_rate_rpt_pub));
462  pub_tx_list.insert(std::make_pair(HeadlightAuxRptMsg::CAN_ID, headlight_aux_rpt_pub));
463 
464  headlight_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/headlight_cmd", 20, callback_headlight_set_cmd)));
465  horn_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/horn_cmd", 20, callback_horn_set_cmd)));
466 
467  std::shared_ptr<LockedData> headlight_data(new LockedData);
468  std::shared_ptr<LockedData> horn_data(new LockedData);
469 
470  rx_list.insert(std::make_pair(HeadlightCmdMsg::CAN_ID, headlight_data));
471  rx_list.insert(std::make_pair(HornCmdMsg::CAN_ID, horn_data));
472  }
473 
475  {
476  detected_object_rpt_pub = n.advertise<pacmod_msgs::DetectedObjectRpt>("parsed_tx/detected_object_rpt", 20);
477  vehicle_dynamics_rpt_pub = n.advertise<pacmod_msgs::VehicleDynamicsRpt>("parsed_tx/vehicle_dynamics_rpt", 20);
478 
479  pub_tx_list.insert(std::make_pair(DetectedObjectRptMsg::CAN_ID, detected_object_rpt_pub));
480  pub_tx_list.insert(std::make_pair(VehicleDynamicsRptMsg::CAN_ID, vehicle_dynamics_rpt_pub));
481  }
482 
484  {
485  steering_pid_rpt_1_pub = n.advertise<pacmod_msgs::SteeringPIDRpt1>("parsed_tx/steer_pid_rpt_1", 20);
486  steering_pid_rpt_2_pub = n.advertise<pacmod_msgs::SteeringPIDRpt2>("parsed_tx/steer_pid_rpt_2", 20);
487  steering_pid_rpt_3_pub = n.advertise<pacmod_msgs::SteeringPIDRpt3>("parsed_tx/steer_pid_rpt_3", 20);
488  steering_pid_rpt_4_pub = n.advertise<pacmod_msgs::SteeringPIDRpt4>("parsed_tx/steer_pid_rpt_4", 20);
489 
490  pub_tx_list.insert(std::make_pair(SteeringPIDRpt1Msg::CAN_ID, steering_pid_rpt_1_pub));
491  pub_tx_list.insert(std::make_pair(SteeringPIDRpt2Msg::CAN_ID, steering_pid_rpt_2_pub));
492  pub_tx_list.insert(std::make_pair(SteeringPIDRpt3Msg::CAN_ID, steering_pid_rpt_3_pub));
493  pub_tx_list.insert(std::make_pair(SteeringPIDRpt4Msg::CAN_ID, steering_pid_rpt_4_pub));
494  }
495 
497  {
498  // cruise_control_buttons_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/cruise_control_buttons_rpt", 20);
499  // dash_controls_left_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/dash_controls_left_rpt", 20);
500  // dash_controls_right_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/dash_controls_right_rpt", 20);
501  // media_controls_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/media_controls_rpt", 20);
502  occupancy_rpt_pub = n.advertise<pacmod_msgs::OccupancyRpt>("parsed_tx/occupancy_rpt", 20);
503  interior_lights_rpt_pub = n.advertise<pacmod_msgs::InteriorLightsRpt>("parsed_tx/interior_lights_rpt", 20);
504  door_rpt_pub = n.advertise<pacmod_msgs::DoorRpt>("parsed_tx/door_rpt", 20);
505  rear_lights_rpt_pub = n.advertise<pacmod_msgs::RearLightsRpt>("parsed_tx/rear_lights_rpt", 20);
506 
507  // pub_tx_list.insert(std::make_pair(CruiseControlButtonsRptMsg::CAN_ID, cruise_control_buttons_rpt_pub));
508  // pub_tx_list.insert(std::make_pair(DashControlsLeftRptMsg::CAN_ID, dash_controls_left_rpt_pub));
509  // pub_tx_list.insert(std::make_pair(DashControlsRightRptMsg::CAN_ID, dash_controls_right_rpt_pub));
510  // pub_tx_list.insert(std::make_pair(MediaControlsRptMsg::CAN_ID, media_controls_rpt_pub));
511  pub_tx_list.insert(std::make_pair(OccupancyRptMsg::CAN_ID, occupancy_rpt_pub));
512  pub_tx_list.insert(std::make_pair(InteriorLightsRptMsg::CAN_ID, interior_lights_rpt_pub));
513  pub_tx_list.insert(std::make_pair(DoorRptMsg::CAN_ID, door_rpt_pub));
514  pub_tx_list.insert(std::make_pair(RearLightsRptMsg::CAN_ID, rear_lights_rpt_pub));
515 
516  // cruise_control_buttons_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/cruise_control_buttons_cmd", 20, callback_cruise_control_buttons_set_cmd)));
517  // dash_controls_left_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/dash_controls_left_cmd", 20, callback_dash_controls_left_set_cmd)));
518  // dash_controls_right_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/dash_controls_right_cmd", 20, callback_dash_controls_right_set_cmd)));
519  // media_controls_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/media_controls_cmd", 20, callback_media_controls_set_cmd)));
520 
521  // std::shared_ptr<LockedData> cruise_control_buttons_data(new LockedData);
522  // std::shared_ptr<LockedData> dash_controls_left_data(new LockedData);
523  // std::shared_ptr<LockedData> dash_controls_right_data(new LockedData);
524  // std::shared_ptr<LockedData> media_controls_data(new LockedData);
525 
526  // rx_list.insert(std::make_pair(CruiseControlButtonsCmdMsg::CAN_ID, cruise_control_buttons_data));
527  // rx_list.insert(std::make_pair(DashControlsLeftCmdMsg::CAN_ID, dash_controls_left_data));
528  // rx_list.insert(std::make_pair(DashControlsRightCmdMsg::CAN_ID, dash_controls_right_data));
529  // rx_list.insert(std::make_pair(MediaControlsCmdMsg::CAN_ID, media_controls_data));
530  }
531 
532  // Initialize rx_list with all 0s
533  for (auto rx_it = rx_list.begin(); rx_it != rx_list.end(); rx_it++)
534  {
535  if (rx_it->first == TurnSignalCmdMsg::CAN_ID)
536  {
537  // Turn signals have non-0 initial value.
538  TurnSignalCmdMsg turn_encoder;
539  turn_encoder.encode(false, false, false, pacmod_msgs::SystemCmdInt::TURN_NONE);
540  rx_it->second->setData(turn_encoder.data);
541  }
542  else
543  {
544  std::vector<uint8_t> empty_vec;
545  empty_vec.assign(8, 0);
546  rx_it->second->setData(empty_vec);
547  }
548  }
549 
550  // Set initial state
551  set_enable(false);
552 
553  // Start CAN sending thread.
554  std::thread can_write_thread(can_write);
555  // Start callback spinner.
556  spinner.start();
557 
558  while (ros::ok())
559  {
560  pacmod_msgs::AllSystemStatuses ss_msg;
561 
562  for (auto system = system_statuses.begin(); system != system_statuses.end(); ++system)
563  {
564  pacmod_msgs::KeyValuePair kvp;
565 
566  if (system->first == AccelRptMsg::CAN_ID)
567  kvp.key = "Accelerator";
568  else if (system->first == BrakeRptMsg::CAN_ID)
569  kvp.key = "Brakes";
570  else if (system->first == CruiseControlButtonsRptMsg::CAN_ID)
571  kvp.key = "Cruise Control Buttons";
572  else if (system->first == DashControlsLeftRptMsg::CAN_ID)
573  kvp.key = "Dash Controls Left";
574  else if (system->first == DashControlsRightRptMsg::CAN_ID)
575  kvp.key = "Dash Controls Right";
576  else if (system->first == HazardLightRptMsg::CAN_ID)
577  kvp.key = "Hazard Lights";
578  else if (system->first == HeadlightRptMsg::CAN_ID)
579  kvp.key = "Headlights";
580  else if (system->first == HornRptMsg::CAN_ID)
581  kvp.key = "Horn";
582  else if (system->first == MediaControlsRptMsg::CAN_ID)
583  kvp.key = "Media Controls";
584  else if (system->first == ParkingBrakeRptMsg::CAN_ID)
585  kvp.key = "Parking Brake";
586  else if (system->first == ShiftRptMsg::CAN_ID)
587  kvp.key = "Shifter";
588  else if (system->first == SteerRptMsg::CAN_ID)
589  kvp.key = "Steering";
590  else if (system->first == TurnSignalRptMsg::CAN_ID)
591  kvp.key = "Turn Signals";
592  else if (system->first == WiperRptMsg::CAN_ID)
593  kvp.key = "Wipers";
594 
595  kvp.value = std::get<0>(system->second) ? "True" : "False";
596 
597  ss_msg.enabled_status.push_back(kvp);
598 
599  kvp.value = std::get<1>(system->second) ? "True" : "False";
600 
601  ss_msg.overridden_status.push_back(kvp);
602 
603  kvp.value = std::get<2>(system->second) ? "True" : "False";
604 
605  ss_msg.fault_status.push_back(kvp);
606  }
607 
608  all_system_statuses_pub.publish(ss_msg);
609 
610  loop_rate.sleep();
611  }
612 
613  // Make sure it's disabled when node shuts down
614  set_enable(false);
615 
616  keep_going_mut.lock();
617  global_keep_going = false;
618  keep_going_mut.unlock();
619 
620  can_write_thread.join();
621 
622  return 0;
623 }
624 
void callback_dash_controls_left_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
ros::Publisher interior_lights_rpt_pub
std::map< long long, std::tuple< bool, bool, bool > > system_statuses
void callback_brake_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr &msg)
void set_enable(bool val)
void callback_shift_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
void lookup_and_encode(const int64_t &can_id, const pacmod_msgs::SystemCmdBool::ConstPtr &msg)
void encode(bool enable, bool ignore_overrides, bool clear_override, uint8_t cmd)
ros::Publisher headlight_aux_rpt_pub
void callback_headlight_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
void can_read(const can_msgs::Frame::ConstPtr &msg)
void callback_media_controls_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
void publish(const boost::shared_ptr< M > &message) const
static std::shared_ptr< Pacmod3TxMsg > make_message(const int64_t &can_id)
static const int64_t CAN_ID
Definition: pacmod3_core.h:779
ros::Publisher wiper_rpt_pub
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::Publisher dash_controls_left_rpt_pub
ros::Publisher steering_pid_rpt_4_pub
Pacmod3TxRosMsgHandler handler
void callback_dash_controls_right_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
ros::Publisher steering_rpt_detail_1_pub
int main(int argc, char *argv[])
ros::Publisher shift_aux_rpt_pub
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Publisher brake_rpt_pub
ros::Publisher steering_rpt_detail_2_pub
void callback_cruise_control_buttons_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
#define ROS_WARN(...)
ros::Publisher steering_rpt_detail_3_pub
void fillAndPublish(const int64_t &can_id, std::string frame_id, ros::Publisher &pub, std::shared_ptr< Pacmod3TxMsg > &parser_class)
ros::Publisher vehicle_specific_rpt_1_pub
ros::Publisher turn_rpt_pub
ros::Publisher media_controls_rpt_pub
ros::Publisher shift_rpt_pub
ros::Publisher occupancy_rpt_pub
std::mutex keep_going_mut
ros::Publisher brake_rpt_detail_1_pub
static const int64_t CAN_ID
Definition: pacmod3_core.h:488
ros::Publisher accel_aux_rpt_pub
std::unordered_map< int64_t, ros::Publisher > pub_tx_list
std::string veh_type_string
ros::Publisher rear_lights_rpt_pub
bool global_keep_going
static const int64_t CAN_ID
Definition: pacmod3_core.h:617
ros::Publisher steer_rpt_pub
ros::Publisher door_rpt_pub
ros::Publisher cruise_control_buttons_rpt_pub
#define ROS_INFO(...)
ros::Publisher turn_aux_rpt_pub
ros::Publisher steering_pid_rpt_1_pub
ROSCPP_DECL bool ok()
ros::Publisher headlight_rpt_pub
std::unordered_map< long long, std::shared_ptr< LockedData > > rx_list
ros::Publisher brake_aux_rpt_pub
ros::Publisher enabled_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher detected_object_rpt_pub
void callback_accel_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr &msg)
ros::Publisher steering_pid_rpt_3_pub
bool sleep()
ros::Publisher steering_pid_rpt_2_pub
void send_can(long id, const std::vector< unsigned char > &vec)
ros::Publisher vehicle_speed_ms_pub
ros::Publisher date_time_rpt_pub
ros::Publisher wheel_speed_rpt_pub
void callback_steer_cmd_sub(const pacmod_msgs::SteerSystemCmd::ConstPtr &msg)
ros::Publisher parking_brake_rpt_pub
ros::Publisher lat_lon_heading_rpt_pub
ros::Publisher yaw_rate_rpt_pub
bool getParam(const std::string &key, std::string &s) const
void callback_horn_set_cmd(const pacmod_msgs::SystemCmdBool::ConstPtr &msg)
ros::Publisher horn_rpt_pub
ros::Publisher vehicle_dynamics_rpt_pub
ros::Publisher wiper_aux_rpt_pub
VehicleType veh_type
void callback_turn_signal_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
void callback_wiper_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr &msg)
static Time now()
static const int64_t CAN_ID
Definition: pacmod3_core.h:189
void can_write()
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)
ros::Publisher brake_rpt_detail_2_pub
ros::Publisher brake_rpt_detail_3_pub
ros::Publisher global_rpt_pub
ros::Publisher vehicle_speed_pub
ros::Publisher steer_aux_rpt_pub
ros::Publisher can_rx_pub
ros::Publisher accel_rpt_pub
ros::Publisher vin_rpt_pub
ros::Publisher dash_controls_right_rpt_pub
ros::Publisher all_system_statuses_pub


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