Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include "publish_control_board_rev2.h"
00013
00014 using namespace AS::Joystick;
00015
00016 PublishControlBoardRev2::PublishControlBoardRev2() :
00017 PublishControl()
00018 {
00019
00020 enable_sub = n.subscribe("/pacmod/as_tx/enable", 20, &PublishControl::callback_pacmod_enable);
00021
00022
00023 turn_signal_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/turn_cmd", 20);
00024 headlight_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/headlight_cmd", 20);
00025 horn_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/horn_cmd", 20);
00026 wiper_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/wiper_cmd", 20);
00027 shift_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/shift_cmd", 20);
00028 accelerator_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/accel_cmd", 20);
00029 steering_set_position_with_speed_limit_pub = n.advertise<pacmod_msgs::PositionWithSpeed>("/pacmod/as_rx/steer_cmd", 20);
00030 brake_set_position_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/brake_cmd", 20);
00031 }
00032
00033 void PublishControlBoardRev2::publish_steering_message(const sensor_msgs::Joy::ConstPtr& msg)
00034 {
00035
00036
00037 pacmod_msgs::PositionWithSpeed steer_msg;
00038
00039 float range_scale = fabs(msg->axes[axes[steering_axis]]) * (STEER_OFFSET - ROT_RANGE_SCALER_LB) + ROT_RANGE_SCALER_LB;
00040 float speed_scale = 1.0;
00041 bool speed_valid = false;
00042 float current_speed = 0.0;
00043
00044 speed_mutex.lock();
00045
00046 if (last_speed_rpt != NULL)
00047 speed_valid = last_speed_rpt->vehicle_speed_valid;
00048
00049 if (speed_valid)
00050 current_speed = last_speed_rpt->vehicle_speed;
00051
00052 speed_mutex.unlock();
00053
00054 if (speed_valid)
00055 speed_scale = STEER_OFFSET - fabs((current_speed / (max_veh_speed * STEER_SCALE_FACTOR)));
00056
00057 steer_msg.angular_position = (range_scale * max_rot_rad) * msg->axes[axes[steering_axis]];
00058 steer_msg.angular_velocity_limit = steering_max_speed * speed_scale;
00059 steering_set_position_with_speed_limit_pub.publish(steer_msg);
00060 }
00061
00062 void PublishControlBoardRev2::publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr& msg)
00063 {
00064 pacmod_msgs::PacmodCmd turn_signal_cmd_pub_msg;
00065
00066 if (msg->axes[axes[DPAD_LR]] == AXES_MAX)
00067 turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_LEFT;
00068 else if (msg->axes[axes[DPAD_LR]] == AXES_MIN)
00069 turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_RIGHT;
00070 else
00071 turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_OFF;
00072
00073
00074 if (controller == HRI_SAFE_REMOTE)
00075 {
00076 if(msg->axes[2] < -0.5)
00077 turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_HAZARD;
00078
00079 if (last_axes.empty() || last_axes[2] != msg->axes[2])
00080 turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
00081 }
00082 else
00083 {
00084 if (msg->axes[axes[DPAD_UD]] == AXES_MIN)
00085 turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_HAZARD;
00086
00087 if (last_axes.empty() ||
00088 last_axes[axes[DPAD_LR]] != msg->axes[axes[DPAD_LR]] ||
00089 last_axes[axes[DPAD_UD]] != msg->axes[axes[DPAD_UD]])
00090 {
00091 turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
00092 }
00093 }
00094 }
00095
00096 void PublishControlBoardRev2::publish_shifting_message(const sensor_msgs::Joy::ConstPtr& msg)
00097 {
00098
00099 if (msg->buttons[btns[RIGHT_BTN]] == BUTTON_DOWN)
00100 {
00101 pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
00102 shift_cmd_pub_msg.ui16_cmd = SHIFT_REVERSE;
00103 shift_cmd_pub.publish(shift_cmd_pub_msg);
00104 }
00105
00106
00107 if (msg->buttons[btns[BOTTOM_BTN]] == BUTTON_DOWN)
00108 {
00109 pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
00110 shift_cmd_pub_msg.ui16_cmd = SHIFT_LOW;
00111 shift_cmd_pub.publish(shift_cmd_pub_msg);
00112 }
00113
00114
00115 if (msg->buttons[btns[TOP_BTN]] == BUTTON_DOWN)
00116 {
00117 pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
00118 shift_cmd_pub_msg.ui16_cmd = SHIFT_PARK;
00119 shift_cmd_pub.publish(shift_cmd_pub_msg);
00120 }
00121
00122
00123 if (msg->buttons[btns[LEFT_BTN]] == BUTTON_DOWN)
00124 {
00125 pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
00126 shift_cmd_pub_msg.ui16_cmd = SHIFT_NEUTRAL;
00127 shift_cmd_pub.publish(shift_cmd_pub_msg);
00128 }
00129 }
00130
00131 void PublishControlBoardRev2::publish_accelerator_message(const sensor_msgs::Joy::ConstPtr& msg)
00132 {
00133 pacmod_msgs::PacmodCmd accelerator_cmd_pub_msg;
00134
00135 if (controller == HRI_SAFE_REMOTE)
00136 {
00137
00138 if (msg->axes[axes[RIGHT_STICK_UD]] >= 0.0)
00139 {
00140
00141 accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (msg->axes[axes[RIGHT_STICK_UD]]) * ACCEL_SCALE_FACTOR
00142 + ACCEL_OFFSET;
00143 }
00144 }
00145 else if(controller == LOGITECH_G29)
00146 {
00147 if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
00148 PublishControl::accel_0_rcvd = true;
00149
00150 if (PublishControl::accel_0_rcvd)
00151 {
00152 if ((vehicle_type == LEXUS_RX_450H) ||
00153 (vehicle_type == VEHICLE_4))
00154 accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0));
00155 else
00156 accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0)) * ACCEL_SCALE_FACTOR
00157 + ACCEL_OFFSET;
00158 }
00159 else
00160 {
00161 accelerator_cmd_pub_msg.f64_cmd = 0;
00162 }
00163 }
00164 else
00165 {
00166 if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
00167 PublishControl::accel_0_rcvd = true;
00168
00169 if (PublishControl::accel_0_rcvd)
00170 {
00171 if ((vehicle_type == LEXUS_RX_450H) ||
00172 (vehicle_type == VEHICLE_4))
00173 accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0));
00174 else
00175 accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
00176 }
00177 else
00178 {
00179 accelerator_cmd_pub_msg.f64_cmd = 0;
00180 }
00181 }
00182
00183 accelerator_cmd_pub.publish(accelerator_cmd_pub_msg);
00184 }
00185
00186 void PublishControlBoardRev2::publish_brake_message(const sensor_msgs::Joy::ConstPtr& msg)
00187 {
00188 pacmod_msgs::PacmodCmd brake_msg;
00189
00190 if (controller == HRI_SAFE_REMOTE)
00191 {
00192 brake_msg.f64_cmd = (msg->axes[axes[RIGHT_STICK_UD]] > 0.0) ? 0.0 : -(brake_scale_val * msg->axes[4]);
00193 }
00194 else if(controller == LOGITECH_G29)
00195 {
00196 if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
00197 PublishControl::brake_0_rcvd = true;
00198
00199 if (PublishControl::brake_0_rcvd)
00200 brake_msg.f64_cmd = ((msg->axes[axes[LEFT_TRIGGER_AXIS]] + 1.0) / 2.0) * brake_scale_val;
00201 else
00202 brake_msg.f64_cmd = 0;
00203 }
00204 else
00205 {
00206 if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
00207 PublishControl::brake_0_rcvd = true;
00208
00209 if (PublishControl::brake_0_rcvd)
00210 brake_msg.f64_cmd = -((msg->axes[axes[LEFT_TRIGGER_AXIS]] - 1.0) / 2.0) * brake_scale_val;
00211 else
00212 brake_msg.f64_cmd = 0;
00213 }
00214
00215 brake_set_position_pub.publish(brake_msg);
00216 }
00217
00218 void PublishControlBoardRev2::publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr& msg)
00219 {
00220 static uint16_t headlight_state = 0;
00221 static uint16_t wiper_state = 0;
00222
00223 if (vehicle_type == 2 && controller != HRI_SAFE_REMOTE)
00224 {
00225
00226 if (msg->axes[axes[DPAD_UD]] == AXES_MAX)
00227 {
00228
00229 headlight_state++;
00230
00231 if(headlight_state >= NUM_HEADLIGHT_STATES)
00232 headlight_state = HEADLIGHT_STATE_START_VALUE;
00233
00234 pacmod_msgs::PacmodCmd headlight_cmd_pub_msg;
00235 headlight_cmd_pub_msg.ui16_cmd = headlight_state;
00236 headlight_cmd_pub.publish(headlight_cmd_pub_msg);
00237 }
00238
00239
00240 pacmod_msgs::PacmodCmd horn_cmd_pub_msg;
00241
00242 if (msg->buttons[btns[RIGHT_BUMPER]] == BUTTON_DOWN)
00243 horn_cmd_pub_msg.ui16_cmd = 1;
00244 else
00245 horn_cmd_pub_msg.ui16_cmd = 0;
00246
00247 horn_cmd_pub.publish(horn_cmd_pub_msg);
00248 }
00249
00250 if (vehicle_type == 3 && controller != HRI_SAFE_REMOTE)
00251 {
00252
00253 if (msg->buttons[btns[LEFT_BUMPER]] == BUTTON_DOWN)
00254 {
00255
00256 wiper_state++;
00257
00258 if(wiper_state >= NUM_WIPER_STATES)
00259 wiper_state = WIPER_STATE_START_VALUE;
00260
00261 pacmod_msgs::PacmodCmd wiper_cmd_pub_msg;
00262 wiper_cmd_pub_msg.ui16_cmd = wiper_state;
00263 wiper_cmd_pub.publish(wiper_cmd_pub_msg);
00264 }
00265 }
00266 }