Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008 #include "publish_control_board_rev3.h"
00009
00010 using namespace AS::Joystick;
00011
00012 int PublishControlBoardRev3::last_shift_cmd = SHIFT_NEUTRAL;
00013 int PublishControlBoardRev3::last_turn_cmd = SIGNAL_OFF;
00014 float PublishControlBoardRev3::last_brake_cmd = 0.0;
00015
00016 PublishControlBoardRev3::PublishControlBoardRev3() :
00017 PublishControl()
00018 {
00019
00020 enable_sub = n.subscribe("/pacmod/as_tx/enabled", 20, &PublishControl::callback_pacmod_enable);
00021 shift_sub = n.subscribe("/pacmod/parsed_tx/shift_rpt", 20, &PublishControlBoardRev3::callback_shift_rpt);
00022 turn_sub = n.subscribe("/pacmod/parsed_tx/turn_rpt", 20, &PublishControlBoardRev3::callback_turn_rpt);
00023
00024
00025 turn_signal_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/turn_cmd", 20);
00026 headlight_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/headlight_cmd", 20);
00027 horn_cmd_pub = n.advertise<pacmod_msgs::SystemCmdBool>("/pacmod/as_rx/horn_cmd", 20);
00028 wiper_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/wiper_cmd", 20);
00029 shift_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/shift_cmd", 20);
00030 accelerator_cmd_pub = n.advertise<pacmod_msgs::SystemCmdFloat>("/pacmod/as_rx/accel_cmd", 20);
00031 steering_set_position_with_speed_limit_pub = n.advertise<pacmod_msgs::SteerSystemCmd>("/pacmod/as_rx/steer_cmd", 20);
00032 brake_set_position_pub = n.advertise<pacmod_msgs::SystemCmdFloat>("/pacmod/as_rx/brake_cmd", 20);
00033 }
00034
00035 void PublishControlBoardRev3::callback_shift_rpt(const pacmod_msgs::SystemRptInt::ConstPtr& msg)
00036 {
00037 shift_mutex.lock();
00038
00039 last_shift_cmd = msg->output;
00040 shift_mutex.unlock();
00041 }
00042
00043 void PublishControlBoardRev3::callback_turn_rpt(const pacmod_msgs::SystemRptInt::ConstPtr& msg)
00044 {
00045 turn_mutex.lock();
00046
00047 last_turn_cmd = msg->output;
00048 turn_mutex.unlock();
00049 }
00050
00051 void PublishControlBoardRev3::publish_steering_message(const sensor_msgs::Joy::ConstPtr& msg)
00052 {
00053
00054
00055 pacmod_msgs::SteerSystemCmd steer_msg;
00056
00057 steer_msg.enable = local_enable;
00058 steer_msg.ignore_overrides = false;
00059
00060
00061 if (!prev_enable && local_enable)
00062 steer_msg.clear_override = true;
00063
00064 float range_scale;
00065 if (vehicle_type == VEHICLE_4 || vehicle_type == VEHICLE_6)
00066 range_scale = 1.0;
00067 else
00068 range_scale = fabs(msg->axes[axes[steering_axis]]) * (STEER_OFFSET - ROT_RANGE_SCALER_LB) + ROT_RANGE_SCALER_LB;
00069
00070 float speed_scale = 1.0;
00071 bool speed_valid = false;
00072 float current_speed = 0.0;
00073
00074 speed_mutex.lock();
00075
00076 if (last_speed_rpt != NULL)
00077 speed_valid = last_speed_rpt->vehicle_speed_valid;
00078
00079 if (speed_valid)
00080 current_speed = last_speed_rpt->vehicle_speed;
00081
00082 speed_mutex.unlock();
00083
00084 if (speed_valid)
00085 speed_scale = STEER_OFFSET - fabs((current_speed / (max_veh_speed * STEER_SCALE_FACTOR)));
00086
00087 steer_msg.command = (range_scale * max_rot_rad) * msg->axes[axes[steering_axis]];
00088 steer_msg.rotation_rate = steering_max_speed * speed_scale;
00089 steering_set_position_with_speed_limit_pub.publish(steer_msg);
00090 }
00091
00092 void PublishControlBoardRev3::publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr& msg)
00093 {
00094 pacmod_msgs::SystemCmdInt turn_signal_cmd_pub_msg;
00095
00096 turn_signal_cmd_pub_msg.enable = local_enable;
00097 turn_signal_cmd_pub_msg.ignore_overrides = false;
00098
00099
00100 if (!prev_enable && local_enable)
00101 turn_signal_cmd_pub_msg.clear_override = true;
00102
00103 if (msg->axes[axes[DPAD_LR]] == AXES_MAX)
00104 turn_signal_cmd_pub_msg.command = SIGNAL_LEFT;
00105 else if (msg->axes[axes[DPAD_LR]] == AXES_MIN)
00106 turn_signal_cmd_pub_msg.command = SIGNAL_RIGHT;
00107 else if (local_enable != prev_enable)
00108 {
00109 if (vehicle_type == VEHICLE_6)
00110 turn_signal_cmd_pub_msg.command = SIGNAL_OFF;
00111 else
00112 turn_signal_cmd_pub_msg.command = last_turn_cmd;
00113 }
00114 else
00115 turn_signal_cmd_pub_msg.command = SIGNAL_OFF;
00116
00117
00118 if (controller == HRI_SAFE_REMOTE)
00119 {
00120 if(msg->axes[2] < -0.5)
00121 turn_signal_cmd_pub_msg.command = SIGNAL_HAZARD;
00122
00123 if ((last_axes.empty() ||
00124 last_axes[2] != msg->axes[2]) ||
00125 (local_enable != prev_enable))
00126 turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
00127 }
00128 else
00129 {
00130 if (msg->axes[axes[DPAD_UD]] == AXES_MIN)
00131 turn_signal_cmd_pub_msg.command = SIGNAL_HAZARD;
00132
00133 if ((last_axes.empty() ||
00134 last_axes[axes[DPAD_LR]] != msg->axes[axes[DPAD_LR]] ||
00135 last_axes[axes[DPAD_UD]] != msg->axes[axes[DPAD_UD]]) ||
00136 (local_enable != prev_enable))
00137 {
00138 turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
00139 }
00140 }
00141 }
00142
00143 void PublishControlBoardRev3::publish_shifting_message(const sensor_msgs::Joy::ConstPtr& msg)
00144 {
00145
00146 if (last_brake_cmd > 0.25)
00147 {
00148
00149 if (msg->buttons[btns[RIGHT_BTN]] == BUTTON_DOWN)
00150 {
00151 pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
00152 shift_cmd_pub_msg.enable = local_enable;
00153 shift_cmd_pub_msg.ignore_overrides = false;
00154
00155
00156 if (!prev_enable && local_enable)
00157 shift_cmd_pub_msg.clear_override = true;
00158
00159 shift_cmd_pub_msg.command = SHIFT_REVERSE;
00160 shift_cmd_pub.publish(shift_cmd_pub_msg);
00161 }
00162
00163
00164 if (msg->buttons[btns[BOTTOM_BTN]] == BUTTON_DOWN)
00165 {
00166 pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
00167 shift_cmd_pub_msg.enable = local_enable;
00168 shift_cmd_pub_msg.ignore_overrides = false;
00169
00170
00171 if (!prev_enable && local_enable)
00172 shift_cmd_pub_msg.clear_override = true;
00173
00174 shift_cmd_pub_msg.command = SHIFT_LOW;
00175 shift_cmd_pub.publish(shift_cmd_pub_msg);
00176 }
00177
00178
00179 if (msg->buttons[btns[TOP_BTN]] == BUTTON_DOWN)
00180 {
00181 pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
00182 shift_cmd_pub_msg.enable = local_enable;
00183 shift_cmd_pub_msg.ignore_overrides = false;
00184
00185
00186 if (!prev_enable && local_enable)
00187 shift_cmd_pub_msg.clear_override = true;
00188
00189 shift_cmd_pub_msg.command = SHIFT_PARK;
00190 shift_cmd_pub.publish(shift_cmd_pub_msg);
00191 }
00192
00193
00194 if (msg->buttons[btns[LEFT_BTN]] == BUTTON_DOWN)
00195 {
00196 pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
00197 shift_cmd_pub_msg.enable = local_enable;
00198 shift_cmd_pub_msg.ignore_overrides = false;
00199
00200
00201 if (!prev_enable && local_enable)
00202 shift_cmd_pub_msg.clear_override = true;
00203
00204 shift_cmd_pub_msg.command = SHIFT_NEUTRAL;
00205 shift_cmd_pub.publish(shift_cmd_pub_msg);
00206 }
00207 }
00208
00209
00210 if (local_enable != prev_enable)
00211 {
00212 pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
00213 shift_cmd_pub_msg.enable = local_enable;
00214 shift_cmd_pub_msg.ignore_overrides = false;
00215
00216
00217 if (!prev_enable && local_enable)
00218 shift_cmd_pub_msg.clear_override = true;
00219
00220 shift_cmd_pub_msg.command = last_shift_cmd;
00221 shift_cmd_pub.publish(shift_cmd_pub_msg);
00222 }
00223 }
00224
00225 void PublishControlBoardRev3::publish_accelerator_message(const sensor_msgs::Joy::ConstPtr& msg)
00226 {
00227 pacmod_msgs::SystemCmdFloat accelerator_cmd_pub_msg;
00228
00229 accelerator_cmd_pub_msg.enable = local_enable;
00230 accelerator_cmd_pub_msg.ignore_overrides = false;
00231
00232
00233 if (!prev_enable && local_enable)
00234 accelerator_cmd_pub_msg.clear_override = true;
00235
00236 if (controller == HRI_SAFE_REMOTE)
00237 {
00238
00239 if (msg->axes[axes[RIGHT_STICK_UD]] >= 0.0)
00240 {
00241
00242 accelerator_cmd_pub_msg.command = accel_scale_val * (msg->axes[axes[RIGHT_STICK_UD]]) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
00243 }
00244 }
00245 else if(controller == LOGITECH_G29)
00246 {
00247 if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
00248 PublishControl::accel_0_rcvd = true;
00249
00250 if (PublishControl::accel_0_rcvd)
00251 {
00252 if (vehicle_type == LEXUS_RX_450H ||
00253 vehicle_type == VEHICLE_4 ||
00254 vehicle_type == VEHICLE_5 ||
00255 vehicle_type == VEHICLE_6)
00256 accelerator_cmd_pub_msg.command = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0));
00257 else
00258 accelerator_cmd_pub_msg.command = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
00259 }
00260 else
00261 {
00262 accelerator_cmd_pub_msg.command = 0;
00263 }
00264 }
00265 else
00266 {
00267 if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
00268 PublishControl::accel_0_rcvd = true;
00269
00270 if (PublishControl::accel_0_rcvd)
00271 {
00272 if (vehicle_type == LEXUS_RX_450H ||
00273 vehicle_type == VEHICLE_4 ||
00274 vehicle_type == VEHICLE_5 ||
00275 vehicle_type == VEHICLE_6)
00276 accelerator_cmd_pub_msg.command = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0));
00277 else
00278 accelerator_cmd_pub_msg.command = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
00279 }
00280 else
00281 {
00282 accelerator_cmd_pub_msg.command = 0;
00283 }
00284 }
00285
00286 accelerator_cmd_pub.publish(accelerator_cmd_pub_msg);
00287 }
00288
00289 void PublishControlBoardRev3::publish_brake_message(const sensor_msgs::Joy::ConstPtr& msg)
00290 {
00291 pacmod_msgs::SystemCmdFloat brake_msg;
00292
00293 brake_msg.enable = local_enable;
00294 brake_msg.ignore_overrides = false;
00295
00296
00297 if (!prev_enable && local_enable)
00298 brake_msg.clear_override = true;
00299
00300 if (controller == HRI_SAFE_REMOTE)
00301 {
00302 brake_msg.command = (msg->axes[axes[RIGHT_STICK_UD]] > 0.0) ? 0.0 : -(brake_scale_val * msg->axes[4]);
00303 }
00304 else if(controller == LOGITECH_G29)
00305 {
00306 if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
00307 PublishControl::brake_0_rcvd = true;
00308
00309 if (PublishControl::brake_0_rcvd)
00310 {
00311 brake_msg.command = ((msg->axes[axes[LEFT_TRIGGER_AXIS]] + 1.0) / 2.0) * brake_scale_val;
00312 }
00313 else
00314 {
00315 brake_msg.command = 0;
00316 }
00317 }
00318 else
00319 {
00320 if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
00321 PublishControl::brake_0_rcvd = true;
00322
00323 if (PublishControl::brake_0_rcvd)
00324 {
00325 float brake_value = -((msg->axes[axes[LEFT_TRIGGER_AXIS]] - 1.0) / 2.0) * brake_scale_val;
00326 if(vehicle_type == LEXUS_RX_450H)
00327 {
00328
00329 brake_msg.command = fmin(pow(brake_value, 3) * 2.0F - pow(brake_value, 2) * 1.5F + brake_value * 0.625F, 1.0F);
00330 }
00331 else
00332 {
00333 brake_msg.command = brake_value;
00334 }
00335 }
00336 else
00337 {
00338 brake_msg.command = 0;
00339 }
00340 }
00341
00342 last_brake_cmd = brake_msg.command;
00343
00344 brake_set_position_pub.publish(brake_msg);
00345 }
00346
00347 void PublishControlBoardRev3::publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr& msg)
00348 {
00349 if ((vehicle_type == LEXUS_RX_450H ||
00350 vehicle_type == VEHICLE_5) &&
00351 controller != HRI_SAFE_REMOTE)
00352 {
00353 pacmod_msgs::SystemCmdInt headlight_cmd_pub_msg;
00354 headlight_cmd_pub_msg.enable = local_enable;
00355 headlight_cmd_pub_msg.ignore_overrides = false;
00356
00357
00358 if (msg->axes[axes[DPAD_UD]] == AXES_MAX)
00359 {
00360 if (vehicle_type == VEHICLE_5)
00361 {
00362 if (PublishControl::headlight_state == 4)
00363 PublishControl::headlight_state = 5;
00364 else
00365 PublishControl::headlight_state = 4;
00366 }
00367 else
00368 {
00369
00370 if (!PublishControl::headlight_state_change)
00371 {
00372 PublishControl::headlight_state++;
00373 PublishControl::headlight_state_change = true;
00374 }
00375
00376 if (PublishControl::headlight_state >= NUM_HEADLIGHT_STATES)
00377 PublishControl::headlight_state = HEADLIGHT_STATE_START_VALUE;
00378 }
00379
00380
00381 if (!prev_enable && local_enable)
00382 {
00383 headlight_cmd_pub_msg.clear_override = true;
00384 PublishControl::headlight_state = HEADLIGHT_STATE_START_VALUE;
00385 }
00386 }
00387 else
00388 {
00389 PublishControl::headlight_state_change = false;
00390 }
00391
00392 headlight_cmd_pub_msg.command = PublishControl::headlight_state;
00393 headlight_cmd_pub.publish(headlight_cmd_pub_msg);
00394
00395
00396 pacmod_msgs::SystemCmdBool horn_cmd_pub_msg;
00397 horn_cmd_pub_msg.enable = local_enable;
00398 horn_cmd_pub_msg.ignore_overrides = false;
00399
00400
00401 if (!prev_enable && local_enable)
00402 horn_cmd_pub_msg.clear_override = true;
00403
00404 if (msg->buttons[btns[RIGHT_BUMPER]] == BUTTON_DOWN)
00405 horn_cmd_pub_msg.command = 1;
00406 else
00407 horn_cmd_pub_msg.command = 0;
00408
00409 horn_cmd_pub.publish(horn_cmd_pub_msg);
00410 }
00411
00412 if (vehicle_type == INTERNATIONAL_PROSTAR && controller != HRI_SAFE_REMOTE)
00413 {
00414 pacmod_msgs::SystemCmdInt wiper_cmd_pub_msg;
00415 wiper_cmd_pub_msg.enable = local_enable;
00416 wiper_cmd_pub_msg.ignore_overrides = false;
00417
00418
00419 if (msg->axes[7] == AXES_MAX)
00420 {
00421
00422 PublishControl::wiper_state++;
00423
00424 if (PublishControl::wiper_state >= NUM_WIPER_STATES)
00425 PublishControl::wiper_state = WIPER_STATE_START_VALUE;
00426
00427
00428 if (!prev_enable && local_enable)
00429 {
00430 wiper_cmd_pub_msg.clear_override = true;
00431 PublishControl::wiper_state = WIPER_STATE_START_VALUE;
00432 }
00433
00434 wiper_cmd_pub_msg.command = PublishControl::wiper_state;
00435 }
00436
00437 wiper_cmd_pub.publish(wiper_cmd_pub_msg);
00438 }
00439 }