00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "DbwNode.h"
00036 #include <dbw_mkz_can/dispatch.h>
00037 #include <dbw_mkz_can/pedal_lut.h>
00038 #include <dbw_mkz_can/sonar_lut.h>
00039
00040 namespace dbw_mkz_can
00041 {
00042
00043
00044 PlatformMap FIRMWARE_LATEST({
00045 {PlatformVersion(P_FORD_CD4, M_BPEC, ModuleVersion(2,1,2))},
00046 {PlatformVersion(P_FORD_CD4, M_TPEC, ModuleVersion(2,1,2))},
00047 {PlatformVersion(P_FORD_CD4, M_STEER, ModuleVersion(2,1,2))},
00048 {PlatformVersion(P_FORD_CD4, M_SHIFT, ModuleVersion(2,1,2))},
00049 {PlatformVersion(P_FORD_P5, M_TPEC, ModuleVersion(1,0,2))},
00050 {PlatformVersion(P_FORD_P5, M_STEER, ModuleVersion(1,0,2))},
00051 {PlatformVersion(P_FORD_P5, M_SHIFT, ModuleVersion(1,0,2))},
00052 {PlatformVersion(P_FORD_P5, M_ABS, ModuleVersion(1,0,2))},
00053 {PlatformVersion(P_FORD_P5, M_BOO, ModuleVersion(1,0,2))},
00054 {PlatformVersion(P_FORD_C1, M_TPEC, ModuleVersion(0,0,1))},
00055 {PlatformVersion(P_FORD_C1, M_STEER, ModuleVersion(0,0,1))},
00056 {PlatformVersion(P_FORD_C1, M_SHIFT, ModuleVersion(0,0,1))},
00057 {PlatformVersion(P_FORD_C1, M_ABS, ModuleVersion(0,0,1))},
00058 {PlatformVersion(P_FORD_C1, M_BOO, ModuleVersion(0,0,1))},
00059 {PlatformVersion(P_FORD_C1, M_EPS, ModuleVersion(0,0,1))},
00060 });
00061
00062
00063 PlatformMap FIRMWARE_TIMEOUT({
00064 {PlatformVersion(P_FORD_CD4, M_BPEC, ModuleVersion(2,0,0))},
00065 {PlatformVersion(P_FORD_CD4, M_TPEC, ModuleVersion(2,0,0))},
00066 {PlatformVersion(P_FORD_CD4, M_STEER, ModuleVersion(2,0,0))},
00067 });
00068
00069
00070 PlatformMap FIRMWARE_CMDTYPE({
00071 {PlatformVersion(P_FORD_CD4, M_BPEC, ModuleVersion(2,0,7))},
00072 {PlatformVersion(P_FORD_CD4, M_TPEC, ModuleVersion(2,0,7))},
00073 });
00074
00075
00076 PlatformMap FIRMWARE_HIGH_RATE_LIMIT({
00077 {PlatformVersion(P_FORD_CD4, M_STEER, ModuleVersion(2,2,0))},
00078 {PlatformVersion(P_FORD_P5, M_STEER, ModuleVersion(1,1,0))},
00079 });
00080
00081 DbwNode::DbwNode(ros::NodeHandle &node, ros::NodeHandle &priv_nh)
00082 : sync_imu_(10, boost::bind(&DbwNode::recvCanImu, this, _1), ID_REPORT_ACCEL, ID_REPORT_GYRO)
00083 , sync_gps_(10, boost::bind(&DbwNode::recvCanGps, this, _1), ID_REPORT_GPS1, ID_REPORT_GPS2, ID_REPORT_GPS3)
00084 {
00085
00086 sync_imu_.setInterMessageLowerBound(0, ros::Duration(0.003));
00087 sync_imu_.setInterMessageLowerBound(1, ros::Duration(0.003));
00088 sync_gps_.setInterMessageLowerBound(0, ros::Duration(0.3));
00089 sync_gps_.setInterMessageLowerBound(1, ros::Duration(0.3));
00090 sync_gps_.setInterMessageLowerBound(2, ros::Duration(0.3));
00091
00092
00093 prev_enable_ = true;
00094 enable_ = false;
00095 override_brake_ = false;
00096 override_throttle_ = false;
00097 override_steering_ = false;
00098 override_gear_ = false;
00099 fault_brakes_ = false;
00100 fault_throttle_ = false;
00101 fault_steering_ = false;
00102 fault_steering_cal_ = false;
00103 fault_watchdog_ = false;
00104 fault_watchdog_using_brakes_ = false;
00105 fault_watchdog_warned_ = false;
00106 timeout_brakes_ = false;
00107 timeout_throttle_ = false;
00108 timeout_steering_ = false;
00109 enabled_brakes_ = false;
00110 enabled_throttle_ = false;
00111 enabled_steering_ = false;
00112 gear_warned_ = false;
00113
00114
00115 frame_id_ = "base_footprint";
00116 priv_nh.getParam("frame_id", frame_id_);
00117
00118
00119 warn_cmds_ = true;
00120 priv_nh.getParam("warn_cmds", warn_cmds_);
00121
00122
00123 buttons_ = true;
00124 priv_nh.getParam("buttons", buttons_);
00125
00126
00127 pedal_luts_ = false;
00128 priv_nh.getParam("pedal_luts", pedal_luts_);
00129
00130
00131 acker_wheelbase_ = 2.8498;
00132 acker_track_ = 1.5824;
00133 steering_ratio_ = 14.8;
00134 priv_nh.getParam("ackermann_wheelbase", acker_wheelbase_);
00135 priv_nh.getParam("ackermann_track", acker_track_);
00136 priv_nh.getParam("steering_ratio", steering_ratio_);
00137
00138
00139 joint_state_.position.resize(JOINT_COUNT);
00140 joint_state_.velocity.resize(JOINT_COUNT);
00141 joint_state_.effort.resize(JOINT_COUNT);
00142 joint_state_.name.resize(JOINT_COUNT);
00143 joint_state_.name[JOINT_FL] = "wheel_fl";
00144 joint_state_.name[JOINT_FR] = "wheel_fr";
00145 joint_state_.name[JOINT_RL] = "wheel_rl";
00146 joint_state_.name[JOINT_RR] = "wheel_rr";
00147 joint_state_.name[JOINT_SL] = "steer_fl";
00148 joint_state_.name[JOINT_SR] = "steer_fr";
00149
00150
00151 pub_can_ = node.advertise<can_msgs::Frame>("can_tx", 10);
00152 pub_brake_ = node.advertise<dbw_mkz_msgs::BrakeReport>("brake_report", 2);
00153 pub_throttle_ = node.advertise<dbw_mkz_msgs::ThrottleReport>("throttle_report", 2);
00154 pub_steering_ = node.advertise<dbw_mkz_msgs::SteeringReport>("steering_report", 2);
00155 pub_gear_ = node.advertise<dbw_mkz_msgs::GearReport>("gear_report", 2);
00156 pub_misc_1_ = node.advertise<dbw_mkz_msgs::Misc1Report>("misc_1_report", 2);
00157 pub_wheel_speeds_ = node.advertise<dbw_mkz_msgs::WheelSpeedReport>("wheel_speed_report", 2);
00158 pub_wheel_positions_ = node.advertise<dbw_mkz_msgs::WheelPositionReport>("wheel_position_report", 2);
00159 pub_tire_pressure_ = node.advertise<dbw_mkz_msgs::TirePressureReport>("tire_pressure_report", 2);
00160 pub_fuel_level_ = node.advertise<dbw_mkz_msgs::FuelLevelReport>("fuel_level_report", 2);
00161 pub_surround_ = node.advertise<dbw_mkz_msgs::SurroundReport>("surround_report", 2);
00162 pub_sonar_cloud_ = node.advertise<sensor_msgs::PointCloud2>("sonar_cloud", 2);
00163 pub_brake_info_ = node.advertise<dbw_mkz_msgs::BrakeInfoReport>("brake_info_report", 2);
00164 pub_throttle_info_ = node.advertise<dbw_mkz_msgs::ThrottleInfoReport>("throttle_info_report", 2);
00165 pub_driver_assist_ = node.advertise<dbw_mkz_msgs::DriverAssistReport>("driver_assist_report", 2);
00166 pub_imu_ = node.advertise<sensor_msgs::Imu>("imu/data_raw", 10);
00167 pub_gps_fix_ = node.advertise<sensor_msgs::NavSatFix>("gps/fix", 10);
00168 pub_gps_vel_ = node.advertise<geometry_msgs::TwistStamped>("gps/vel", 10);
00169 pub_gps_time_ = node.advertise<sensor_msgs::TimeReference>("gps/time", 10);
00170 pub_joint_states_ = node.advertise<sensor_msgs::JointState>("joint_states", 10);
00171 pub_twist_ = node.advertise<geometry_msgs::TwistStamped>("twist", 10);
00172 pub_vin_ = node.advertise<std_msgs::String>("vin", 1, true);
00173 pub_sys_enable_ = node.advertise<std_msgs::Bool>("dbw_enabled", 1, true);
00174 publishDbwEnabled();
00175
00176
00177 const ros::TransportHints NODELAY = ros::TransportHints().tcpNoDelay();
00178 sub_enable_ = node.subscribe("enable", 10, &DbwNode::recvEnable, this, NODELAY);
00179 sub_disable_ = node.subscribe("disable", 10, &DbwNode::recvDisable, this, NODELAY);
00180 sub_can_ = node.subscribe("can_rx", 100, &DbwNode::recvCAN, this, NODELAY);
00181 sub_brake_ = node.subscribe("brake_cmd", 1, &DbwNode::recvBrakeCmd, this, NODELAY);
00182 sub_throttle_ = node.subscribe("throttle_cmd", 1, &DbwNode::recvThrottleCmd, this, NODELAY);
00183 sub_steering_ = node.subscribe("steering_cmd", 1, &DbwNode::recvSteeringCmd, this, NODELAY);
00184 sub_gear_ = node.subscribe("gear_cmd", 1, &DbwNode::recvGearCmd, this, NODELAY);
00185 sub_turn_signal_ = node.subscribe("turn_signal_cmd", 1, &DbwNode::recvTurnSignalCmd, this, NODELAY);
00186
00187
00188 timer_ = node.createTimer(ros::Duration(1 / 20.0), &DbwNode::timerCallback, this);
00189 }
00190
00191 DbwNode::~DbwNode()
00192 {
00193 }
00194
00195 void DbwNode::recvEnable(const std_msgs::Empty::ConstPtr& msg)
00196 {
00197 enableSystem();
00198 }
00199
00200 void DbwNode::recvDisable(const std_msgs::Empty::ConstPtr& msg)
00201 {
00202 disableSystem();
00203 }
00204
00205 void DbwNode::recvCAN(const can_msgs::Frame::ConstPtr& msg)
00206 {
00207 sync_imu_.processMsg(msg);
00208 sync_gps_.processMsg(msg);
00209 if (!msg->is_rtr && !msg->is_error && !msg->is_extended) {
00210 switch (msg->id) {
00211 case ID_BRAKE_REPORT:
00212 if (msg->dlc >= sizeof(MsgBrakeReport)) {
00213 const MsgBrakeReport *ptr = (const MsgBrakeReport*)msg->data.elems;
00214 faultBrakes(ptr->FLT1 || ptr->FLT2);
00215 faultWatchdog(ptr->FLTWDC, ptr->WDCSRC, ptr->WDCBRK);
00216 dbw_mkz_msgs::BrakeReport out;
00217 out.header.stamp = msg->header.stamp;
00219 out.pedal_input = (float)ptr->PI / UINT16_MAX;
00220 out.pedal_cmd = (float)ptr->PC / UINT16_MAX;
00221 out.pedal_output = (float)ptr->PO / UINT16_MAX;
00222 out.torque_input = brakeTorqueFromPedal(out.pedal_input);
00223 out.torque_cmd = brakeTorqueFromPedal(out.pedal_cmd);
00224 out.torque_output = brakeTorqueFromPedal(out.pedal_output);
00225 out.boo_input = ptr->BI ? true : false;
00226 out.boo_cmd = ptr->BC ? true : false;
00227 out.boo_output = ptr->BI || ptr->BC;
00228 out.enabled = ptr->ENABLED ? true : false;
00229 out.override = ptr->OVERRIDE ? true : false;
00230 out.driver = ptr->DRIVER ? true : false;
00231 out.watchdog_counter.source = ptr->WDCSRC;
00232 out.watchdog_braking = ptr->WDCBRK ? true : false;
00233 out.fault_wdc = ptr->FLTWDC ? true : false;
00234 out.fault_ch1 = ptr->FLT1 ? true : false;
00235 out.fault_ch2 = ptr->FLT2 ? true : false;
00236 out.fault_power = ptr->FLTPWR ? true : false;
00237 if (firmware_.findPlatform(M_BPEC) >= FIRMWARE_TIMEOUT) {
00238 timeoutBrake(ptr->TMOUT, ptr->ENABLED);
00239 out.timeout = ptr->TMOUT ? true : false;
00240 }
00241 overrideBrake(ptr->OVERRIDE, out.timeout);
00242 pub_brake_.publish(out);
00243 if (ptr->FLT1 || ptr->FLT2 || ptr->FLTPWR) {
00244 ROS_WARN_THROTTLE(5.0, "Brake fault. FLT1: %s FLT2: %s FLTPWR: %s",
00245 ptr->FLT1 ? "true, " : "false,",
00246 ptr->FLT2 ? "true, " : "false,",
00247 ptr->FLTPWR ? "true" : "false");
00248 }
00249 }
00250 break;
00251
00252 case ID_THROTTLE_REPORT:
00253 if (msg->dlc >= sizeof(MsgThrottleReport)) {
00254 const MsgThrottleReport *ptr = (const MsgThrottleReport*)msg->data.elems;
00255 faultThrottle(ptr->FLT1 || ptr->FLT2);
00256 faultWatchdog(ptr->FLTWDC, ptr->WDCSRC);
00257 dbw_mkz_msgs::ThrottleReport out;
00258 out.header.stamp = msg->header.stamp;
00259 out.pedal_input = (float)ptr->PI / UINT16_MAX;
00260 out.pedal_cmd = (float)ptr->PC / UINT16_MAX;
00261 out.pedal_output = (float)ptr->PO / UINT16_MAX;
00262 out.enabled = ptr->ENABLED ? true : false;
00263 out.override = ptr->OVERRIDE ? true : false;
00264 out.driver = ptr->DRIVER ? true : false;
00265 out.watchdog_counter.source = ptr->WDCSRC;
00266 out.fault_wdc = ptr->FLTWDC ? true : false;
00267 out.fault_ch1 = ptr->FLT1 ? true : false;
00268 out.fault_ch2 = ptr->FLT2 ? true : false;
00269 out.fault_power = ptr->FLTPWR ? true : false;
00270 if (firmware_.findPlatform(M_TPEC) >= FIRMWARE_TIMEOUT) {
00271 timeoutThrottle(ptr->TMOUT, ptr->ENABLED);
00272 out.timeout = ptr->TMOUT ? true : false;
00273 }
00274 overrideThrottle(ptr->OVERRIDE, out.timeout);
00275 pub_throttle_.publish(out);
00276 if (ptr->FLT1 || ptr->FLT2 || ptr->FLTPWR) {
00277 ROS_WARN_THROTTLE(5.0, "Throttle fault. FLT1: %s FLT2: %s FLTPWR: %s",
00278 ptr->FLT1 ? "true, " : "false,",
00279 ptr->FLT2 ? "true, " : "false,",
00280 ptr->FLTPWR ? "true" : "false");
00281 }
00282 }
00283 break;
00284
00285 case ID_STEERING_REPORT:
00286 if (msg->dlc >= sizeof(MsgSteeringReport)) {
00287 const MsgSteeringReport *ptr = (const MsgSteeringReport*)msg->data.elems;
00288 faultSteering(ptr->FLTBUS1 || ptr->FLTBUS2);
00289 faultSteeringCal(ptr->FLTCAL);
00290 faultWatchdog(ptr->FLTWDC);
00291 dbw_mkz_msgs::SteeringReport out;
00292 out.header.stamp = msg->header.stamp;
00293 out.steering_wheel_angle = (float)ptr->ANGLE * (float)(0.1 * M_PI / 180);
00294 out.steering_wheel_cmd_type = ptr->TMODE ? dbw_mkz_msgs::SteeringReport::CMD_TORQUE : dbw_mkz_msgs::SteeringReport::CMD_ANGLE;
00295 if (out.steering_wheel_cmd_type == dbw_mkz_msgs::SteeringReport::CMD_ANGLE) {
00296 out.steering_wheel_cmd = (float)ptr->CMD * (float)(0.1 * M_PI / 180);
00297 } else {
00298 out.steering_wheel_cmd = (float)ptr->CMD / 128.0f;
00299 }
00300 out.steering_wheel_torque = (float)ptr->TORQUE * (float)0.0625;
00301 out.speed = (float)ptr->SPEED * (float)(0.01 / 3.6) * (float)speedSign();
00302 out.enabled = ptr->ENABLED ? true : false;
00303 out.override = ptr->OVERRIDE ? true : false;
00304 out.fault_wdc = ptr->FLTWDC ? true : false;
00305 out.fault_bus1 = ptr->FLTBUS1 ? true : false;
00306 out.fault_bus2 = ptr->FLTBUS2 ? true : false;
00307 out.fault_calibration = ptr->FLTCAL ? true : false;
00308 out.fault_power = ptr->FLTPWR ? true : false;
00309 if (firmware_.findPlatform(M_STEER) >= FIRMWARE_TIMEOUT) {
00310 timeoutSteering(ptr->TMOUT, ptr->ENABLED);
00311 out.timeout = ptr->TMOUT ? true : false;
00312 }
00313 overrideSteering(ptr->OVERRIDE, out.timeout);
00314 pub_steering_.publish(out);
00315 geometry_msgs::TwistStamped twist;
00316 twist.header.stamp = out.header.stamp;
00317 twist.header.frame_id = frame_id_;
00318 twist.twist.linear.x = out.speed;
00319 twist.twist.angular.z = out.speed * tan(out.steering_wheel_angle / steering_ratio_) / acker_wheelbase_;
00320 pub_twist_.publish(twist);
00321 publishJointStates(msg->header.stamp, NULL, &out);
00322 if (ptr->FLTBUS1 || ptr->FLTBUS2 || ptr->FLTPWR) {
00323 ROS_WARN_THROTTLE(5.0, "Steering fault. FLT1: %s FLT2: %s FLTPWR: %s",
00324 ptr->FLTBUS1 ? "true, " : "false,",
00325 ptr->FLTBUS2 ? "true, " : "false,",
00326 ptr->FLTPWR ? "true" : "false");
00327 } else if (ptr->FLTCAL) {
00328 ROS_WARN_THROTTLE(5.0, "Steering calibration fault. Drive at least 25 mph for at least 10 seconds in a straight line.");
00329 }
00330 }
00331 break;
00332
00333 case ID_GEAR_REPORT:
00334 if (msg->dlc >= 1) {
00335 const MsgGearReport *ptr = (const MsgGearReport*)msg->data.elems;
00336 overrideGear(ptr->OVERRIDE);
00337 dbw_mkz_msgs::GearReport out;
00338 out.header.stamp = msg->header.stamp;
00339 out.state.gear = ptr->STATE;
00340 out.cmd.gear = ptr->CMD;
00341 out.override = ptr->OVERRIDE ? true : false;
00342 out.fault_bus = ptr->FLTBUS ? true : false;
00343 if (msg->dlc >= sizeof(MsgGearReport)) {
00344 out.reject.value = ptr->REJECT;
00345 if (out.reject.value == dbw_mkz_msgs::GearReject::NONE) {
00346 gear_warned_ = false;
00347 } else if (!gear_warned_) {
00348 gear_warned_ = true;
00349 switch (out.reject.value) {
00350 case dbw_mkz_msgs::GearReject::SHIFT_IN_PROGRESS:
00351 ROS_WARN("Gear shift rejected: Shift in progress");
00352 break;
00353 case dbw_mkz_msgs::GearReject::OVERRIDE:
00354 ROS_WARN("Gear shift rejected: Override on brake, throttle, or steering");
00355 break;
00356 case dbw_mkz_msgs::GearReject::ROTARY_LOW:
00357 ROS_WARN("Gear shift rejected: Rotary shifter can't shift to Low");
00358 break;
00359 case dbw_mkz_msgs::GearReject::ROTARY_PARK:
00360 ROS_WARN("Gear shift rejected: Rotary shifter can't shift out of Park");
00361 break;
00362 case dbw_mkz_msgs::GearReject::VEHICLE:
00363 ROS_WARN("Gear shift rejected: Rejected by vehicle, try pressing the brakes");
00364 break;
00365 }
00366 }
00367 }
00368 pub_gear_.publish(out);
00369 }
00370 break;
00371
00372 case ID_MISC_REPORT:
00373 if (msg->dlc >= 3) {
00374 const MsgMiscReport *ptr = (const MsgMiscReport*)msg->data.elems;
00375 if (buttons_) {
00376 if (ptr->btn_cc_gap_inc || ptr->btn_cc_cncl) {
00377 buttonCancel();
00378 } else if ((ptr->btn_cc_set_dec && ptr->btn_cc_gap_dec) || (ptr->btn_cc_set_inc && ptr->btn_cc_off) || (ptr->btn_cc_set_dec && ptr->btn_cc_res)) {
00379 enableSystem();
00380 }
00381 }
00382 dbw_mkz_msgs::Misc1Report out;
00383 out.header.stamp = msg->header.stamp;
00384 out.turn_signal.value = ptr->turn_signal;
00385 out.high_beam_headlights = ptr->head_light_hi ? true : false;
00386 out.wiper.status = ptr->wiper_front;
00387 out.ambient_light.status = ptr->light_ambient;
00388 out.btn_cc_on = ptr->btn_cc_on ? true : false;
00389 out.btn_cc_off = ptr->btn_cc_off ? true : false;
00390 out.btn_cc_on_off = ptr->btn_cc_on_off ? true : false;
00391 out.btn_cc_res = ptr->btn_cc_res ? true : false;
00392 out.btn_cc_cncl = ptr->btn_cc_cncl ? true : false;
00393 out.btn_cc_res_cncl = ptr->btn_cc_res_cncl ? true : false;
00394 out.btn_cc_res_inc = ptr->btn_cc_res_inc ? true : false;
00395 out.btn_cc_res_dec = ptr->btn_cc_res_dec ? true : false;
00396 out.btn_cc_set_inc = ptr->btn_cc_set_inc ? true : false;
00397 out.btn_cc_set_dec = ptr->btn_cc_set_dec ? true : false;
00398 out.btn_cc_gap_inc = ptr->btn_cc_gap_inc ? true : false;
00399 out.btn_cc_gap_dec = ptr->btn_cc_gap_dec ? true : false;
00400 out.btn_la_on_off = ptr->btn_la_on_off ? true : false;
00401 out.fault_bus = ptr->FLTBUS ? true : false;
00402 if (msg->dlc >= 5) {
00403 out.door_driver = ptr->door_driver ? true : false;
00404 out.door_passenger = ptr->door_passenger ? true : false;
00405 out.door_rear_left = ptr->door_rear_left ? true : false;
00406 out.door_rear_right = ptr->door_rear_right ? true : false;
00407 out.door_hood = ptr->door_hood ? true : false;
00408 out.door_trunk = ptr->door_trunk ? true : false;
00409 out.passenger_detect = ptr->pasngr_detect ? true : false;
00410 out.passenger_airbag = ptr->pasngr_airbag ? true : false;
00411 out.buckle_driver = ptr->buckle_driver ? true : false;
00412 out.buckle_passenger = ptr->buckle_pasngr ? true : false;
00413 out.btn_ld_ok = ptr->btn_ld_ok ? true : false;
00414 out.btn_ld_up = ptr->btn_ld_up ? true : false;
00415 out.btn_ld_down = ptr->btn_ld_down ? true : false;
00416 out.btn_ld_left = ptr->btn_ld_left ? true : false;
00417 out.btn_ld_right = ptr->btn_ld_right ? true : false;
00418 }
00419 if ((msg->dlc >= 8) && (ptr->outside_air_temp < 0xFE)) {
00420 out.outside_temperature = ((float)ptr->outside_air_temp * 0.5f) - 40.0f;
00421 } else {
00422 out.outside_temperature = NAN;
00423 }
00424 pub_misc_1_.publish(out);
00425 }
00426 break;
00427
00428 case ID_REPORT_WHEEL_SPEED:
00429 if (msg->dlc >= sizeof(MsgReportWheelSpeed)) {
00430 const MsgReportWheelSpeed *ptr = (const MsgReportWheelSpeed*)msg->data.elems;
00431 dbw_mkz_msgs::WheelSpeedReport out;
00432 out.header.stamp = msg->header.stamp;
00433 out.front_left = (float)ptr->front_left * 0.01f;
00434 out.front_right = (float)ptr->front_right * 0.01f;
00435 out.rear_left = (float)ptr->rear_left * 0.01f;
00436 out.rear_right = (float)ptr->rear_right * 0.01f;
00437 pub_wheel_speeds_.publish(out);
00438 publishJointStates(msg->header.stamp, &out, NULL);
00439 }
00440 break;
00441
00442 case ID_REPORT_WHEEL_POSITION:
00443 if (msg->dlc >= sizeof(MsgReportWheelPosition)) {
00444 const MsgReportWheelPosition *ptr = (const MsgReportWheelPosition*)msg->data.elems;
00445 dbw_mkz_msgs::WheelPositionReport out;
00446 out.header.stamp = msg->header.stamp;
00447 out.front_left = ptr->front_left;
00448 out.front_right = ptr->front_right;
00449 out.rear_left = ptr->rear_left;
00450 out.rear_right = ptr->rear_right;
00451 pub_wheel_positions_.publish(out);
00452 }
00453 break;
00454
00455 case ID_REPORT_TIRE_PRESSURE:
00456 if (msg->dlc >= sizeof(MsgReportTirePressure)) {
00457 const MsgReportTirePressure *ptr = (const MsgReportTirePressure*)msg->data.elems;
00458 dbw_mkz_msgs::TirePressureReport out;
00459 out.header.stamp = msg->header.stamp;
00460 out.front_left = (float)ptr->front_left;
00461 out.front_right = (float)ptr->front_right;
00462 out.rear_left = (float)ptr->rear_left;
00463 out.rear_right = (float)ptr->rear_right;
00464 pub_tire_pressure_.publish(out);
00465 }
00466 break;
00467
00468 case ID_REPORT_FUEL_LEVEL:
00469 if (msg->dlc >= 2) {
00470 const MsgReportFuelLevel *ptr = (const MsgReportFuelLevel*)msg->data.elems;
00471 dbw_mkz_msgs::FuelLevelReport out;
00472 out.header.stamp = msg->header.stamp;
00473 out.fuel_level = (float)ptr->fuel_level * 0.108696f;
00474 if (msg->dlc >= sizeof(MsgReportFuelLevel)) {
00475 out.battery_12v = (float)ptr->battery_12v * 0.0625f;
00476 out.battery_hev = (float)ptr->battery_hev * 0.5f;
00477 out.odometer = (float)ptr->odometer * 0.1f;
00478 }
00479 pub_fuel_level_.publish(out);
00480 }
00481 break;
00482
00483 case ID_REPORT_SURROUND:
00484 if (msg->dlc >= sizeof(MsgReportSurround)) {
00485 const MsgReportSurround *ptr = (const MsgReportSurround*)msg->data.elems;
00486 dbw_mkz_msgs::SurroundReport out;
00487 out.header.stamp = msg->header.stamp;
00488 out.cta_left_alert = ptr->l_cta_alert ? true : false;
00489 out.cta_right_alert = ptr->r_cta_alert ? true : false;
00490 out.cta_left_enabled = ptr->l_cta_enabled ? true : false;
00491 out.cta_right_enabled = ptr->r_cta_enabled ? true : false;
00492 out.blis_left_alert = ptr->l_blis_alert ? true : false;
00493 out.blis_right_alert = ptr->r_blis_alert ? true : false;
00494 out.blis_left_enabled = ptr->l_blis_enabled ? true : false;
00495 out.blis_right_enabled = ptr->r_blis_enabled ? true : false;
00496 out.sonar_enabled = ptr->sonar_enabled ? true : false;
00497 out.sonar_fault = ptr->sonar_fault ? true : false;
00498 if (out.sonar_enabled) {
00499 out.sonar[0] = sonarMetersFromBits(ptr->sonar_00);
00500 out.sonar[1] = sonarMetersFromBits(ptr->sonar_01);
00501 out.sonar[2] = sonarMetersFromBits(ptr->sonar_02);
00502 out.sonar[3] = sonarMetersFromBits(ptr->sonar_03);
00503 out.sonar[4] = sonarMetersFromBits(ptr->sonar_04);
00504 out.sonar[5] = sonarMetersFromBits(ptr->sonar_05);
00505 out.sonar[6] = sonarMetersFromBits(ptr->sonar_06);
00506 out.sonar[7] = sonarMetersFromBits(ptr->sonar_07);
00507 out.sonar[8] = sonarMetersFromBits(ptr->sonar_08);
00508 out.sonar[9] = sonarMetersFromBits(ptr->sonar_09);
00509 out.sonar[10] = sonarMetersFromBits(ptr->sonar_10);
00510 out.sonar[11] = sonarMetersFromBits(ptr->sonar_11);
00511 }
00512 pub_surround_.publish(out);
00513 sensor_msgs::PointCloud2 cloud;
00514 sonarBuildPointCloud2(cloud, out);
00515 pub_sonar_cloud_.publish(cloud);
00516 }
00517 break;
00518
00519 case ID_REPORT_BRAKE_INFO:
00520 if (msg->dlc >= sizeof(MsgReportBrakeInfo)) {
00521 const MsgReportBrakeInfo *ptr = (const MsgReportBrakeInfo*)msg->data.elems;
00522 dbw_mkz_msgs::BrakeInfoReport out;
00523 out.header.stamp = msg->header.stamp;
00524 out.brake_torque_request = (float)ptr->brake_torque_request * 4.0f;
00525 out.brake_torque_actual = (float)ptr->brake_torque_actual * 4.0f;
00526 out.wheel_torque_actual = (float)ptr->wheel_torque * 4.0f;
00527 out.accel_over_ground = (float)ptr->accel_over_ground_est * 0.035f;
00528 out.brake_pedal_qf.value = ptr->bped_qf;
00529 out.hsa.status = ptr->hsa_stat;
00530 out.hsa.mode = ptr->hsa_mode;
00531 out.abs_active = ptr->abs_active ? true : false;
00532 out.abs_enabled = ptr->abs_enabled ? true : false;
00533 out.stab_active = ptr->stab_active ? true : false;
00534 out.stab_enabled = ptr->stab_enabled ? true : false;
00535 out.trac_active = ptr->trac_active ? true : false;
00536 out.trac_enabled = ptr->trac_enabled ? true : false;
00537 out.parking_brake.status = ptr->parking_brake;
00538 out.stationary = ptr->stationary;
00539 pub_brake_info_.publish(out);
00540 if (ptr->bped_qf != dbw_mkz_msgs::QualityFactor::OK) {
00541 ROS_WARN_THROTTLE(5.0, "Brake pedal limp-home: %u", ptr->bped_qf);
00542 }
00543 }
00544 break;
00545
00546 case ID_REPORT_THROTTLE_INFO:
00547 if (msg->dlc >= sizeof(MsgReportThrottleInfo)) {
00548 const MsgReportThrottleInfo *ptr = (const MsgReportThrottleInfo*)msg->data.elems;
00549 dbw_mkz_msgs::ThrottleInfoReport out;
00550 out.header.stamp = msg->header.stamp;
00551 out.throttle_pc = (float)ptr->throttle_pc * 1e-3f;
00552 out.throttle_rate = (float)ptr->throttle_rate * 4e-4f;
00553 out.throttle_pedal_qf.value = ptr->aped_qf;
00554 out.engine_rpm = (float)ptr->engine_rpm * 0.25f;
00555 pub_throttle_info_.publish(out);
00556 if (ptr->aped_qf != dbw_mkz_msgs::QualityFactor::OK) {
00557 ROS_WARN_THROTTLE(5.0, "Throttle pedal limp-home: %u", ptr->aped_qf);
00558 }
00559 }
00560 break;
00561
00562 case ID_REPORT_DRIVER_ASSIST:
00563 if (msg->dlc >= sizeof(MsgReportDriverAssist)) {
00564 const MsgReportDriverAssist *ptr = (const MsgReportDriverAssist*)msg->data.elems;
00565 dbw_mkz_msgs::DriverAssistReport out;
00566 out.header.stamp = msg->header.stamp;
00567 out.decel = (float)ptr->decel * 0.0625f;
00568 out.decel_src = ptr->decel_src;
00569 out.fcw_enabled = ptr->fcw_enabled;
00570 out.fcw_active = ptr->fcw_active;
00571 out.aeb_enabled = ptr->aeb_enabled;
00572 out.aeb_precharge = ptr->aeb_precharge;
00573 out.aeb_braking = ptr->aeb_braking;
00574 out.acc_enabled = ptr->acc_enabled;
00575 out.acc_braking = ptr->acc_braking;
00576 pub_driver_assist_.publish(out);
00577 if (out.fcw_active) {
00578 ROS_WARN_THROTTLE(5.0, "Forward collision warning activated!");
00579 }
00580 if (out.aeb_braking) {
00581 ROS_WARN_THROTTLE(5.0, "Automatic emergency braking activated!");
00582 }
00583 }
00584 break;
00585
00586 case ID_LICENSE:
00587 if (msg->dlc >= sizeof(MsgLicense)) {
00588 const MsgLicense *ptr = (const MsgLicense*)msg->data.elems;
00589 if (ptr->ready) {
00590 ROS_INFO_ONCE("DBW Licensing: Ready");
00591 if (ptr->trial) {
00592 ROS_WARN_ONCE("DBW Licensing: One or more features licensed as a counted trial. Visit http://dataspeedinc.com/maintenance/ to request a full license.");
00593 }
00594 if (ptr->expired) {
00595 ROS_WARN_ONCE("DBW Licensing: One or more feature licenses expired due to the firmware build date");
00596 }
00597 } else {
00598 ROS_INFO_THROTTLE(10.0, "DBW Licensing: Waiting to resolve VIN...");
00599 }
00600 if (ptr->mux == LIC_MUX_MAC) {
00601 ROS_INFO_ONCE("Detected firmware MAC: %02X:%02X:%02X:%02X:%02X:%02X",
00602 ptr->mac.addr0, ptr->mac.addr1,
00603 ptr->mac.addr2, ptr->mac.addr3,
00604 ptr->mac.addr4, ptr->mac.addr5);
00605 } else if (ptr->mux == LIC_MUX_DATE0) {
00606 if (date_.size() == 0) {
00607 date_.push_back(ptr->date0.date0);
00608 date_.push_back(ptr->date0.date1);
00609 date_.push_back(ptr->date0.date2);
00610 date_.push_back(ptr->date0.date3);
00611 date_.push_back(ptr->date0.date4);
00612 date_.push_back(ptr->date0.date5);
00613 }
00614 } else if (ptr->mux == LIC_MUX_DATE1) {
00615 if (date_.size() == 6) {
00616 date_.push_back(ptr->date1.date6);
00617 date_.push_back(ptr->date1.date7);
00618 date_.push_back(ptr->date1.date8);
00619 date_.push_back(ptr->date1.date9);
00620 ROS_INFO("Detected firmware build date: %s", date_.c_str());
00621 }
00622 } else if (ptr->mux == LIC_MUX_VIN0) {
00623 if (vin_.size() == 0) {
00624 vin_.push_back(ptr->vin0.vin00);
00625 vin_.push_back(ptr->vin0.vin01);
00626 vin_.push_back(ptr->vin0.vin02);
00627 vin_.push_back(ptr->vin0.vin03);
00628 vin_.push_back(ptr->vin0.vin04);
00629 vin_.push_back(ptr->vin0.vin05);
00630 }
00631 } else if (ptr->mux == LIC_MUX_VIN1) {
00632 if (vin_.size() == 6) {
00633 vin_.push_back(ptr->vin1.vin06);
00634 vin_.push_back(ptr->vin1.vin07);
00635 vin_.push_back(ptr->vin1.vin08);
00636 vin_.push_back(ptr->vin1.vin09);
00637 vin_.push_back(ptr->vin1.vin10);
00638 vin_.push_back(ptr->vin1.vin11);
00639 }
00640 } else if (ptr->mux == LIC_MUX_VIN2) {
00641 if (vin_.size() == 12) {
00642 vin_.push_back(ptr->vin2.vin12);
00643 vin_.push_back(ptr->vin2.vin13);
00644 vin_.push_back(ptr->vin2.vin14);
00645 vin_.push_back(ptr->vin2.vin15);
00646 vin_.push_back(ptr->vin2.vin16);
00647 std_msgs::String msg; msg.data = vin_;
00648 pub_vin_.publish(msg);
00649 ROS_INFO("Detected VIN: %s", vin_.c_str());
00650 }
00651 } else if (ptr->mux == LIC_MUX_F0) {
00652 const char * const NAME = "BASE";
00653 if (ptr->license.enabled) {
00654 ROS_INFO_ONCE("DBW Licensing: Feature '%s' enabled%s", NAME, ptr->license.trial ? " as a counted trial" : "");
00655 } else if (ptr->ready) {
00656 ROS_WARN_ONCE("DBW Licensing: Feature '%s' not licensed. Visit http://dataspeedinc.com/maintenance/ to request a license.", NAME);
00657 }
00658 if (ptr->ready && (ptr->license.trial || !ptr->license.enabled)) {
00659 ROS_INFO_ONCE("DBW Licensing: Feature '%s' trials used: %u, remaining: %u", NAME,
00660 ptr->license.trials_used, ptr->license.trials_left);
00661 }
00662 }
00663 }
00664 break;
00665
00666 case ID_VERSION:
00667 if (msg->dlc >= sizeof(MsgVersion)) {
00668 const MsgVersion *ptr = (const MsgVersion*)msg->data.elems;
00669 const PlatformVersion version((Platform)ptr->platform, (Module)ptr->module, ptr->major, ptr->minor, ptr->build);
00670 const ModuleVersion latest = FIRMWARE_LATEST.findModule(version);
00671 const char * str_p = platformToString(version.p);
00672 const char * str_m = moduleToString(version.m);
00673 if (firmware_.findModule(version) != version.v) {
00674 firmware_.insert(version);
00675 if (latest.valid()) {
00676 ROS_INFO("Detected %s %s firmware version %u.%u.%u", str_p, str_m, ptr->major, ptr->minor, ptr->build);
00677 } else {
00678 ROS_WARN("Detected %s %s firmware version %u.%u.%u, which is unsupported. Platform: 0x%02X, Module: %u", str_p, str_m,
00679 ptr->major, ptr->minor, ptr->build, ptr->platform, ptr->module);
00680 }
00681 if (version < latest) {
00682 ROS_WARN("Firmware %s %s has old version %u.%u.%u, updating to %u.%u.%u is suggested.", str_p, str_m,
00683 version.v.major(), version.v.minor(), version.v.build(),
00684 latest.major(), latest.minor(), latest.build());
00685 }
00686 }
00687 }
00688 break;
00689
00690 case ID_BRAKE_CMD:
00691 ROS_WARN_COND(warn_cmds_ && !((const MsgBrakeCmd*)msg->data.elems)->RES1,
00692 "DBW system: Another node on the CAN bus is commanding the vehicle!!! Subsystem: Brake. Id: 0x%03X", ID_BRAKE_CMD);
00693 break;
00694 case ID_THROTTLE_CMD:
00695 ROS_WARN_COND(warn_cmds_ && !((const MsgThrottleCmd*)msg->data.elems)->RES1,
00696 "DBW system: Another node on the CAN bus is commanding the vehicle!!! Subsystem: Throttle. Id: 0x%03X", ID_THROTTLE_CMD);
00697 break;
00698 case ID_STEERING_CMD:
00699 ROS_WARN_COND(warn_cmds_, "DBW system: Another node on the CAN bus is commanding the vehicle!!! Subsystem: Steering. Id: 0x%03X", ID_STEERING_CMD);
00700 break;
00701 case ID_GEAR_CMD:
00702 ROS_WARN_COND(warn_cmds_, "DBW system: Another node on the CAN bus is commanding the vehicle!!! Subsystem: Shifting. Id: 0x%03X", ID_GEAR_CMD);
00703 break;
00704 case ID_MISC_CMD:
00705 ROS_WARN_COND(warn_cmds_, "DBW system: Another node on the CAN bus is commanding the vehicle!!! Subsystem: Turn Signals. Id: 0x%03X", ID_MISC_CMD);
00706 break;
00707 }
00708 }
00709 #if 0
00710 ROS_INFO("ena: %s, clr: %s, brake: %s, throttle: %s, steering: %s, gear: %s",
00711 enabled() ? "true " : "false",
00712 clear() ? "true " : "false",
00713 override_brake_ ? "true " : "false",
00714 override_throttle_ ? "true " : "false",
00715 override_steering_ ? "true " : "false",
00716 override_gear_ ? "true " : "false"
00717 );
00718 #endif
00719 }
00720
00721 void DbwNode::recvCanImu(const std::vector<can_msgs::Frame::ConstPtr> &msgs) {
00722 ROS_ASSERT(msgs.size() == 2);
00723 ROS_ASSERT(msgs[0]->id == ID_REPORT_ACCEL);
00724 ROS_ASSERT(msgs[1]->id == ID_REPORT_GYRO);
00725 if ((msgs[0]->dlc >= sizeof(MsgReportAccel)) && (msgs[1]->dlc >= sizeof(MsgReportGyro))) {
00726 const MsgReportAccel *ptr_accel = (const MsgReportAccel*)msgs[0]->data.elems;
00727 const MsgReportGyro *ptr_gyro = (const MsgReportGyro*)msgs[1]->data.elems;
00728 sensor_msgs::Imu out;
00729 out.header.stamp = msgs[0]->header.stamp;
00730 out.header.frame_id = frame_id_;
00731 out.orientation_covariance[0] = -1;
00732 out.linear_acceleration.x = (double)ptr_accel->accel_long * 0.01;
00733 out.linear_acceleration.y = (double)ptr_accel->accel_lat * -0.01;
00734 out.linear_acceleration.z = (double)ptr_accel->accel_vert * -0.01;
00735 out.angular_velocity.x = (double)ptr_gyro->gyro_roll * 0.0002;
00736 out.angular_velocity.z = (double)ptr_gyro->gyro_yaw * 0.0002;
00737 pub_imu_.publish(out);
00738 }
00739 #if 0
00740 ROS_INFO("Time: %u.%u, %u.%u, delta: %fms",
00741 msgs[0]->header.stamp.sec, msgs[0]->header.stamp.nsec,
00742 msgs[1]->header.stamp.sec, msgs[1]->header.stamp.nsec,
00743 labs((msgs[1]->header.stamp - msgs[0]->header.stamp).toNSec()) / 1000000.0);
00744 #endif
00745 }
00746
00747 void DbwNode::recvCanGps(const std::vector<can_msgs::Frame::ConstPtr> &msgs) {
00748 ROS_ASSERT(msgs.size() == 3);
00749 ROS_ASSERT(msgs[0]->id == ID_REPORT_GPS1);
00750 ROS_ASSERT(msgs[1]->id == ID_REPORT_GPS2);
00751 ROS_ASSERT(msgs[2]->id == ID_REPORT_GPS3);
00752 if ((msgs[0]->dlc >= sizeof(MsgReportGps1)) && (msgs[1]->dlc >= sizeof(MsgReportGps2)) && (msgs[2]->dlc >= sizeof(MsgReportGps3))) {
00753 const MsgReportGps1 *ptr1 = (const MsgReportGps1*)msgs[0]->data.elems;
00754 const MsgReportGps2 *ptr2 = (const MsgReportGps2*)msgs[1]->data.elems;
00755 const MsgReportGps3 *ptr3 = (const MsgReportGps3*)msgs[2]->data.elems;
00756
00757 sensor_msgs::NavSatFix msg_fix;
00758 msg_fix.header.stamp = msgs[0]->header.stamp;
00759 msg_fix.latitude = (double)ptr1->latitude / 3e6;
00760 msg_fix.longitude = (double)ptr1->longitude / 3e6;
00761 msg_fix.altitude = (double)ptr3->altitude * 0.25;
00762 msg_fix.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
00763 msg_fix.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
00764 switch (ptr3->quality) {
00765 case 0:
00766 default:
00767 msg_fix.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
00768 break;
00769 case 1:
00770 case 2:
00771 msg_fix.status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
00772 break;
00773 }
00774 pub_gps_fix_.publish(msg_fix);
00775
00776 geometry_msgs::TwistStamped msg_vel;
00777 msg_vel.header.stamp = msgs[0]->header.stamp;
00778 double heading = (double)ptr3->heading * (0.01 * M_PI / 180);
00779 double speed = (double)ptr3->speed * 0.44704;
00780 msg_vel.twist.linear.x = cos(heading) * speed;
00781 msg_vel.twist.linear.y = sin(heading) * speed;
00782 pub_gps_vel_.publish(msg_vel);
00783
00784 sensor_msgs::TimeReference msg_time;
00785 struct tm unix_time;
00786 unix_time.tm_year = ptr2->utc_year + 100;
00787 unix_time.tm_mon = ptr2->utc_month - 1;
00788 unix_time.tm_mday = ptr2->utc_day;
00789 unix_time.tm_hour = ptr2->utc_hours;
00790 unix_time.tm_min = ptr2->utc_minutes;
00791 unix_time.tm_sec = ptr2->utc_seconds;
00792 msg_time.header.stamp = msgs[0]->header.stamp;
00793 msg_time.time_ref.sec = timegm(&unix_time);
00794 msg_time.time_ref.nsec = 0;
00795 pub_gps_time_.publish(msg_time);
00796
00797 #if 0
00798 ROS_INFO("UTC Time: %04d-%02d-%02d %02d:%02d:%02d",
00799 2000 + ptr2->utc_year, ptr2->utc_month, ptr2->utc_day,
00800 ptr2->utc_hours, ptr2->utc_minutes, ptr2->utc_seconds);
00801 #endif
00802 }
00803 #if 0
00804 ROS_INFO("Time: %u.%u, %u.%u, %u.%u, delta: %fms",
00805 msgs[0]->header.stamp.sec, msgs[0]->header.stamp.nsec,
00806 msgs[1]->header.stamp.sec, msgs[1]->header.stamp.nsec,
00807 msgs[2]->header.stamp.sec, msgs[2]->header.stamp.nsec,
00808 std::max(std::max(
00809 labs((msgs[1]->header.stamp - msgs[0]->header.stamp).toNSec()),
00810 labs((msgs[2]->header.stamp - msgs[1]->header.stamp).toNSec())),
00811 labs((msgs[0]->header.stamp - msgs[2]->header.stamp).toNSec())) / 1000000.0);
00812 #endif
00813 }
00814
00815 void DbwNode::recvBrakeCmd(const dbw_mkz_msgs::BrakeCmd::ConstPtr& msg)
00816 {
00817 can_msgs::Frame out;
00818 out.id = ID_BRAKE_CMD;
00819 out.is_extended = false;
00820 out.dlc = sizeof(MsgBrakeCmd);
00821 MsgBrakeCmd *ptr = (MsgBrakeCmd*)out.data.elems;
00822 memset(ptr, 0x00, sizeof(*ptr));
00823 if (enabled()) {
00824 bool fwd_abs = firmware_.findModule(M_ABS).valid();
00825 bool fwd_bpe = firmware_.findPlatform(M_BPEC) >= FIRMWARE_CMDTYPE;
00826 bool fwd = !pedal_luts_;
00827 fwd |= fwd_abs;
00828 fwd &= fwd_bpe;
00829 switch (msg->pedal_cmd_type) {
00830 case dbw_mkz_msgs::BrakeCmd::CMD_NONE:
00831 break;
00832 case dbw_mkz_msgs::BrakeCmd::CMD_PEDAL:
00833 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_PEDAL;
00834 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, msg->pedal_cmd * UINT16_MAX));
00835 if (!firmware_.findModule(M_BPEC).valid() && firmware_.findModule(M_ABS).valid()) {
00836 ROS_WARN_THROTTLE(1.0, "Module ABS does not support brake command type PEDAL");
00837 }
00838 break;
00839 case dbw_mkz_msgs::BrakeCmd::CMD_PERCENT:
00840 if (fwd) {
00841 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_PERCENT;
00842 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, msg->pedal_cmd * UINT16_MAX));
00843 } else {
00844 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_PEDAL;
00845 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, brakePedalFromPercent(msg->pedal_cmd) * UINT16_MAX));
00846 }
00847 break;
00848 case dbw_mkz_msgs::BrakeCmd::CMD_TORQUE:
00849 if (fwd) {
00850 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_TORQUE;
00851 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, msg->pedal_cmd));
00852 } else {
00853 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_PEDAL;
00854 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, brakePedalFromTorque(msg->pedal_cmd) * UINT16_MAX));
00855 }
00856 if (!firmware_.findModule(M_BPEC).valid() && firmware_.findModule(M_ABS).valid()) {
00857 ROS_WARN_THROTTLE(1.0, "Module ABS does not support brake command type TORQUE");
00858 }
00859 break;
00860 case dbw_mkz_msgs::BrakeCmd::CMD_TORQUE_RQ:
00861 if (fwd_abs || fwd_bpe) {
00862
00863 fwd = true;
00864 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_TORQUE_RQ;
00865 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, msg->pedal_cmd));
00866 } else if (fwd) {
00867
00868 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_TORQUE;
00869 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, msg->pedal_cmd));
00870 } else {
00871
00872 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_PEDAL;
00873 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, brakePedalFromTorque(msg->pedal_cmd) * UINT16_MAX));
00874 }
00875 if (!firmware_.findModule(M_BPEC).valid() && firmware_.findModule(M_ABS).valid()) {
00876 ROS_WARN_THROTTLE(1.0, "Module ABS does not support brake command type TORQUE_RQ");
00877 }
00878 break;
00879 case dbw_mkz_msgs::BrakeCmd::CMD_DECEL:
00880
00881 ptr->CMD_TYPE = dbw_mkz_msgs::BrakeCmd::CMD_DECEL;
00882 ptr->PCMD = std::max((float)0.0, std::min((float)10e3, msg->pedal_cmd * 1e3f));
00883 if (!firmware_.findModule(M_ABS).valid() && firmware_.findModule(M_BPEC).valid()) {
00884 ROS_WARN_THROTTLE(1.0, "Module BPEC does not support brake command type DECEL");
00885 }
00886 break;
00887 default:
00888 ROS_WARN("Unknown brake command type: %u", msg->pedal_cmd_type);
00889 break;
00890 }
00891 #if 1 // Manually implement auto BOO control (brake lights) for legacy firmware
00892 ptr->ABOO = 1;
00893 const PlatformVersion firmware_bpec = firmware_.findPlatform(M_BPEC);
00894 if (firmware_bpec.v.valid() && (firmware_bpec < FIRMWARE_CMDTYPE)) {
00895 const uint16_t BOO_THRESH_LO = 0.20 * UINT16_MAX;
00896 const uint16_t BOO_THRESH_HI = 0.22 * UINT16_MAX;
00897 static bool boo_status_ = false;
00898 if (boo_status_) {
00899 ptr->BCMD = 1;
00900 }
00901 if (!boo_status_ && (ptr->PCMD > BOO_THRESH_HI)) {
00902 ptr->BCMD = 1;
00903 boo_status_ = true;
00904 } else if (boo_status_ && (ptr->PCMD < BOO_THRESH_LO)) {
00905 ptr->BCMD = 0;
00906 boo_status_ = false;
00907 }
00908 }
00909 #endif
00910 if (msg->enable) {
00911 ptr->EN = 1;
00912 }
00913 }
00914 if (clear() || msg->clear) {
00915 ptr->CLEAR = 1;
00916 }
00917 if (msg->ignore) {
00918 ptr->IGNORE = 1;
00919 }
00920 ptr->COUNT = msg->count;
00921 pub_can_.publish(out);
00922 }
00923
00924 void DbwNode::recvThrottleCmd(const dbw_mkz_msgs::ThrottleCmd::ConstPtr& msg)
00925 {
00926 can_msgs::Frame out;
00927 out.id = ID_THROTTLE_CMD;
00928 out.is_extended = false;
00929 out.dlc = sizeof(MsgThrottleCmd);
00930 MsgThrottleCmd *ptr = (MsgThrottleCmd*)out.data.elems;
00931 memset(ptr, 0x00, sizeof(*ptr));
00932 if (enabled()) {
00933 bool fwd = !pedal_luts_;
00934 fwd &= firmware_.findPlatform(M_TPEC) >= FIRMWARE_CMDTYPE;
00935 float cmd = 0.0;
00936 switch (msg->pedal_cmd_type) {
00937 case dbw_mkz_msgs::ThrottleCmd::CMD_NONE:
00938 break;
00939 case dbw_mkz_msgs::ThrottleCmd::CMD_PEDAL:
00940 ptr->CMD_TYPE = dbw_mkz_msgs::ThrottleCmd::CMD_PEDAL;
00941 cmd = msg->pedal_cmd;
00942 break;
00943 case dbw_mkz_msgs::ThrottleCmd::CMD_PERCENT:
00944 if (fwd) {
00945 ptr->CMD_TYPE = dbw_mkz_msgs::ThrottleCmd::CMD_PERCENT;
00946 cmd = msg->pedal_cmd;
00947 } else {
00948 ptr->CMD_TYPE = dbw_mkz_msgs::ThrottleCmd::CMD_PEDAL;
00949 cmd = throttlePedalFromPercent(msg->pedal_cmd);
00950 }
00951 break;
00952 default:
00953 ROS_WARN("Unknown throttle command type: %u", msg->pedal_cmd_type);
00954 break;
00955 }
00956 ptr->PCMD = std::max((float)0.0, std::min((float)UINT16_MAX, cmd * UINT16_MAX));
00957 if (msg->enable) {
00958 ptr->EN = 1;
00959 }
00960 }
00961 if (clear() || msg->clear) {
00962 ptr->CLEAR = 1;
00963 }
00964 if (msg->ignore) {
00965 ptr->IGNORE = 1;
00966 }
00967 ptr->COUNT = msg->count;
00968 pub_can_.publish(out);
00969 }
00970
00971 void DbwNode::recvSteeringCmd(const dbw_mkz_msgs::SteeringCmd::ConstPtr& msg)
00972 {
00973 can_msgs::Frame out;
00974 out.id = ID_STEERING_CMD;
00975 out.is_extended = false;
00976 out.dlc = sizeof(MsgSteeringCmd);
00977 MsgSteeringCmd *ptr = (MsgSteeringCmd*)out.data.elems;
00978 memset(ptr, 0x00, sizeof(*ptr));
00979 if (enabled()) {
00980 switch (msg->cmd_type) {
00981 case dbw_mkz_msgs::SteeringCmd::CMD_ANGLE:
00982 ptr->SCMD = std::max((float)-INT16_MAX, std::min((float)INT16_MAX, (float)(msg->steering_wheel_angle_cmd * (180 / M_PI * 10))));
00983 if (fabsf(msg->steering_wheel_angle_velocity) > 0) {
00984 if (firmware_.findModule(M_EPS).valid() || (firmware_.findPlatform(M_STEER) >= FIRMWARE_HIGH_RATE_LIMIT)) {
00985 ptr->SVEL = std::max((float)1, std::min((float)254, (float)roundf(fabsf(msg->steering_wheel_angle_velocity) * 180 / M_PI / 4)));
00986 } else {
00987 ptr->SVEL = std::max((float)1, std::min((float)254, (float)roundf(fabsf(msg->steering_wheel_angle_velocity) * 180 / M_PI / 2)));
00988 }
00989 }
00990 ptr->CMD_TYPE = dbw_mkz_msgs::SteeringCmd::CMD_ANGLE;
00991 break;
00992 case dbw_mkz_msgs::SteeringCmd::CMD_TORQUE:
00993 ptr->SCMD = std::max((float)-INT16_MAX, std::min((float)INT16_MAX, (float)(msg->steering_wheel_torque_cmd * 128)));
00994 ptr->CMD_TYPE = dbw_mkz_msgs::SteeringCmd::CMD_TORQUE;
00995 if (!firmware_.findModule(M_EPS).valid() && firmware_.findModule(M_STEER).valid()) {
00996 ROS_WARN_THROTTLE(1.0, "Module STEER does not support steering command type TORQUE");
00997 }
00998 break;
00999 default:
01000 ROS_WARN("Unknown steering command type: %u", msg->cmd_type);
01001 break;
01002 }
01003 if (msg->enable) {
01004 ptr->EN = 1;
01005 }
01006 }
01007 if (clear() || msg->clear) {
01008 ptr->CLEAR = 1;
01009 }
01010 if (msg->ignore) {
01011 ptr->IGNORE = 1;
01012 }
01013 if (msg->quiet) {
01014 ptr->QUIET = 1;
01015 }
01016 ptr->COUNT = msg->count;
01017 pub_can_.publish(out);
01018 }
01019
01020 void DbwNode::recvGearCmd(const dbw_mkz_msgs::GearCmd::ConstPtr& msg)
01021 {
01022 can_msgs::Frame out;
01023 out.id = ID_GEAR_CMD;
01024 out.is_extended = false;
01025 out.dlc = sizeof(MsgGearCmd);
01026 MsgGearCmd *ptr = (MsgGearCmd*)out.data.elems;
01027 memset(ptr, 0x00, sizeof(*ptr));
01028 if (enabled()) {
01029 ptr->GCMD = msg->cmd.gear;
01030 }
01031 if (clear() || msg->clear) {
01032 ptr->CLEAR = 1;
01033 }
01034 pub_can_.publish(out);
01035 }
01036
01037 void DbwNode::recvTurnSignalCmd(const dbw_mkz_msgs::TurnSignalCmd::ConstPtr& msg)
01038 {
01039 can_msgs::Frame out;
01040 out.id = ID_MISC_CMD;
01041 out.is_extended = false;
01042 out.dlc = sizeof(MsgTurnSignalCmd);
01043 MsgTurnSignalCmd *ptr = (MsgTurnSignalCmd*)out.data.elems;
01044 memset(ptr, 0x00, sizeof(*ptr));
01045 if (enabled()) {
01046 ptr->TRNCMD = msg->cmd.value;
01047 }
01048 pub_can_.publish(out);
01049 }
01050
01051 bool DbwNode::publishDbwEnabled()
01052 {
01053 bool change = false;
01054 bool en = enabled();
01055 if (prev_enable_ != en) {
01056 std_msgs::Bool msg;
01057 msg.data = en;
01058 pub_sys_enable_.publish(msg);
01059 change = true;
01060 }
01061 prev_enable_ = en;
01062 return change;
01063 }
01064
01065 void DbwNode::timerCallback(const ros::TimerEvent& event)
01066 {
01067 if (clear()) {
01068 can_msgs::Frame out;
01069 out.is_extended = false;
01070
01071 if (override_brake_) {
01072 out.id = ID_BRAKE_CMD;
01073 out.dlc = 4;
01074 memset(out.data.elems, 0x00, 8);
01075 ((MsgBrakeCmd*)out.data.elems)->CLEAR = 1;
01076 pub_can_.publish(out);
01077 }
01078
01079 if (override_throttle_) {
01080 out.id = ID_THROTTLE_CMD;
01081 out.dlc = 4;
01082 memset(out.data.elems, 0x00, 8);
01083 ((MsgThrottleCmd*)out.data.elems)->CLEAR = 1;
01084 pub_can_.publish(out);
01085 }
01086
01087 if (override_steering_) {
01088 out.id = ID_STEERING_CMD;
01089 out.dlc = 4;
01090 memset(out.data.elems, 0x00, 8);
01091 ((MsgSteeringCmd*)out.data.elems)->CLEAR = 1;
01092 pub_can_.publish(out);
01093 }
01094
01095 if (override_gear_) {
01096 out.id = ID_GEAR_CMD;
01097 out.dlc = sizeof(MsgGearCmd);
01098 memset(out.data.elems, 0x00, 8);
01099 ((MsgGearCmd*)out.data.elems)->CLEAR = 1;
01100 pub_can_.publish(out);
01101 }
01102 }
01103 }
01104
01105 void DbwNode::enableSystem()
01106 {
01107 if (!enable_) {
01108 if (fault()) {
01109 if (fault_steering_cal_) {
01110 ROS_WARN("DBW system not enabled. Steering calibration fault.");
01111 }
01112 if (fault_brakes_) {
01113 ROS_WARN("DBW system not enabled. Braking fault.");
01114 }
01115 if (fault_throttle_) {
01116 ROS_WARN("DBW system not enabled. Throttle fault.");
01117 }
01118 if (fault_steering_) {
01119 ROS_WARN("DBW system not enabled. Steering fault.");
01120 }
01121 if (fault_watchdog_) {
01122 ROS_WARN("DBW system not enabled. Watchdog fault.");
01123 }
01124 } else {
01125 enable_ = true;
01126 if (publishDbwEnabled()) {
01127 ROS_INFO("DBW system enabled.");
01128 } else {
01129 ROS_INFO("DBW system enable requested. Waiting for ready.");
01130 }
01131 }
01132 }
01133 }
01134
01135 void DbwNode::disableSystem()
01136 {
01137 if (enable_) {
01138 enable_ = false;
01139 publishDbwEnabled();
01140 ROS_WARN("DBW system disabled.");
01141 }
01142 }
01143
01144 void DbwNode::buttonCancel()
01145 {
01146 if (enable_) {
01147 enable_ = false;
01148 publishDbwEnabled();
01149 ROS_WARN("DBW system disabled. Cancel button pressed.");
01150 }
01151 }
01152
01153 void DbwNode::overrideBrake(bool override, bool timeout)
01154 {
01155 bool en = enabled();
01156 if (en && timeout) {
01157 override = false;
01158 }
01159 if (en && override) {
01160 enable_ = false;
01161 }
01162 override_brake_ = override;
01163 if (publishDbwEnabled()) {
01164 if (en) {
01165 ROS_WARN("DBW system disabled. Driver override on brake/throttle pedal.");
01166 } else {
01167 ROS_INFO("DBW system enabled.");
01168 }
01169 }
01170 }
01171
01172 void DbwNode::overrideThrottle(bool override, bool timeout)
01173 {
01174 bool en = enabled();
01175 if (en && timeout) {
01176 override = false;
01177 }
01178 if (en && override) {
01179 enable_ = false;
01180 }
01181 override_throttle_ = override;
01182 if (publishDbwEnabled()) {
01183 if (en) {
01184 ROS_WARN("DBW system disabled. Driver override on brake/throttle pedal.");
01185 } else {
01186 ROS_INFO("DBW system enabled.");
01187 }
01188 }
01189 }
01190
01191 void DbwNode::overrideSteering(bool override, bool timeout)
01192 {
01193 bool en = enabled();
01194 if (en && timeout) {
01195 override = false;
01196 }
01197 if (en && override) {
01198 enable_ = false;
01199 }
01200 override_steering_ = override;
01201 if (publishDbwEnabled()) {
01202 if (en) {
01203 ROS_WARN("DBW system disabled. Driver override on steering wheel.");
01204 } else {
01205 ROS_INFO("DBW system enabled.");
01206 }
01207 }
01208 }
01209
01210 void DbwNode::overrideGear(bool override)
01211 {
01212 bool en = enabled();
01213 if (en && override) {
01214 enable_ = false;
01215 }
01216 override_gear_ = override;
01217 if (publishDbwEnabled()) {
01218 if (en) {
01219 ROS_WARN("DBW system disabled. Driver override on shifter.");
01220 } else {
01221 ROS_INFO("DBW system enabled.");
01222 }
01223 }
01224 }
01225
01226 void DbwNode::timeoutBrake(bool timeout, bool enabled)
01227 {
01228 if (!timeout_brakes_ && enabled_brakes_ && timeout && !enabled) {
01229 ROS_WARN("Brake subsystem disabled after 100ms command timeout");
01230 }
01231 timeout_brakes_ = timeout;
01232 enabled_brakes_ = enabled;
01233 }
01234
01235 void DbwNode::timeoutThrottle(bool timeout, bool enabled)
01236 {
01237 if (!timeout_throttle_ && enabled_throttle_ && timeout && !enabled) {
01238 ROS_WARN("Throttle subsystem disabled after 100ms command timeout");
01239 }
01240 timeout_throttle_ = timeout;
01241 enabled_throttle_ = enabled;
01242 }
01243
01244 void DbwNode::timeoutSteering(bool timeout, bool enabled)
01245 {
01246 if (!timeout_steering_ && enabled_steering_ && timeout && !enabled) {
01247 ROS_WARN("Steering subsystem disabled after 100ms command timeout");
01248 }
01249 timeout_steering_ = timeout;
01250 enabled_steering_ = enabled;
01251 }
01252
01253 void DbwNode::faultBrakes(bool fault)
01254 {
01255 bool en = enabled();
01256 if (fault && en) {
01257 enable_ = false;
01258 }
01259 fault_brakes_ = fault;
01260 if (publishDbwEnabled()) {
01261 if (en) {
01262 ROS_ERROR("DBW system disabled. Braking fault.");
01263 } else {
01264 ROS_INFO("DBW system enabled.");
01265 }
01266 }
01267 }
01268
01269 void DbwNode::faultThrottle(bool fault)
01270 {
01271 bool en = enabled();
01272 if (fault && en) {
01273 enable_ = false;
01274 }
01275 fault_throttle_ = fault;
01276 if (publishDbwEnabled()) {
01277 if (en) {
01278 ROS_ERROR("DBW system disabled. Throttle fault.");
01279 } else {
01280 ROS_INFO("DBW system enabled.");
01281 }
01282 }
01283 }
01284
01285 void DbwNode::faultSteering(bool fault)
01286 {
01287 bool en = enabled();
01288 if (fault && en) {
01289 enable_ = false;
01290 }
01291 fault_steering_ = fault;
01292 if (publishDbwEnabled()) {
01293 if (en) {
01294 ROS_ERROR("DBW system disabled. Steering fault.");
01295 } else {
01296 ROS_INFO("DBW system enabled.");
01297 }
01298 }
01299 }
01300
01301 void DbwNode::faultSteeringCal(bool fault)
01302 {
01303 bool en = enabled();
01304 if (fault && en) {
01305 enable_ = false;
01306 }
01307 fault_steering_cal_ = fault;
01308 if (publishDbwEnabled()) {
01309 if (en) {
01310 ROS_ERROR("DBW system disabled. Steering calibration fault.");
01311 } else {
01312 ROS_INFO("DBW system enabled.");
01313 }
01314 }
01315 }
01316
01317 void DbwNode::faultWatchdog(bool fault, uint8_t src, bool braking)
01318 {
01319 bool en = enabled();
01320 if (fault && en) {
01321 enable_ = false;
01322 }
01323 fault_watchdog_ = fault;
01324 if (publishDbwEnabled()) {
01325 if (en) {
01326 ROS_ERROR("DBW system disabled. Watchdog fault.");
01327 } else {
01328 ROS_INFO("DBW system enabled.");
01329 }
01330 }
01331 if (braking && !fault_watchdog_using_brakes_) {
01332 ROS_WARN("Watchdog event: Alerting driver and applying brakes.");
01333 } else if (!braking && fault_watchdog_using_brakes_) {
01334 ROS_INFO("Watchdog event: Driver has successfully taken control.");
01335 }
01336 if (fault && src && !fault_watchdog_warned_) {
01337 switch (src) {
01338 case dbw_mkz_msgs::WatchdogCounter::OTHER_BRAKE:
01339 ROS_WARN("Watchdog event: Fault determined by brake controller");
01340 break;
01341 case dbw_mkz_msgs::WatchdogCounter::OTHER_THROTTLE:
01342 ROS_WARN("Watchdog event: Fault determined by throttle controller");
01343 break;
01344 case dbw_mkz_msgs::WatchdogCounter::OTHER_STEERING:
01345 ROS_WARN("Watchdog event: Fault determined by steering controller");
01346 break;
01347 case dbw_mkz_msgs::WatchdogCounter::BRAKE_COUNTER:
01348 ROS_WARN("Watchdog event: Brake command counter failed to increment");
01349 break;
01350 case dbw_mkz_msgs::WatchdogCounter::BRAKE_DISABLED:
01351 ROS_WARN("Watchdog event: Brake transition to disabled while in gear or moving");
01352 break;
01353 case dbw_mkz_msgs::WatchdogCounter::BRAKE_COMMAND:
01354 ROS_WARN("Watchdog event: Brake command timeout after 100ms");
01355 break;
01356 case dbw_mkz_msgs::WatchdogCounter::BRAKE_REPORT:
01357 ROS_WARN("Watchdog event: Brake report timeout after 100ms");
01358 break;
01359 case dbw_mkz_msgs::WatchdogCounter::THROTTLE_COUNTER:
01360 ROS_WARN("Watchdog event: Throttle command counter failed to increment");
01361 break;
01362 case dbw_mkz_msgs::WatchdogCounter::THROTTLE_DISABLED:
01363 ROS_WARN("Watchdog event: Throttle transition to disabled while in gear or moving");
01364 break;
01365 case dbw_mkz_msgs::WatchdogCounter::THROTTLE_COMMAND:
01366 ROS_WARN("Watchdog event: Throttle command timeout after 100ms");
01367 break;
01368 case dbw_mkz_msgs::WatchdogCounter::THROTTLE_REPORT:
01369 ROS_WARN("Watchdog event: Throttle report timeout after 100ms");
01370 break;
01371 case dbw_mkz_msgs::WatchdogCounter::STEERING_COUNTER:
01372 ROS_WARN("Watchdog event: Steering command counter failed to increment");
01373 break;
01374 case dbw_mkz_msgs::WatchdogCounter::STEERING_DISABLED:
01375 ROS_WARN("Watchdog event: Steering transition to disabled while in gear or moving");
01376 break;
01377 case dbw_mkz_msgs::WatchdogCounter::STEERING_COMMAND:
01378 ROS_WARN("Watchdog event: Steering command timeout after 100ms");
01379 break;
01380 case dbw_mkz_msgs::WatchdogCounter::STEERING_REPORT:
01381 ROS_WARN("Watchdog event: Steering report timeout after 100ms");
01382 break;
01383 }
01384 fault_watchdog_warned_ = true;
01385 } else if (!fault) {
01386 fault_watchdog_warned_ = false;
01387 }
01388 fault_watchdog_using_brakes_ = braking;
01389 if (fault && !fault_watchdog_using_brakes_ && fault_watchdog_warned_) {
01390 ROS_WARN_THROTTLE(2.0, "Watchdog event: Press left OK button on the steering wheel or cycle power to clear event.");
01391 }
01392 }
01393
01394 void DbwNode::faultWatchdog(bool fault, uint8_t src) {
01395 faultWatchdog(fault, src, fault_watchdog_using_brakes_);
01396 }
01397
01398 void DbwNode::publishJointStates(const ros::Time &stamp, const dbw_mkz_msgs::WheelSpeedReport *wheels, const dbw_mkz_msgs::SteeringReport *steering)
01399 {
01400 double dt = (stamp - joint_state_.header.stamp).toSec();
01401 if (wheels) {
01402 joint_state_.velocity[JOINT_FL] = wheels->front_left;
01403 joint_state_.velocity[JOINT_FR] = wheels->front_right;
01404 joint_state_.velocity[JOINT_RL] = wheels->rear_left;
01405 joint_state_.velocity[JOINT_RR] = wheels->rear_right;
01406 }
01407 if (steering) {
01408 const double L = acker_wheelbase_;
01409 const double W = acker_track_;
01410 const double r = L / tan(steering->steering_wheel_angle / steering_ratio_);
01411 joint_state_.position[JOINT_SL] = atan(L / (r - W/2));
01412 joint_state_.position[JOINT_SR] = atan(L / (r + W/2));
01413 }
01414 if (dt < 0.5) {
01415 for (unsigned int i = JOINT_FL; i <= JOINT_RR; i++) {
01416 joint_state_.position[i] = fmod(joint_state_.position[i] + dt * joint_state_.velocity[i], 2*M_PI);
01417 }
01418 }
01419 joint_state_.header.stamp = stamp;
01420 pub_joint_states_.publish(joint_state_);
01421 }
01422
01423 }
01424