pacmod3_node.cpp
Go to the documentation of this file.
00001 /*
00002 * Unpublished Copyright (c) 2009-2017 AutonomouStuff, LLC, All Rights Reserved.
00003 *
00004 * This file is part of the PACMod3 v3 ROS 1.0 driver which is released under the MIT license.
00005 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
00006 */
00007 
00008 #include <pacmod3_ros_msg_handler.h>
00009 #include <signal.h>
00010 #include <queue>
00011 #include <thread>
00012 #include <unistd.h>
00013 #include <time.h>
00014 #include <map>
00015 #include <unordered_map>
00016 #include <tuple>
00017 
00018 #include <std_msgs/Bool.h>
00019 #include <std_msgs/Float64.h>
00020 #include <can_msgs/Frame.h>
00021 
00022 using namespace AS::Drivers::PACMod3;
00023 
00024 std::string veh_type_string = "POLARIS_GEM";
00025 VehicleType veh_type = VehicleType::POLARIS_GEM;
00026 std::unordered_map<int64_t, ros::Publisher> pub_tx_list;
00027 Pacmod3TxRosMsgHandler handler;
00028 
00029 //Vehicle-Specific Publishers
00030 ros::Publisher cruise_control_buttons_rpt_pub;
00031 ros::Publisher dash_controls_left_rpt_pub;
00032 ros::Publisher dash_controls_right_rpt_pub;
00033 ros::Publisher media_controls_rpt_pub;
00034 ros::Publisher wiper_rpt_pub;
00035 ros::Publisher wiper_aux_rpt_pub;
00036 ros::Publisher headlight_rpt_pub;
00037 ros::Publisher headlight_aux_rpt_pub;
00038 ros::Publisher horn_rpt_pub;
00039 ros::Publisher wheel_speed_rpt_pub;
00040 ros::Publisher steering_pid_rpt_1_pub;
00041 ros::Publisher steering_pid_rpt_2_pub;
00042 ros::Publisher steering_pid_rpt_3_pub;
00043 ros::Publisher steering_pid_rpt_4_pub;
00044 ros::Publisher lat_lon_heading_rpt_pub;
00045 ros::Publisher date_time_rpt_pub;
00046 ros::Publisher parking_brake_rpt_pub;
00047 ros::Publisher yaw_rate_rpt_pub;
00048 ros::Publisher steering_rpt_detail_1_pub;
00049 ros::Publisher steering_rpt_detail_2_pub;
00050 ros::Publisher steering_rpt_detail_3_pub;
00051 ros::Publisher brake_rpt_detail_1_pub;
00052 ros::Publisher brake_rpt_detail_2_pub;
00053 ros::Publisher brake_rpt_detail_3_pub;
00054 ros::Publisher detected_object_rpt_pub;
00055 ros::Publisher vehicle_specific_rpt_1_pub;
00056 ros::Publisher vehicle_dynamics_rpt_pub;
00057 ros::Publisher occupancy_rpt_pub;
00058 ros::Publisher interior_lights_rpt_pub;
00059 ros::Publisher door_rpt_pub;
00060 ros::Publisher rear_lights_rpt_pub;
00061 
00062 // Advertise published messages
00063 ros::Publisher global_rpt_pub;
00064 ros::Publisher vin_rpt_pub;
00065 ros::Publisher turn_rpt_pub;
00066 ros::Publisher shift_rpt_pub;
00067 ros::Publisher accel_rpt_pub;
00068 ros::Publisher steer_rpt_pub;
00069 ros::Publisher brake_rpt_pub;
00070 ros::Publisher vehicle_speed_pub;
00071 ros::Publisher vehicle_speed_ms_pub;
00072 ros::Publisher enabled_pub;
00073 ros::Publisher can_rx_pub;
00074 ros::Publisher accel_aux_rpt_pub;
00075 ros::Publisher brake_aux_rpt_pub;
00076 ros::Publisher shift_aux_rpt_pub;
00077 ros::Publisher steer_aux_rpt_pub;
00078 ros::Publisher turn_aux_rpt_pub;
00079 ros::Publisher all_system_statuses_pub;
00080 
00081 std::unordered_map<long long, std::shared_ptr<LockedData>> rx_list;
00082 std::map<long long, std::tuple<bool, bool, bool>> system_statuses;
00083 
00084 bool global_keep_going = true;
00085 std::mutex keep_going_mut;
00086 
00087 // Sets the PACMod3 enable flag through CAN.
00088 void set_enable(bool val)
00089 {
00090   for (auto rx_it = rx_list.begin(); rx_it != rx_list.end(); rx_it++)
00091   {
00092     // This assumes that all data in rx_list are encoded
00093     // command messages which means the least significant
00094     // bit in their first byte will be the enable flag.
00095     std::vector<uint8_t> current_data = rx_it->second->getData();
00096 
00097     if (val)
00098       current_data[0] |= 0x01; // Enable true
00099     else
00100       current_data[0] &= 0xFE; // Enable false
00101 
00102     rx_it->second->setData(current_data);
00103   }
00104 }
00105 
00106 // Looks up the appropriate LockedData and inserts the command info
00107 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdBool::ConstPtr& msg)
00108 {
00109   auto rx_it = rx_list.find(can_id);
00110 
00111   if (rx_it != rx_list.end())
00112     rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
00113   else
00114     ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
00115 }
00116 
00117 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00118 {
00119   auto rx_it = rx_list.find(can_id);
00120 
00121   if (rx_it != rx_list.end())
00122     rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
00123   else
00124     ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
00125 }
00126 
00127 void lookup_and_encode(const int64_t& can_id, const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
00128 {
00129   auto rx_it = rx_list.find(can_id);
00130 
00131   if (rx_it != rx_list.end())
00132     rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
00133   else
00134     ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
00135 }
00136 
00137 // Listens for incoming requests to change the position of the throttle pedal
00138 void callback_accel_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
00139 {
00140   lookup_and_encode(AccelCmdMsg::CAN_ID, msg);
00141 }
00142 
00143 // Listens for incoming requests to change the position of the brake pedal
00144 void callback_brake_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
00145 {
00146   lookup_and_encode(BrakeCmdMsg::CAN_ID, msg);
00147 }
00148 
00149 void callback_cruise_control_buttons_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00150 {
00151   lookup_and_encode(CruiseControlButtonsCmdMsg::CAN_ID, msg);
00152 }
00153 
00154 void callback_dash_controls_left_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00155 {
00156   lookup_and_encode(DashControlsLeftCmdMsg::CAN_ID, msg);
00157 }
00158 
00159 void callback_dash_controls_right_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00160 {
00161   lookup_and_encode(DashControlsRightCmdMsg::CAN_ID, msg);
00162 }
00163 
00164 //Listens for incoming requests to change the state of the headlights
00165 void callback_headlight_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00166 {
00167   lookup_and_encode(HeadlightCmdMsg::CAN_ID, msg);
00168 }
00169 
00170 //Listens for incoming requests to change the state of the horn
00171 void callback_horn_set_cmd(const pacmod_msgs::SystemCmdBool::ConstPtr& msg)
00172 {
00173   lookup_and_encode(HornCmdMsg::CAN_ID, msg);
00174 }
00175 
00176 void callback_media_controls_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00177 {
00178   lookup_and_encode(MediaControlsCmdMsg::CAN_ID, msg);
00179 }
00180 
00181 // Listens for incoming requests to change the gear shifter state
00182 void callback_shift_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00183 {
00184   lookup_and_encode(ShiftCmdMsg::CAN_ID, msg);
00185 }
00186 
00187 // Listens for incoming requests to change the position of the steering wheel with a speed limit
00188 void callback_steer_cmd_sub(const pacmod_msgs::SteerSystemCmd::ConstPtr& msg)
00189 {
00190   int64_t can_id = SteerCmdMsg::CAN_ID;
00191   auto rx_it = rx_list.find(can_id);
00192 
00193   if (rx_it != rx_list.end())
00194     rx_it->second->setData(Pacmod3RxRosMsgHandler::unpackAndEncode(can_id, msg));
00195   else
00196     ROS_WARN("Received command message for ID 0x%lx for which we did not have an encoder.", can_id);
00197 }
00198 
00199 // Listens for incoming requests to change the state of the turn signals
00200 void callback_turn_signal_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00201 {
00202   lookup_and_encode(TurnSignalCmdMsg::CAN_ID, msg);
00203 }
00204 
00205 // Listens for incoming requests to change the state of the windshield wipers
00206 void callback_wiper_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00207 {
00208   lookup_and_encode(WiperCmdMsg::CAN_ID, msg);
00209 }
00210 
00211 void send_can(long id, const std::vector<unsigned char>& vec)
00212 {
00213   can_msgs::Frame frame;
00214   frame.id = id;
00215   frame.is_rtr = false;
00216   frame.is_extended = false;
00217   frame.is_error = false;
00218   frame.dlc = 8;
00219   std::copy(vec.begin(), vec.end(), frame.data.begin());
00220 
00221   frame.header.stamp = ros::Time::now();
00222 
00223   can_rx_pub.publish(frame);
00224 }
00225 
00226 void can_write()
00227 {
00228   std::vector<unsigned char> data;
00229 
00230   const std::chrono::milliseconds loop_pause = std::chrono::milliseconds(33);
00231   const std::chrono::milliseconds inter_msg_pause = std::chrono::milliseconds(1);
00232   bool keep_going = true;
00233 
00234   //Set local to global value before looping.
00235   keep_going_mut.lock();
00236   keep_going = global_keep_going;
00237   keep_going_mut.unlock();
00238 
00239   while (keep_going)
00240   {
00241     // Write all the data that we have received.
00242     for (const auto& element : rx_list)
00243     {
00244       send_can(element.first, element.second->getData());
00245       std::this_thread::sleep_for(inter_msg_pause);
00246     }
00247 
00248     std::chrono::system_clock::time_point next_time = std::chrono::system_clock::now();
00249     next_time += loop_pause;
00250     std::this_thread::sleep_until(next_time);
00251 
00252     //Set local to global immediately before next loop.
00253     keep_going_mut.lock();
00254     keep_going = global_keep_going;
00255     keep_going_mut.unlock();
00256   }
00257 }
00258 
00259 void can_read(const can_msgs::Frame::ConstPtr &msg)
00260 {
00261   auto parser_class = Pacmod3TxMsg::make_message(msg->id);
00262   auto pub = pub_tx_list.find(msg->id);
00263 
00264   // Only parse messages for which we have a parser and a publisher.
00265   if (parser_class != NULL && pub != pub_tx_list.end())
00266   {
00267     parser_class->parse(const_cast<unsigned char *>(&msg->data[0]));
00268     handler.fillAndPublish(msg->id, "pacmod", pub->second, parser_class);
00269 
00270     if (parser_class->isSystem())
00271     {
00272       auto dc_parser = std::dynamic_pointer_cast<SystemRptMsg>(parser_class);
00273 
00274       system_statuses[msg->id] = std::make_tuple(dc_parser->enabled,
00275                                                  dc_parser->override_active,
00276                                                  (dc_parser->command_output_fault |
00277                                                   dc_parser->input_output_fault |
00278                                                   dc_parser->output_reported_fault |
00279                                                   dc_parser->pacmod_fault |
00280                                                   dc_parser->vehicle_fault));
00281     }
00282 
00283     if (msg->id == GlobalRptMsg::CAN_ID)
00284     {
00285       auto dc_parser = std::dynamic_pointer_cast<GlobalRptMsg>(parser_class);
00286 
00287       std_msgs::Bool bool_pub_msg;
00288       bool_pub_msg.data = (dc_parser->enabled);
00289       enabled_pub.publish(bool_pub_msg);
00290 
00291       if (dc_parser->override_active ||
00292           dc_parser->fault_active)
00293         set_enable(false);
00294     }
00295     else if (msg->id == VehicleSpeedRptMsg::CAN_ID)
00296     {
00297       auto dc_parser = std::dynamic_pointer_cast<VehicleSpeedRptMsg>(parser_class);
00298 
00299       // Now publish by itself
00300       std_msgs::Float64 veh_spd_ms_msg;
00301       veh_spd_ms_msg.data = (dc_parser->vehicle_speed);
00302       vehicle_speed_ms_pub.publish(veh_spd_ms_msg);
00303     }
00304   }
00305 }
00306 
00307 int main(int argc, char *argv[])
00308 { 
00309   ros::init(argc, argv, "pacmod3");
00310   ros::AsyncSpinner spinner(2);
00311   ros::NodeHandle n;
00312   ros::NodeHandle priv("~");
00313   ros::Rate loop_rate(30); //PACMod3 is sending at ~30Hz.
00314 
00315   //Vehicle-Specific Subscribers
00316   std::shared_ptr<ros::Subscriber> wiper_set_cmd_sub,
00317                                    headlight_set_cmd_sub,
00318                                    horn_set_cmd_sub,
00319                                    cruise_control_buttons_set_cmd_sub,
00320                                    dash_controls_left_set_cmd_sub,
00321                                    dash_controls_right_set_cmd_sub,
00322                                    media_controls_set_cmd_sub;
00323 
00324   // Wait for time to be valid
00325   while (ros::Time::now().nsec == 0);
00326 
00327   // Get and validate parameters    
00328   if (priv.getParam("vehicle_type", veh_type_string))
00329   {
00330     ROS_INFO("PACMod3 - Got vehicle type of: %s", veh_type_string.c_str());
00331 
00332     if (veh_type_string == "POLARIS_GEM")
00333       veh_type = POLARIS_GEM;
00334     else if (veh_type_string == "POLARIS_RANGER")
00335       veh_type = POLARIS_RANGER;
00336     else if (veh_type_string == "INTERNATIONAL_PROSTAR_122")
00337       veh_type = INTERNATIONAL_PROSTAR_122;
00338     else if (veh_type_string == "LEXUS_RX_450H")
00339       veh_type = LEXUS_RX_450H;
00340     else if (veh_type_string == "VEHICLE_4")
00341       veh_type = VEHICLE_4;
00342     else if (veh_type_string == "VEHICLE_5")
00343       veh_type = VEHICLE_5;
00344     else
00345     {
00346       veh_type = VehicleType::POLARIS_GEM;
00347       ROS_WARN("PACMod3 - An invalid vehicle type was entered. Assuming POLARIS_GEM.");
00348     }
00349   }
00350 
00351   // Advertise published messages
00352   can_rx_pub = n.advertise<can_msgs::Frame>("can_rx", 20);
00353   global_rpt_pub = n.advertise<pacmod_msgs::GlobalRpt>("parsed_tx/global_rpt", 20);
00354   accel_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/accel_rpt", 20);
00355   brake_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/brake_rpt", 20);
00356   shift_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/shift_rpt", 20);
00357   steer_rpt_pub = n.advertise<pacmod_msgs::SystemRptFloat>("parsed_tx/steer_rpt", 20);
00358   turn_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/turn_rpt", 20);
00359   vehicle_speed_pub = n.advertise<pacmod_msgs::VehicleSpeedRpt>("parsed_tx/vehicle_speed_rpt", 20);
00360   vin_rpt_pub = n.advertise<pacmod_msgs::VinRpt>("parsed_tx/vin_rpt", 5);
00361   accel_aux_rpt_pub = n.advertise<pacmod_msgs::AccelAuxRpt>("parsed_tx/accel_aux_rpt", 20);
00362   brake_aux_rpt_pub = n.advertise<pacmod_msgs::BrakeAuxRpt>("parsed_tx/brake_aux_rpt", 20);
00363   shift_aux_rpt_pub = n.advertise<pacmod_msgs::ShiftAuxRpt>("parsed_tx/shift_aux_rpt", 20);
00364   steer_aux_rpt_pub = n.advertise<pacmod_msgs::SteerAuxRpt>("parsed_tx/steer_aux_rpt", 20);
00365   turn_aux_rpt_pub = n.advertise<pacmod_msgs::TurnAuxRpt>("parsed_tx/turn_aux_rpt", 20);
00366 
00367   enabled_pub = n.advertise<std_msgs::Bool>("as_tx/enabled", 20, true);
00368   vehicle_speed_ms_pub = n.advertise<std_msgs::Float64>("as_tx/vehicle_speed", 20);
00369   all_system_statuses_pub = n.advertise<pacmod_msgs::AllSystemStatuses>("as_tx/all_system_statuses", 20);
00370 
00371   std::string frame_id = "pacmod";
00372 
00373   //Populate handler list
00374   pub_tx_list.insert(std::make_pair(GlobalRptMsg::CAN_ID, global_rpt_pub));
00375   pub_tx_list.insert(std::make_pair(AccelRptMsg::CAN_ID, accel_rpt_pub));
00376   pub_tx_list.insert(std::make_pair(BrakeRptMsg::CAN_ID, brake_rpt_pub));
00377   pub_tx_list.insert(std::make_pair(ShiftRptMsg::CAN_ID, shift_rpt_pub));
00378   pub_tx_list.insert(std::make_pair(SteerRptMsg::CAN_ID, steer_rpt_pub));
00379   pub_tx_list.insert(std::make_pair(TurnSignalRptMsg::CAN_ID, turn_rpt_pub));
00380   pub_tx_list.insert(std::make_pair(VehicleSpeedRptMsg::CAN_ID, vehicle_speed_pub));
00381   pub_tx_list.insert(std::make_pair(VinRptMsg::CAN_ID, vin_rpt_pub));
00382   pub_tx_list.insert(std::make_pair(AccelAuxRptMsg::CAN_ID, accel_aux_rpt_pub));
00383   pub_tx_list.insert(std::make_pair(BrakeAuxRptMsg::CAN_ID, brake_aux_rpt_pub));
00384   pub_tx_list.insert(std::make_pair(ShiftAuxRptMsg::CAN_ID, shift_aux_rpt_pub));
00385   pub_tx_list.insert(std::make_pair(SteerAuxRptMsg::CAN_ID, steer_aux_rpt_pub));
00386   pub_tx_list.insert(std::make_pair(TurnAuxRptMsg::CAN_ID, turn_aux_rpt_pub));
00387 
00388   // Subscribe to messages
00389   ros::Subscriber can_tx_sub = n.subscribe("can_tx", 20, can_read);
00390 
00391   ros::Subscriber accel_cmd_sub = n.subscribe("as_rx/accel_cmd", 20, callback_accel_cmd_sub);
00392   ros::Subscriber brake_cmd_sub = n.subscribe("as_rx/brake_cmd", 20, callback_brake_cmd_sub);
00393   ros::Subscriber shift_cmd_sub = n.subscribe("as_rx/shift_cmd", 20, callback_shift_set_cmd);  
00394   ros::Subscriber steer_cmd_sub = n.subscribe("as_rx/steer_cmd", 20, callback_steer_cmd_sub);
00395   ros::Subscriber turn_cmd_sub = n.subscribe("as_rx/turn_cmd", 20, callback_turn_signal_set_cmd);  
00396 
00397   // Populate rx list
00398   std::shared_ptr<LockedData> accel_data(new LockedData);
00399   std::shared_ptr<LockedData> brake_data(new LockedData);
00400   std::shared_ptr<LockedData> shift_data(new LockedData);
00401   std::shared_ptr<LockedData> steer_data(new LockedData);
00402   std::shared_ptr<LockedData> turn_data(new LockedData);
00403 
00404   rx_list.insert(std::make_pair(AccelCmdMsg::CAN_ID, accel_data));
00405   rx_list.insert(std::make_pair(BrakeCmdMsg::CAN_ID, brake_data));
00406   rx_list.insert(std::make_pair(ShiftCmdMsg::CAN_ID, shift_data));
00407   rx_list.insert(std::make_pair(SteerCmdMsg::CAN_ID, steer_data));
00408   rx_list.insert(std::make_pair(TurnSignalCmdMsg::CAN_ID, turn_data));
00409 
00410   if (veh_type == VehicleType::POLARIS_GEM ||
00411       veh_type == VehicleType::POLARIS_RANGER ||
00412       veh_type == VehicleType::INTERNATIONAL_PROSTAR_122)
00413   {
00414     brake_rpt_detail_1_pub = n.advertise<pacmod_msgs::MotorRpt1>("parsed_tx/brake_rpt_detail_1", 20);
00415     brake_rpt_detail_2_pub = n.advertise<pacmod_msgs::MotorRpt2>("parsed_tx/brake_rpt_detail_2", 20);
00416     brake_rpt_detail_3_pub = n.advertise<pacmod_msgs::MotorRpt3>("parsed_tx/brake_rpt_detail_3", 20);
00417     steering_rpt_detail_1_pub = n.advertise<pacmod_msgs::MotorRpt1>("parsed_tx/steer_rpt_detail_1", 20);
00418     steering_rpt_detail_2_pub = n.advertise<pacmod_msgs::MotorRpt2>("parsed_tx/steer_rpt_detail_2", 20);
00419     steering_rpt_detail_3_pub = n.advertise<pacmod_msgs::MotorRpt3>("parsed_tx/steer_rpt_detail_3", 20);
00420 
00421     pub_tx_list.insert(std::make_pair(BrakeMotorRpt1Msg::CAN_ID, brake_rpt_detail_1_pub));
00422     pub_tx_list.insert(std::make_pair(BrakeMotorRpt2Msg::CAN_ID, brake_rpt_detail_2_pub));
00423     pub_tx_list.insert(std::make_pair(BrakeMotorRpt3Msg::CAN_ID, brake_rpt_detail_3_pub));
00424     pub_tx_list.insert(std::make_pair(SteerMotorRpt1Msg::CAN_ID, steering_rpt_detail_1_pub));
00425     pub_tx_list.insert(std::make_pair(SteerMotorRpt2Msg::CAN_ID, steering_rpt_detail_2_pub));
00426     pub_tx_list.insert(std::make_pair(SteerMotorRpt3Msg::CAN_ID, steering_rpt_detail_3_pub));
00427   }
00428 
00429   if (veh_type == VehicleType::INTERNATIONAL_PROSTAR_122)
00430   {
00431     wiper_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/wiper_rpt", 20);
00432     wiper_aux_rpt_pub = n.advertise<pacmod_msgs::WiperAuxRpt>("parsed_tx/wiper_aux_rpt", 20);
00433 
00434     pub_tx_list.insert(std::make_pair(WiperRptMsg::CAN_ID, wiper_rpt_pub));
00435     pub_tx_list.insert(std::make_pair(WiperAuxRptMsg::CAN_ID, wiper_aux_rpt_pub));
00436 
00437     wiper_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/wiper_cmd", 20, callback_wiper_set_cmd)));
00438 
00439     std::shared_ptr<LockedData> wiper_data(new LockedData);
00440     rx_list.insert(std::make_pair(WiperCmdMsg::CAN_ID, wiper_data));
00441   }
00442 
00443   if (veh_type == VehicleType::LEXUS_RX_450H ||
00444       veh_type == VehicleType::VEHICLE_5)
00445   {
00446     date_time_rpt_pub = n.advertise<pacmod_msgs::DateTimeRpt>("parsed_tx/date_time_rpt", 20);
00447     headlight_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/headlight_rpt", 20);
00448     horn_rpt_pub = n.advertise<pacmod_msgs::SystemRptBool>("parsed_tx/horn_rpt", 20);
00449     lat_lon_heading_rpt_pub = n.advertise<pacmod_msgs::LatLonHeadingRpt>("parsed_tx/lat_lon_heading_rpt", 20);
00450     parking_brake_rpt_pub = n.advertise<pacmod_msgs::SystemRptBool>("parsed_tx/parking_brake_status_rpt", 20);
00451     wheel_speed_rpt_pub = n.advertise<pacmod_msgs::WheelSpeedRpt>("parsed_tx/wheel_speed_rpt", 20);
00452     yaw_rate_rpt_pub = n.advertise<pacmod_msgs::YawRateRpt>("parsed_tx/yaw_rate_rpt", 20);
00453     headlight_aux_rpt_pub = n.advertise<pacmod_msgs::HeadlightAuxRpt>("parsed_tx/headlight_aux_rpt", 20);
00454 
00455     pub_tx_list.insert(std::make_pair(DateTimeRptMsg::CAN_ID, date_time_rpt_pub));
00456     pub_tx_list.insert(std::make_pair(HeadlightRptMsg::CAN_ID, headlight_rpt_pub));
00457     pub_tx_list.insert(std::make_pair(HornRptMsg::CAN_ID, horn_rpt_pub));
00458     pub_tx_list.insert(std::make_pair(LatLonHeadingRptMsg::CAN_ID, lat_lon_heading_rpt_pub));
00459     pub_tx_list.insert(std::make_pair(ParkingBrakeRptMsg::CAN_ID, parking_brake_rpt_pub));
00460     pub_tx_list.insert(std::make_pair(WheelSpeedRptMsg::CAN_ID, wheel_speed_rpt_pub));
00461     pub_tx_list.insert(std::make_pair(YawRateRptMsg::CAN_ID, yaw_rate_rpt_pub));
00462     pub_tx_list.insert(std::make_pair(HeadlightAuxRptMsg::CAN_ID, headlight_aux_rpt_pub));
00463 
00464     headlight_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/headlight_cmd", 20, callback_headlight_set_cmd)));
00465     horn_set_cmd_sub = std::shared_ptr<ros::Subscriber>(new ros::Subscriber(n.subscribe("as_rx/horn_cmd", 20, callback_horn_set_cmd)));
00466 
00467     std::shared_ptr<LockedData> headlight_data(new LockedData);
00468     std::shared_ptr<LockedData> horn_data(new LockedData);
00469 
00470     rx_list.insert(std::make_pair(HeadlightCmdMsg::CAN_ID, headlight_data));
00471     rx_list.insert(std::make_pair(HornCmdMsg::CAN_ID, horn_data));
00472   }
00473 
00474   if (veh_type == VehicleType::VEHICLE_4)
00475   {
00476     detected_object_rpt_pub = n.advertise<pacmod_msgs::DetectedObjectRpt>("parsed_tx/detected_object_rpt", 20);
00477     vehicle_dynamics_rpt_pub = n.advertise<pacmod_msgs::VehicleDynamicsRpt>("parsed_tx/vehicle_dynamics_rpt", 20);
00478 
00479     pub_tx_list.insert(std::make_pair(DetectedObjectRptMsg::CAN_ID, detected_object_rpt_pub));
00480     pub_tx_list.insert(std::make_pair(VehicleDynamicsRptMsg::CAN_ID, vehicle_dynamics_rpt_pub));
00481   }
00482 
00483   if (veh_type == VehicleType::LEXUS_RX_450H)
00484   {
00485     steering_pid_rpt_1_pub = n.advertise<pacmod_msgs::SteeringPIDRpt1>("parsed_tx/steer_pid_rpt_1", 20);
00486     steering_pid_rpt_2_pub = n.advertise<pacmod_msgs::SteeringPIDRpt2>("parsed_tx/steer_pid_rpt_2", 20);
00487     steering_pid_rpt_3_pub = n.advertise<pacmod_msgs::SteeringPIDRpt3>("parsed_tx/steer_pid_rpt_3", 20);
00488     steering_pid_rpt_4_pub = n.advertise<pacmod_msgs::SteeringPIDRpt4>("parsed_tx/steer_pid_rpt_4", 20);
00489 
00490     pub_tx_list.insert(std::make_pair(SteeringPIDRpt1Msg::CAN_ID, steering_pid_rpt_1_pub));
00491     pub_tx_list.insert(std::make_pair(SteeringPIDRpt2Msg::CAN_ID, steering_pid_rpt_2_pub));
00492     pub_tx_list.insert(std::make_pair(SteeringPIDRpt3Msg::CAN_ID, steering_pid_rpt_3_pub));
00493     pub_tx_list.insert(std::make_pair(SteeringPIDRpt4Msg::CAN_ID, steering_pid_rpt_4_pub));
00494   }
00495 
00496   if (veh_type == VehicleType::VEHICLE_5)
00497   {
00498     // cruise_control_buttons_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/cruise_control_buttons_rpt", 20);
00499     // dash_controls_left_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/dash_controls_left_rpt", 20);
00500     // dash_controls_right_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/dash_controls_right_rpt", 20);
00501     // media_controls_rpt_pub = n.advertise<pacmod_msgs::SystemRptInt>("parsed_tx/media_controls_rpt", 20);
00502     occupancy_rpt_pub = n.advertise<pacmod_msgs::OccupancyRpt>("parsed_tx/occupancy_rpt", 20);
00503     interior_lights_rpt_pub = n.advertise<pacmod_msgs::InteriorLightsRpt>("parsed_tx/interior_lights_rpt", 20);
00504     door_rpt_pub = n.advertise<pacmod_msgs::DoorRpt>("parsed_tx/door_rpt", 20);
00505     rear_lights_rpt_pub = n.advertise<pacmod_msgs::RearLightsRpt>("parsed_tx/rear_lights_rpt", 20);
00506 
00507     // pub_tx_list.insert(std::make_pair(CruiseControlButtonsRptMsg::CAN_ID, cruise_control_buttons_rpt_pub));
00508     // pub_tx_list.insert(std::make_pair(DashControlsLeftRptMsg::CAN_ID, dash_controls_left_rpt_pub));
00509     // pub_tx_list.insert(std::make_pair(DashControlsRightRptMsg::CAN_ID, dash_controls_right_rpt_pub));
00510     // pub_tx_list.insert(std::make_pair(MediaControlsRptMsg::CAN_ID, media_controls_rpt_pub));
00511     pub_tx_list.insert(std::make_pair(OccupancyRptMsg::CAN_ID, occupancy_rpt_pub));
00512     pub_tx_list.insert(std::make_pair(InteriorLightsRptMsg::CAN_ID, interior_lights_rpt_pub));
00513     pub_tx_list.insert(std::make_pair(DoorRptMsg::CAN_ID, door_rpt_pub));
00514     pub_tx_list.insert(std::make_pair(RearLightsRptMsg::CAN_ID, rear_lights_rpt_pub));
00515 
00516     // 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)));
00517     // 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)));
00518     // 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)));
00519     // 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)));
00520 
00521     // std::shared_ptr<LockedData> cruise_control_buttons_data(new LockedData);
00522     // std::shared_ptr<LockedData> dash_controls_left_data(new LockedData);
00523     // std::shared_ptr<LockedData> dash_controls_right_data(new LockedData);
00524     // std::shared_ptr<LockedData> media_controls_data(new LockedData);
00525 
00526     // rx_list.insert(std::make_pair(CruiseControlButtonsCmdMsg::CAN_ID, cruise_control_buttons_data));
00527     // rx_list.insert(std::make_pair(DashControlsLeftCmdMsg::CAN_ID, dash_controls_left_data));
00528     // rx_list.insert(std::make_pair(DashControlsRightCmdMsg::CAN_ID, dash_controls_right_data));
00529     // rx_list.insert(std::make_pair(MediaControlsCmdMsg::CAN_ID, media_controls_data));
00530   }
00531 
00532   // Initialize rx_list with all 0s
00533   for (auto rx_it = rx_list.begin(); rx_it != rx_list.end(); rx_it++)
00534   {
00535     if (rx_it->first == TurnSignalCmdMsg::CAN_ID)
00536     {
00537       // Turn signals have non-0 initial value.
00538       TurnSignalCmdMsg turn_encoder;
00539       turn_encoder.encode(false, false, false, pacmod_msgs::SystemCmdInt::TURN_NONE);
00540       rx_it->second->setData(turn_encoder.data);
00541     }
00542     else
00543     {
00544       std::vector<uint8_t> empty_vec;
00545       empty_vec.assign(8, 0);
00546       rx_it->second->setData(empty_vec);
00547     }
00548   }
00549 
00550   // Set initial state
00551   set_enable(false);
00552     
00553   // Start CAN sending thread.
00554   std::thread can_write_thread(can_write);
00555   // Start callback spinner.
00556   spinner.start();
00557 
00558   while (ros::ok())
00559   {
00560     pacmod_msgs::AllSystemStatuses ss_msg;
00561 
00562     for (auto system = system_statuses.begin(); system != system_statuses.end(); ++system)
00563     {
00564       pacmod_msgs::KeyValuePair kvp;
00565 
00566       if (system->first == AccelRptMsg::CAN_ID)
00567         kvp.key = "Accelerator";
00568       else if (system->first == BrakeRptMsg::CAN_ID)
00569         kvp.key = "Brakes";
00570       else if (system->first == CruiseControlButtonsRptMsg::CAN_ID)
00571         kvp.key = "Cruise Control Buttons";
00572       else if (system->first == DashControlsLeftRptMsg::CAN_ID)
00573         kvp.key = "Dash Controls Left";
00574       else if (system->first == DashControlsRightRptMsg::CAN_ID)
00575         kvp.key = "Dash Controls Right";
00576       else if (system->first == HazardLightRptMsg::CAN_ID)
00577         kvp.key = "Hazard Lights";
00578       else if (system->first == HeadlightRptMsg::CAN_ID)
00579         kvp.key = "Headlights";
00580       else if (system->first == HornRptMsg::CAN_ID)
00581         kvp.key = "Horn";
00582       else if (system->first == MediaControlsRptMsg::CAN_ID)
00583         kvp.key = "Media Controls";
00584       else if (system->first == ParkingBrakeRptMsg::CAN_ID)
00585         kvp.key = "Parking Brake";
00586       else if (system->first == ShiftRptMsg::CAN_ID)
00587         kvp.key = "Shifter";
00588       else if (system->first == SteerRptMsg::CAN_ID)
00589         kvp.key = "Steering";
00590       else if (system->first == TurnSignalRptMsg::CAN_ID)
00591         kvp.key = "Turn Signals";
00592       else if (system->first == WiperRptMsg::CAN_ID)
00593         kvp.key = "Wipers";
00594 
00595       kvp.value = std::get<0>(system->second) ? "True" : "False";
00596 
00597       ss_msg.enabled_status.push_back(kvp);
00598 
00599       kvp.value = std::get<1>(system->second) ? "True" : "False";
00600 
00601       ss_msg.overridden_status.push_back(kvp);
00602 
00603       kvp.value = std::get<2>(system->second) ? "True" : "False";
00604 
00605       ss_msg.fault_status.push_back(kvp);
00606     }
00607 
00608     all_system_statuses_pub.publish(ss_msg);
00609 
00610     loop_rate.sleep();
00611   }
00612 
00613   // Make sure it's disabled when node shuts down
00614   set_enable(false);
00615 
00616   keep_going_mut.lock();
00617   global_keep_going = false;
00618   keep_going_mut.unlock();
00619 
00620   can_write_thread.join();
00621 
00622   return 0;
00623 }
00624 


pacmod3
Author(s): Joe Driscoll , Josh Whitley
autogenerated on Thu Jun 6 2019 17:34:14