00001
00002
00003
00004
00005
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
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
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
00088 void set_enable(bool val)
00089 {
00090 for (auto rx_it = rx_list.begin(); rx_it != rx_list.end(); rx_it++)
00091 {
00092
00093
00094
00095 std::vector<uint8_t> current_data = rx_it->second->getData();
00096
00097 if (val)
00098 current_data[0] |= 0x01;
00099 else
00100 current_data[0] &= 0xFE;
00101
00102 rx_it->second->setData(current_data);
00103 }
00104 }
00105
00106
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
00138 void callback_accel_cmd_sub(const pacmod_msgs::SystemCmdFloat::ConstPtr& msg)
00139 {
00140 lookup_and_encode(AccelCmdMsg::CAN_ID, msg);
00141 }
00142
00143
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
00165 void callback_headlight_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00166 {
00167 lookup_and_encode(HeadlightCmdMsg::CAN_ID, msg);
00168 }
00169
00170
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
00182 void callback_shift_set_cmd(const pacmod_msgs::SystemCmdInt::ConstPtr& msg)
00183 {
00184 lookup_and_encode(ShiftCmdMsg::CAN_ID, msg);
00185 }
00186
00187
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
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
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
00235 keep_going_mut.lock();
00236 keep_going = global_keep_going;
00237 keep_going_mut.unlock();
00238
00239 while (keep_going)
00240 {
00241
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
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
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
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);
00314
00315
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
00325 while (ros::Time::now().nsec == 0);
00326
00327
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
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
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
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
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
00499
00500
00501
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
00508
00509
00510
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
00517
00518
00519
00520
00521
00522
00523
00524
00525
00526
00527
00528
00529
00530 }
00531
00532
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
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
00551 set_enable(false);
00552
00553
00554 std::thread can_write_thread(can_write);
00555
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
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