startup_checks.cpp
Go to the documentation of this file.
1 /*
2 * Unpublished Copyright (c) 2009-2018 AutonomouStuff, LLC, All Rights Reserved.
3 *
4 * This file is part of the PACMod ROS 1.0 driver which is released under the MIT license.
5 * See file LICENSE included with this software or go to https://opensource.org/licenses/MIT for full license details.
6 */
7 /*
8  * Series of startup checks for the pacmod game control node.
9  */
10 
11 #include "startup_checks.h"
12 
13 // Check which steering stick we should use on the joypad
14 bool AS::Joystick::check_steering_stick_left_right(ros::NodeHandle * nodeH)
15 {
16  std::string steering_stick_string;
17  bool exit = false;
18 
19  if (nodeH->getParam("steering_stick", steering_stick_string))
20  {
21  ROS_INFO("Got steering_stick: %s", steering_stick_string.c_str());
22 
23  if (steering_stick_string == "LEFT")
24  {
25  PublishControl::steering_axis = LEFT_STICK_LR;
26  }
27  else if (steering_stick_string == "RIGHT")
28  {
29  PublishControl::steering_axis = RIGHT_STICK_LR;
30  }
31  else
32  {
33  ROS_ERROR("steering_stick is invalid. Exiting.");
34  exit = true;
35  }
36  }
37  else
38  {
39  ROS_ERROR("Parameter steering_stick is missing. Exiting.");
40  exit = true;
41  }
42 
43  return exit;
44 }
45 
46 bool AS::Joystick::check_vehicle_type(ros::NodeHandle * nodeH)
47 {
48  bool exit = false;
49  std::string vehicle_type_string;
50  int vehicle_type = -1;
51 
52  if (nodeH->getParam("pacmod_vehicle_type", vehicle_type_string))
53  {
54  ROS_INFO("Got pacmod_vehicle_type: %s", vehicle_type_string.c_str());
55 
56  if(vehicle_type_string == "POLARIS_GEM")
57  vehicle_type = POLARIS_GEM;
58  else if(vehicle_type_string == "POLARIS_RANGER")
59  vehicle_type = POLARIS_RANGER;
60  else if(vehicle_type_string == "LEXUS_RX_450H")
61  vehicle_type = LEXUS_RX_450H;
62  else if(vehicle_type_string == "INTERNATIONAL_PROSTAR_122")
63  vehicle_type = INTERNATIONAL_PROSTAR;
64  else if(vehicle_type_string == "VEHICLE_4")
65  vehicle_type = VEHICLE_4;
66  else if(vehicle_type_string == "VEHICLE_5")
67  vehicle_type = VEHICLE_5;
68  else if(vehicle_type_string == "VEHICLE_6")
69  vehicle_type = VEHICLE_6;
70  else
71  {
72  ROS_ERROR("pacmod_vehicle_type is invalid");
73  exit = true;
74  }
75  }
76  else
77  {
78  ROS_ERROR("Parameter pacmod_vehicle_type is missing. Exiting.");
79  exit = true;
80  }
81 
82  if (vehicle_type == LEXUS_RX_450H)
83  PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE2;
84  else if (vehicle_type == VEHICLE_4)
85  PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE4;
86  else if (vehicle_type == VEHICLE_5)
87  PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE5;
88  else if (vehicle_type == VEHICLE_6)
89  PublishControl::max_rot_rad = MAX_ROT_RAD_VEHICLE6;
90 
91  PublishControl::vehicle_type = vehicle_type;
92 
93  return exit;
94 }
95 
96 bool AS::Joystick::check_controller_type(ros::NodeHandle * nodeH)
97 {
98  std::string controller_string;
99  bool exit = false;
100 
101  if (nodeH->getParam("controller_type", controller_string))
102  {
103  ROS_INFO("Got controller_type: %s", controller_string.c_str());
104 
105  if (controller_string == "LOGITECH_F310" || controller_string == "XBOX_ONE")
106  {
107  PublishControl::controller = (controller_string == "LOGITECH_F310") ? LOGITECH_F310 : XBOX_ONE;
108 
109  PublishControl::axes[LEFT_STICK_LR] = 0;
110  PublishControl::axes[LEFT_STICK_UD] = 1;
111  PublishControl::axes[LEFT_TRIGGER_AXIS] = 2;
112  PublishControl::axes[RIGHT_STICK_LR] = 3;
113  PublishControl::axes[RIGHT_STICK_UD] = 4;
114  PublishControl::axes[RIGHT_TRIGGER_AXIS] = 5;
115  PublishControl::axes[DPAD_LR] = 6;
116  PublishControl::axes[DPAD_UD] = 7;
117 
118  PublishControl::btns[BOTTOM_BTN] = 0;
119  PublishControl::btns[RIGHT_BTN] = 1;
120  PublishControl::btns[LEFT_BTN] = 2;
121  PublishControl::btns[TOP_BTN] = 3;
122  PublishControl::btns[LEFT_BUMPER] = 4;
123  PublishControl::btns[RIGHT_BUMPER] = 5;
124  PublishControl::btns[BACK_SELECT_MINUS] = 6;
125  PublishControl::btns[START_PLUS] = 7;
126  PublishControl::btns[LEFT_STICK_PUSH] = 9;
127  PublishControl::btns[RIGHT_STICK_PUSH] = 10;
128  }
129  else if (controller_string == "HRI_SAFE_REMOTE")
130  {
131  PublishControl::controller = HRI_SAFE_REMOTE;
132 
133  // TODO: Complete missing buttons
134  PublishControl::axes[LEFT_STICK_LR] = 0;
135  PublishControl::axes[RIGHT_STICK_LR] = 3;
136  PublishControl::axes[RIGHT_STICK_UD] = 4;
137  PublishControl::axes[DPAD_LR] = 6;
138  PublishControl::axes[DPAD_UD] = 7;
139 
140  PublishControl::btns[BOTTOM_BTN] = 0;
141  PublishControl::btns[RIGHT_BTN] = 1;
142  PublishControl::btns[TOP_BTN] = 2;
143  PublishControl::btns[LEFT_BTN] = 3;
144  }
145  else if (controller_string == "LOGITECH_G29")
146  {
147  PublishControl::controller = LOGITECH_G29;
148 
149  // Set to match the G29 controller's max center-to-lock steering angle (radians).
150  PublishControl::max_rot_rad = 7.85;
151 
152 
153 
154 
155  // steering wheel, not right stick
156  PublishControl::axes[RIGHT_STICK_LR] = 0;
157  // throttle pedal, not right trigger
158  PublishControl::axes[RIGHT_TRIGGER_AXIS] = 2;
159  // brake pedal, not left trigger
160  PublishControl::axes[LEFT_TRIGGER_AXIS] = 3;
161  PublishControl::axes[DPAD_LR] = 4;
162  PublishControl::axes[DPAD_UD] = 5;
163 
164  PublishControl::btns[BOTTOM_BTN] = 0;
165  PublishControl::btns[RIGHT_BTN] = 2;
166  PublishControl::btns[LEFT_BTN] = 1;
167  PublishControl::btns[TOP_BTN] = 3;
168 
169  // Following two are two blue buttons on the left
170  PublishControl::btns[LEFT_BUMPER] = 7;
171  PublishControl::btns[BACK_SELECT_MINUS] = 11;
172  // Following two are two blue buttons on the right
173  PublishControl::btns[RIGHT_BUMPER] = 6;
174  PublishControl::btns[START_PLUS] = 10;
175 
176  }
177  else if (controller_string == "NINTENDO_SWITCH_WIRED_PLUS")
178  {
179  PublishControl::controller = NINTENDO_SWITCH_WIRED_PLUS;
180 
181  PublishControl::axes[LEFT_STICK_LR] = 0;
182  PublishControl::axes[LEFT_STICK_UD] = 1;
183  PublishControl::axes[RIGHT_STICK_LR] = 2;
184  PublishControl::axes[RIGHT_STICK_UD] = 3;
185  PublishControl::axes[DPAD_LR] = 4;
186  PublishControl::axes[DPAD_UD] = 5;
187 
188  PublishControl::btns[LEFT_BTN] = 0;
189  PublishControl::btns[BOTTOM_BTN] = 1;
190  PublishControl::btns[RIGHT_BTN] = 2;
191  PublishControl::btns[TOP_BTN] = 3;
192  PublishControl::btns[LEFT_BUMPER] = 4;
193  PublishControl::btns[RIGHT_BUMPER] = 5;
194  PublishControl::btns[LEFT_TRIGGER_BTN] = 6;
195  PublishControl::btns[RIGHT_TRIGGER_BTN] = 7;
196  PublishControl::btns[BACK_SELECT_MINUS] = 8;
197  PublishControl::btns[START_PLUS] = 9;
198  PublishControl::btns[LEFT_STICK_PUSH] = 10;
199  PublishControl::btns[RIGHT_STICK_PUSH] = 11;
200  }
201  else
202  {
203  ROS_ERROR("Provided controller_type is invalid. Exiting.");
204  exit = true;
205  }
206  }
207  else
208  {
209  ROS_ERROR("Parameter controller_type is missing. Exiting.");
210  exit = true;
211  }
212 
213  return exit;
214 }
215 
216 bool AS::Joystick::check_scale_values(ros::NodeHandle * nodeH)
217 {
218  bool exit = false;
219 
220  if (nodeH->getParam("steering_max_speed", PublishControl::steering_max_speed))
221  {
222  ROS_INFO("Got steering_max_speed: %f", PublishControl::steering_max_speed);
223 
224  if (PublishControl::steering_max_speed <= 0)
225  {
226  ROS_ERROR("Parameter steering_max_speed is invalid. Exiting.");
227  exit = true;
228  }
229  }
230  else
231  {
232  ROS_ERROR("Parameter steering_max_speed_scale_val is missing. Exiting.");
233  exit = true;
234  }
235 
236  if (nodeH->getParam("max_veh_speed", PublishControl::max_veh_speed))
237  {
238  ROS_INFO("Got max_veh_speed: %f", PublishControl::max_veh_speed);
239 
240  if (PublishControl::max_veh_speed <= 0)
241  {
242  ROS_ERROR("Parameter max_veh_speed is invalid. Exiting.");
243  exit = true;
244  }
245  }
246  else
247  {
248  ROS_ERROR("Parameter max_veh_speed is missing. Exiting.");
249  exit = true;
250  }
251 
252  if (nodeH->getParam("accel_scale_val", PublishControl::accel_scale_val))
253  {
254  ROS_INFO("Got accel_scale_val: %f", PublishControl::accel_scale_val);
255 
256  if (PublishControl::accel_scale_val <= 0 ||
257  PublishControl::accel_scale_val > 1.0)
258  {
259  ROS_ERROR("Parameter accel_scale_val is invalid. Exiting.");
260  exit = true;
261  }
262  }
263  else
264  {
265  ROS_ERROR("Parameter accel_scale_val is missing. Exiting.");
266  exit = true;
267  }
268 
269  if (nodeH->getParam("brake_scale_val", PublishControl::brake_scale_val))
270  {
271  ROS_INFO("Got brake_scale_val: %f", PublishControl::brake_scale_val);
272 
273  if (PublishControl::brake_scale_val <= 0 ||
274  PublishControl::brake_scale_val > 1.0)
275  {
276  ROS_ERROR("Parameter brake_scale_val is invalid. Exiting.");
277  exit = true;
278  }
279  }
280  else
281  {
282  ROS_ERROR("Parameter brake_scale_val is missing. Exiting.");
283  exit = true;
284  }
285 
286  return exit;
287 }
288 
289 bool AS::Joystick::run_startup_checks_error(ros::NodeHandle * nodeH)
290 {
291  bool willExit = false;
292 
293  // Run startup checks
294  willExit = check_steering_stick_left_right(nodeH);
295  willExit = check_vehicle_type(nodeH);
296  willExit = check_controller_type(nodeH);
297  willExit = check_scale_values(nodeH);
298 
299  return willExit;
300 }
startup_checks.h
AS::Joystick::check_steering_stick_left_right
bool check_steering_stick_left_right(ros::NodeHandle *)
Definition: startup_checks.cpp:14
AS::Joystick::check_scale_values
bool check_scale_values(ros::NodeHandle *nodeH)
Definition: startup_checks.cpp:216
ros::NodeHandle
AS::Joystick::check_controller_type
bool check_controller_type(ros::NodeHandle *nodeH)
Definition: startup_checks.cpp:96
AS::Joystick::MAX_ROT_RAD_VEHICLE4
static const float MAX_ROT_RAD_VEHICLE4
Definition: globals.h:107
AS::Joystick::MAX_ROT_RAD_VEHICLE6
static const float MAX_ROT_RAD_VEHICLE6
Definition: globals.h:109
AS::Joystick::MAX_ROT_RAD_VEHICLE2
static const float MAX_ROT_RAD_VEHICLE2
Definition: globals.h:106
ROS_INFO
#define ROS_INFO(...)
AS::Joystick::run_startup_checks_error
bool run_startup_checks_error(ros::NodeHandle *nodeH)
Definition: startup_checks.cpp:289
AS::Joystick::check_vehicle_type
bool check_vehicle_type(ros::NodeHandle *nodeH)
Definition: startup_checks.cpp:46
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
AS::Joystick::MAX_ROT_RAD_VEHICLE5
static const float MAX_ROT_RAD_VEHICLE5
Definition: globals.h:108
ROS_ERROR
#define ROS_ERROR(...)

pacmod_game_control
Author(s): Joe Driscoll
autogenerated on Thu Jun 6 2019 19:19:40