publish_control_board_rev3.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 
9 
10 using namespace AS::Joystick;
11 
15 
18 {
19  // Subscribe to messages
20  enable_sub = n.subscribe("/pacmod/as_tx/enabled", 20, &PublishControl::callback_pacmod_enable);
21  shift_sub = n.subscribe("/pacmod/parsed_tx/shift_rpt", 20, &PublishControlBoardRev3::callback_shift_rpt);
22  turn_sub = n.subscribe("/pacmod/parsed_tx/turn_rpt", 20, &PublishControlBoardRev3::callback_turn_rpt);
23 
24  // Advertise published messages
25  turn_signal_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/turn_cmd", 20);
26  headlight_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/headlight_cmd", 20);
27  horn_cmd_pub = n.advertise<pacmod_msgs::SystemCmdBool>("/pacmod/as_rx/horn_cmd", 20);
28  wiper_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/wiper_cmd", 20);
29  shift_cmd_pub = n.advertise<pacmod_msgs::SystemCmdInt>("/pacmod/as_rx/shift_cmd", 20);
30  accelerator_cmd_pub = n.advertise<pacmod_msgs::SystemCmdFloat>("/pacmod/as_rx/accel_cmd", 20);
31  steering_set_position_with_speed_limit_pub = n.advertise<pacmod_msgs::SteerSystemCmd>("/pacmod/as_rx/steer_cmd", 20);
32  brake_set_position_pub = n.advertise<pacmod_msgs::SystemCmdFloat>("/pacmod/as_rx/brake_cmd", 20);
33 }
34 
35 void PublishControlBoardRev3::callback_shift_rpt(const pacmod_msgs::SystemRptInt::ConstPtr& msg)
36 {
37  shift_mutex.lock();
38  // Store the latest value read from the gear state to be sent on enable/disable
39  last_shift_cmd = msg->output;
40  shift_mutex.unlock();
41 }
42 
43 void PublishControlBoardRev3::callback_turn_rpt(const pacmod_msgs::SystemRptInt::ConstPtr& msg)
44 {
45  turn_mutex.lock();
46  // Store the latest value read from the gear state to be sent on enable/disable
47  last_turn_cmd = msg->output;
48  turn_mutex.unlock();
49 }
50 
51 void PublishControlBoardRev3::publish_steering_message(const sensor_msgs::Joy::ConstPtr& msg)
52 {
53  // Steering
54  // Axis 0 is left thumbstick, axis 3 is right. Speed in rad/sec.
55  pacmod_msgs::SteerSystemCmd steer_msg;
56 
57  steer_msg.enable = local_enable;
58  steer_msg.ignore_overrides = false;
59 
60  // If the enable flag just went to true, send an override clear
61  if (!prev_enable && local_enable)
62  steer_msg.clear_override = true;
63 
64  float range_scale;
66  range_scale = 1.0;
67  else
68  range_scale = fabs(msg->axes[axes[steering_axis]]) * (STEER_OFFSET - ROT_RANGE_SCALER_LB) + ROT_RANGE_SCALER_LB;
69 
70  float speed_scale = 1.0;
71  bool speed_valid = false;
72  float current_speed = 0.0;
73 
74  speed_mutex.lock();
75 
76  if (last_speed_rpt != NULL)
77  speed_valid = last_speed_rpt->vehicle_speed_valid;
78 
79  if (speed_valid)
80  current_speed = last_speed_rpt->vehicle_speed;
81 
82  speed_mutex.unlock();
83 
84  if (speed_valid)
85  speed_scale = STEER_OFFSET - fabs((current_speed / (max_veh_speed * STEER_SCALE_FACTOR))); //Never want to reach 0 speed scale.
86 
87  steer_msg.command = (range_scale * max_rot_rad) * msg->axes[axes[steering_axis]];
88  steer_msg.rotation_rate = steering_max_speed * speed_scale;
90 }
91 
92 void PublishControlBoardRev3::publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr& msg)
93 {
94  pacmod_msgs::SystemCmdInt turn_signal_cmd_pub_msg;
95 
96  turn_signal_cmd_pub_msg.enable = local_enable;
97  turn_signal_cmd_pub_msg.ignore_overrides = false;
98 
99  // If the enable flag just went to true, send an override clear
100  if (!prev_enable && local_enable)
101  turn_signal_cmd_pub_msg.clear_override = true;
102 
103  if (msg->axes[axes[DPAD_LR]] == AXES_MAX)
104  turn_signal_cmd_pub_msg.command = SIGNAL_LEFT;
105  else if (msg->axes[axes[DPAD_LR]] == AXES_MIN)
106  turn_signal_cmd_pub_msg.command = SIGNAL_RIGHT;
107  else if (local_enable != prev_enable)
108  {
109  if (vehicle_type == VEHICLE_6)
110  turn_signal_cmd_pub_msg.command = SIGNAL_OFF;
111  else
112  turn_signal_cmd_pub_msg.command = last_turn_cmd;
113  }
114  else
115  turn_signal_cmd_pub_msg.command = SIGNAL_OFF;
116 
117  // Hazard lights (both left and right turn signals)
119  {
120  if(msg->axes[2] < -0.5)
121  turn_signal_cmd_pub_msg.command = SIGNAL_HAZARD;
122 
123  if ((last_axes.empty() ||
124  last_axes[2] != msg->axes[2]) ||
126  turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
127  }
128  else
129  {
130  if (msg->axes[axes[DPAD_UD]] == AXES_MIN)
131  turn_signal_cmd_pub_msg.command = SIGNAL_HAZARD;
132 
133  if ((last_axes.empty() ||
134  last_axes[axes[DPAD_LR]] != msg->axes[axes[DPAD_LR]] ||
135  last_axes[axes[DPAD_UD]] != msg->axes[axes[DPAD_UD]]) ||
137  {
138  turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
139  }
140  }
141 }
142 
143 void PublishControlBoardRev3::publish_shifting_message(const sensor_msgs::Joy::ConstPtr& msg)
144 {
145  // Only shift if brake command is higher than 25%
146  if (last_brake_cmd > 0.25)
147  {
148  // Shifting: reverse
149  if (msg->buttons[btns[RIGHT_BTN]] == BUTTON_DOWN)
150  {
151  pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
152  shift_cmd_pub_msg.enable = local_enable;
153  shift_cmd_pub_msg.ignore_overrides = false;
154 
155  // If the enable flag just went to true, send an override clear
156  if (!prev_enable && local_enable)
157  shift_cmd_pub_msg.clear_override = true;
158 
159  shift_cmd_pub_msg.command = SHIFT_REVERSE;
160  shift_cmd_pub.publish(shift_cmd_pub_msg);
161  }
162 
163  // Shifting: drive/high
164  if (msg->buttons[btns[BOTTOM_BTN]] == BUTTON_DOWN)
165  {
166  pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
167  shift_cmd_pub_msg.enable = local_enable;
168  shift_cmd_pub_msg.ignore_overrides = false;
169 
170  // If the enable flag just went to true, send an override clear
171  if (!prev_enable && local_enable)
172  shift_cmd_pub_msg.clear_override = true;
173 
174  shift_cmd_pub_msg.command = SHIFT_LOW;
175  shift_cmd_pub.publish(shift_cmd_pub_msg);
176  }
177 
178  // Shifting: park
179  if (msg->buttons[btns[TOP_BTN]] == BUTTON_DOWN)
180  {
181  pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
182  shift_cmd_pub_msg.enable = local_enable;
183  shift_cmd_pub_msg.ignore_overrides = false;
184 
185  // If the enable flag just went to true, send an override clear
186  if (!prev_enable && local_enable)
187  shift_cmd_pub_msg.clear_override = true;
188 
189  shift_cmd_pub_msg.command = SHIFT_PARK;
190  shift_cmd_pub.publish(shift_cmd_pub_msg);
191  }
192 
193  // Shifting: neutral
194  if (msg->buttons[btns[LEFT_BTN]] == BUTTON_DOWN)
195  {
196  pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
197  shift_cmd_pub_msg.enable = local_enable;
198  shift_cmd_pub_msg.ignore_overrides = false;
199 
200  // If the enable flag just went to true, send an override clear
201  if (!prev_enable && local_enable)
202  shift_cmd_pub_msg.clear_override = true;
203 
204  shift_cmd_pub_msg.command = SHIFT_NEUTRAL;
205  shift_cmd_pub.publish(shift_cmd_pub_msg);
206  }
207  }
208 
209  // If only an enable/disable button was pressed
210  if (local_enable != prev_enable)
211  {
212  pacmod_msgs::SystemCmdInt shift_cmd_pub_msg;
213  shift_cmd_pub_msg.enable = local_enable;
214  shift_cmd_pub_msg.ignore_overrides = false;
215 
216  // If the enable flag just went to true, send an override clear
217  if (!prev_enable && local_enable)
218  shift_cmd_pub_msg.clear_override = true;
219 
220  shift_cmd_pub_msg.command = last_shift_cmd;
221  shift_cmd_pub.publish(shift_cmd_pub_msg);
222  }
223 }
224 
225 void PublishControlBoardRev3::publish_accelerator_message(const sensor_msgs::Joy::ConstPtr& msg)
226 {
227  pacmod_msgs::SystemCmdFloat accelerator_cmd_pub_msg;
228 
229  accelerator_cmd_pub_msg.enable = local_enable;
230  accelerator_cmd_pub_msg.ignore_overrides = false;
231 
232  // If the enable flag just went to true, send an override clear
233  if (!prev_enable && local_enable)
234  accelerator_cmd_pub_msg.clear_override = true;
235 
237  {
238  // Accelerator
239  if (msg->axes[axes[RIGHT_STICK_UD]] >= 0.0)
240  {
241  // only consider center-to-up range as accelerator motion
242  accelerator_cmd_pub_msg.command = accel_scale_val * (msg->axes[axes[RIGHT_STICK_UD]]) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
243  }
244  }
245  else if(controller == LOGITECH_G29)
246  {
247  if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
249 
251  {
252  if (vehicle_type == LEXUS_RX_450H ||
253  vehicle_type == VEHICLE_4 ||
254  vehicle_type == VEHICLE_5 ||
256  accelerator_cmd_pub_msg.command = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0));
257  else
258  accelerator_cmd_pub_msg.command = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
259  }
260  else
261  {
262  accelerator_cmd_pub_msg.command = 0;
263  }
264  }
265  else
266  {
267  if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
269 
271  {
272  if (vehicle_type == LEXUS_RX_450H ||
273  vehicle_type == VEHICLE_4 ||
274  vehicle_type == VEHICLE_5 ||
276  accelerator_cmd_pub_msg.command = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0));
277  else
278  accelerator_cmd_pub_msg.command = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
279  }
280  else
281  {
282  accelerator_cmd_pub_msg.command = 0;
283  }
284  }
285 
286  accelerator_cmd_pub.publish(accelerator_cmd_pub_msg);
287 }
288 
289 void PublishControlBoardRev3::publish_brake_message(const sensor_msgs::Joy::ConstPtr& msg)
290 {
291  pacmod_msgs::SystemCmdFloat brake_msg;
292 
293  brake_msg.enable = local_enable;
294  brake_msg.ignore_overrides = false;
295 
296  // If the enable flag just went to true, send an override clear
297  if (!prev_enable && local_enable)
298  brake_msg.clear_override = true;
299 
301  {
302  brake_msg.command = (msg->axes[axes[RIGHT_STICK_UD]] > 0.0) ? 0.0 : -(brake_scale_val * msg->axes[4]);
303  }
304  else if(controller == LOGITECH_G29)
305  {
306  if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
308 
310  {
311  brake_msg.command = ((msg->axes[axes[LEFT_TRIGGER_AXIS]] + 1.0) / 2.0) * brake_scale_val;
312  }
313  else
314  {
315  brake_msg.command = 0;
316  }
317  }
318  else
319  {
320  if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
322 
324  {
325  float brake_value = -((msg->axes[axes[LEFT_TRIGGER_AXIS]] - 1.0) / 2.0) * brake_scale_val;
327  {
328  // These constants came from playing around in excel until stuff looked good. Seems to work okay
329  brake_msg.command = fmin(pow(brake_value, 3) * 2.0F - pow(brake_value, 2) * 1.5F + brake_value * 0.625F, 1.0F);
330  }
331  else
332  {
333  brake_msg.command = brake_value;
334  }
335  }
336  else
337  {
338  brake_msg.command = 0;
339  }
340  }
341 
342  last_brake_cmd = brake_msg.command;
343 
344  brake_set_position_pub.publish(brake_msg);
345 }
346 
347 void PublishControlBoardRev3::publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr& msg)
348 {
349  if ((vehicle_type == LEXUS_RX_450H ||
350  vehicle_type == VEHICLE_5) &&
352  {
353  pacmod_msgs::SystemCmdInt headlight_cmd_pub_msg;
354  headlight_cmd_pub_msg.enable = local_enable;
355  headlight_cmd_pub_msg.ignore_overrides = false;
356 
357  // Headlights
358  if (msg->axes[axes[DPAD_UD]] == AXES_MAX)
359  {
360  if (vehicle_type == VEHICLE_5)
361  {
364  else
366  }
367  else
368  {
369  // Rotate through headlight states as button is pressed
371  {
374  }
375 
378  }
379 
380  // If the enable flag just went to true, send an override clear
381  if (!prev_enable && local_enable)
382  {
383  headlight_cmd_pub_msg.clear_override = true;
385  }
386  }
387  else
388  {
390  }
391 
392  headlight_cmd_pub_msg.command = PublishControl::headlight_state;
393  headlight_cmd_pub.publish(headlight_cmd_pub_msg);
394 
395  // Horn
396  pacmod_msgs::SystemCmdBool horn_cmd_pub_msg;
397  horn_cmd_pub_msg.enable = local_enable;
398  horn_cmd_pub_msg.ignore_overrides = false;
399 
400  // If the enable flag just went to true, send an override clear
401  if (!prev_enable && local_enable)
402  horn_cmd_pub_msg.clear_override = true;
403 
404  if (msg->buttons[btns[RIGHT_BUMPER]] == BUTTON_DOWN)
405  horn_cmd_pub_msg.command = 1;
406  else
407  horn_cmd_pub_msg.command = 0;
408 
409  horn_cmd_pub.publish(horn_cmd_pub_msg);
410  }
411 
413  {
414  pacmod_msgs::SystemCmdInt wiper_cmd_pub_msg;
415  wiper_cmd_pub_msg.enable = local_enable;
416  wiper_cmd_pub_msg.ignore_overrides = false;
417 
418  // Windshield wipers
419  if (msg->axes[7] == AXES_MAX)
420  {
421  // Rotate through wiper states as button is pressed
423 
426 
427  // If the enable flag just went to true, send an override clear
428  if (!prev_enable && local_enable)
429  {
430  wiper_cmd_pub_msg.clear_override = true;
432  }
433 
434  wiper_cmd_pub_msg.command = PublishControl::wiper_state;
435  }
436 
437  wiper_cmd_pub.publish(wiper_cmd_pub_msg);
438  }
439 }
static GamepadType controller
static const float ROT_RANGE_SCALER_LB
Definition: globals.h:101
void publish(const boost::shared_ptr< M > &message) const
static pacmod_msgs::VehicleSpeedRpt::ConstPtr last_speed_rpt
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
static const float AXES_MIN
Definition: globals.h:111
void publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr &msg)
static const float AXES_MAX
Definition: globals.h:112
void publish_accelerator_message(const sensor_msgs::Joy::ConstPtr &msg)
static std::mutex speed_mutex
Definition: globals.h:122
static const float ACCEL_OFFSET
Definition: globals.h:103
static const uint16_t BUTTON_DOWN
Definition: globals.h:118
static void callback_pacmod_enable(const std_msgs::Bool::ConstPtr &msg)
static const float STEER_OFFSET
Definition: globals.h:105
static std::mutex turn_mutex
Definition: globals.h:125
static std::unordered_map< JoyButton, int, EnumHash > btns
ros::Publisher steering_set_position_with_speed_limit_pub
static const float ACCEL_SCALE_FACTOR
Definition: globals.h:102
std::vector< float > last_axes
void publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr &msg)
void publish_steering_message(const sensor_msgs::Joy::ConstPtr &msg)
static const uint16_t WIPER_STATE_START_VALUE
Definition: globals.h:114
ros::Publisher accelerator_cmd_pub
static std::mutex shift_mutex
Definition: globals.h:124
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Publisher brake_set_position_pub
ros::Publisher turn_signal_cmd_pub
static void callback_turn_rpt(const pacmod_msgs::SystemRptInt::ConstPtr &msg)
static void callback_shift_rpt(const pacmod_msgs::SystemRptInt::ConstPtr &msg)
static const uint16_t HEADLIGHT_STATE_START_VALUE
Definition: globals.h:116
static const uint16_t NUM_HEADLIGHT_STATES
Definition: globals.h:115
static std::unordered_map< JoyAxis, int, EnumHash > axes
void publish_shifting_message(const sensor_msgs::Joy::ConstPtr &msg)
void publish_brake_message(const sensor_msgs::Joy::ConstPtr &msg)
static const float STEER_SCALE_FACTOR
Definition: globals.h:104
static const uint16_t NUM_WIPER_STATES
Definition: globals.h:113


pacmod_game_control
Author(s): Joe Driscoll
autogenerated on Mon Jun 10 2019 14:09:15