Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011 #include "startup_checks.h"
00012
00013
00014 bool AS::Joystick::check_steering_stick_left_right(ros::NodeHandle * nodeH)
00015 {
00016 std::string steering_stick_string;
00017 bool exit = false;
00018
00019 if (nodeH->getParam("steering_stick", steering_stick_string))
00020 {
00021 ROS_INFO("Got steering_stick: %s", steering_stick_string.c_str());
00022
00023 if (steering_stick_string == "LEFT")
00024 {
00025 PublishControl::steering_axis = LEFT_STICK_LR;
00026 }
00027 else if (steering_stick_string == "RIGHT")
00028 {
00029 PublishControl::steering_axis = RIGHT_STICK_LR;
00030 }
00031 else
00032 {
00033 ROS_ERROR("steering_stick is invalid. Exiting.");
00034 exit = true;
00035 }
00036 }
00037 else
00038 {
00039 ROS_ERROR("Parameter steering_stick is missing. Exiting.");
00040 exit = true;
00041 }
00042
00043 return exit;
00044 }
00045
00046 bool AS::Joystick::check_vehicle_type(ros::NodeHandle * nodeH)
00047 {
00048 bool exit = false;
00049 std::string vehicle_type_string;
00050 int vehicle_type = -1;
00051
00052 if (nodeH->getParam("pacmod_vehicle_type", vehicle_type_string))
00053 {
00054 ROS_INFO("Got pacmod_vehicle_type: %s", vehicle_type_string.c_str());
00055
00056 if(vehicle_type_string == "POLARIS_GEM")
00057 vehicle_type = POLARIS_GEM;
00058 else if(vehicle_type_string == "POLARIS_RANGER")
00059 vehicle_type = POLARIS_RANGER;
00060 else if(vehicle_type_string == "LEXUS_RX_450H")
00061 vehicle_type = LEXUS_RX_450H;
00062 else if(vehicle_type_string == "INTERNATIONAL_PROSTAR_122")
00063 vehicle_type = INTERNATIONAL_PROSTAR;
00064 else if(vehicle_type_string == "VEHICLE_4")
00065 vehicle_type = VEHICLE_4;
00066 else if(vehicle_type_string == "VEHICLE_5")
00067 vehicle_type = VEHICLE_5;
00068 else if(vehicle_type_string == "VEHICLE_6")
00069 vehicle_type = VEHICLE_6;
00070 else
00071 {
00072 ROS_ERROR("pacmod_vehicle_type is invalid");
00073 exit = true;
00074 }
00075 }
00076 else
00077 {
00078 ROS_ERROR("Parameter pacmod_vehicle_type is missing. Exiting.");
00079 exit = true;
00080 }
00081
00082 if (vehicle_type == LEXUS_RX_450H)
00083 PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE2;
00084 else if (vehicle_type == VEHICLE_4)
00085 PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE4;
00086 else if (vehicle_type == VEHICLE_5)
00087 PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE5;
00088 else if (vehicle_type == VEHICLE_6)
00089 PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE6;
00090
00091 PublishControl::vehicle_type = vehicle_type;
00092
00093 return exit;
00094 }
00095
00096 bool AS::Joystick::check_controller_type(ros::NodeHandle * nodeH)
00097 {
00098 std::string controller_string;
00099 bool exit = false;
00100
00101 if (nodeH->getParam("controller_type", controller_string))
00102 {
00103 ROS_INFO("Got controller_type: %s", controller_string.c_str());
00104
00105 if (controller_string == "LOGITECH_F310" || controller_string == "XBOX_ONE")
00106 {
00107 PublishControl::controller = (controller_string == "LOGITECH_F310") ? LOGITECH_F310 : XBOX_ONE;
00108
00109 PublishControl::axes[LEFT_STICK_LR] = 0;
00110 PublishControl::axes[LEFT_STICK_UD] = 1;
00111 PublishControl::axes[LEFT_TRIGGER_AXIS] = 2;
00112 PublishControl::axes[RIGHT_STICK_LR] = 3;
00113 PublishControl::axes[RIGHT_STICK_UD] = 4;
00114 PublishControl::axes[RIGHT_TRIGGER_AXIS] = 5;
00115 PublishControl::axes[DPAD_LR] = 6;
00116 PublishControl::axes[DPAD_UD] = 7;
00117
00118 PublishControl::btns[BOTTOM_BTN] = 0;
00119 PublishControl::btns[RIGHT_BTN] = 1;
00120 PublishControl::btns[LEFT_BTN] = 2;
00121 PublishControl::btns[TOP_BTN] = 3;
00122 PublishControl::btns[LEFT_BUMPER] = 4;
00123 PublishControl::btns[RIGHT_BUMPER] = 5;
00124 PublishControl::btns[BACK_SELECT_MINUS] = 6;
00125 PublishControl::btns[START_PLUS] = 7;
00126 PublishControl::btns[LEFT_STICK_PUSH] = 9;
00127 PublishControl::btns[RIGHT_STICK_PUSH] = 10;
00128 }
00129 else if (controller_string == "HRI_SAFE_REMOTE")
00130 {
00131 PublishControl::controller = HRI_SAFE_REMOTE;
00132
00133
00134 PublishControl::axes[LEFT_STICK_LR] = 0;
00135 PublishControl::axes[RIGHT_STICK_LR] = 3;
00136 PublishControl::axes[RIGHT_STICK_UD] = 4;
00137 PublishControl::axes[DPAD_LR] = 6;
00138 PublishControl::axes[DPAD_UD] = 7;
00139
00140 PublishControl::btns[BOTTOM_BTN] = 0;
00141 PublishControl::btns[RIGHT_BTN] = 1;
00142 PublishControl::btns[TOP_BTN] = 2;
00143 PublishControl::btns[LEFT_BTN] = 3;
00144 }
00145 else if (controller_string == "LOGITECH_G29")
00146 {
00147 PublishControl::controller = LOGITECH_G29;
00148
00149
00150 PublishControl::max_rot_rad = 7.85;
00151
00152
00153
00154
00155
00156 PublishControl::axes[RIGHT_STICK_LR] = 0;
00157
00158 PublishControl::axes[RIGHT_TRIGGER_AXIS] = 2;
00159
00160 PublishControl::axes[LEFT_TRIGGER_AXIS] = 3;
00161 PublishControl::axes[DPAD_LR] = 4;
00162 PublishControl::axes[DPAD_UD] = 5;
00163
00164 PublishControl::btns[BOTTOM_BTN] = 0;
00165 PublishControl::btns[RIGHT_BTN] = 2;
00166 PublishControl::btns[LEFT_BTN] = 1;
00167 PublishControl::btns[TOP_BTN] = 3;
00168
00169
00170 PublishControl::btns[LEFT_BUMPER] = 7;
00171 PublishControl::btns[BACK_SELECT_MINUS] = 11;
00172
00173 PublishControl::btns[RIGHT_BUMPER] = 6;
00174 PublishControl::btns[START_PLUS] = 10;
00175
00176 }
00177 else if (controller_string == "NINTENDO_SWITCH_WIRED_PLUS")
00178 {
00179 PublishControl::controller = NINTENDO_SWITCH_WIRED_PLUS;
00180
00181 PublishControl::axes[LEFT_STICK_LR] = 0;
00182 PublishControl::axes[LEFT_STICK_UD] = 1;
00183 PublishControl::axes[RIGHT_STICK_LR] = 2;
00184 PublishControl::axes[RIGHT_STICK_UD] = 3;
00185 PublishControl::axes[DPAD_LR] = 4;
00186 PublishControl::axes[DPAD_UD] = 5;
00187
00188 PublishControl::btns[LEFT_BTN] = 0;
00189 PublishControl::btns[BOTTOM_BTN] = 1;
00190 PublishControl::btns[RIGHT_BTN] = 2;
00191 PublishControl::btns[TOP_BTN] = 3;
00192 PublishControl::btns[LEFT_BUMPER] = 4;
00193 PublishControl::btns[RIGHT_BUMPER] = 5;
00194 PublishControl::btns[LEFT_TRIGGER_BTN] = 6;
00195 PublishControl::btns[RIGHT_TRIGGER_BTN] = 7;
00196 PublishControl::btns[BACK_SELECT_MINUS] = 8;
00197 PublishControl::btns[START_PLUS] = 9;
00198 PublishControl::btns[LEFT_STICK_PUSH] = 10;
00199 PublishControl::btns[RIGHT_STICK_PUSH] = 11;
00200 }
00201 else
00202 {
00203 ROS_ERROR("Provided controller_type is invalid. Exiting.");
00204 exit = true;
00205 }
00206 }
00207 else
00208 {
00209 ROS_ERROR("Parameter controller_type is missing. Exiting.");
00210 exit = true;
00211 }
00212
00213 return exit;
00214 }
00215
00216 bool AS::Joystick::check_scale_values(ros::NodeHandle * nodeH)
00217 {
00218 bool exit = false;
00219
00220 if (nodeH->getParam("steering_max_speed", PublishControl::steering_max_speed))
00221 {
00222 ROS_INFO("Got steering_max_speed: %f", PublishControl::steering_max_speed);
00223
00224 if (PublishControl::steering_max_speed <= 0)
00225 {
00226 ROS_ERROR("Parameter steering_max_speed is invalid. Exiting.");
00227 exit = true;
00228 }
00229 }
00230 else
00231 {
00232 ROS_ERROR("Parameter steering_max_speed_scale_val is missing. Exiting.");
00233 exit = true;
00234 }
00235
00236 if (nodeH->getParam("max_veh_speed", PublishControl::max_veh_speed))
00237 {
00238 ROS_INFO("Got max_veh_speed: %f", PublishControl::max_veh_speed);
00239
00240 if (PublishControl::max_veh_speed <= 0)
00241 {
00242 ROS_ERROR("Parameter max_veh_speed is invalid. Exiting.");
00243 exit = true;
00244 }
00245 }
00246 else
00247 {
00248 ROS_ERROR("Parameter max_veh_speed is missing. Exiting.");
00249 exit = true;
00250 }
00251
00252 if (nodeH->getParam("accel_scale_val", PublishControl::accel_scale_val))
00253 {
00254 ROS_INFO("Got accel_scale_val: %f", PublishControl::accel_scale_val);
00255
00256 if (PublishControl::accel_scale_val <= 0 ||
00257 PublishControl::accel_scale_val > 1.0)
00258 {
00259 ROS_ERROR("Parameter accel_scale_val is invalid. Exiting.");
00260 exit = true;
00261 }
00262 }
00263 else
00264 {
00265 ROS_ERROR("Parameter accel_scale_val is missing. Exiting.");
00266 exit = true;
00267 }
00268
00269 if (nodeH->getParam("brake_scale_val", PublishControl::brake_scale_val))
00270 {
00271 ROS_INFO("Got brake_scale_val: %f", PublishControl::brake_scale_val);
00272
00273 if (PublishControl::brake_scale_val <= 0 ||
00274 PublishControl::brake_scale_val > 1.0)
00275 {
00276 ROS_ERROR("Parameter brake_scale_val is invalid. Exiting.");
00277 exit = true;
00278 }
00279 }
00280 else
00281 {
00282 ROS_ERROR("Parameter brake_scale_val is missing. Exiting.");
00283 exit = true;
00284 }
00285
00286 return exit;
00287 }
00288
00289 bool AS::Joystick::run_startup_checks_error(ros::NodeHandle * nodeH)
00290 {
00291 bool willExit = false;
00292
00293
00294 willExit = check_steering_stick_left_right(nodeH);
00295 willExit = check_vehicle_type(nodeH);
00296 willExit = check_controller_type(nodeH);
00297 willExit = check_scale_values(nodeH);
00298
00299 return willExit;
00300 }