publish_control_board_rev2.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 HRI joystick mappings as found by ROS Kinetic Joy node on Ubuntu 16.04
9 Last modified 2-13-2018 by Lucas Buckland
10 */
11 
13 
14 using namespace AS::Joystick;
15 
18 {
19  // Subscribe to messages
20  enable_sub = n.subscribe("/pacmod/as_tx/enable", 20, &PublishControl::callback_pacmod_enable);
21 
22  // Advertise published messages
23  turn_signal_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/turn_cmd", 20);
24  headlight_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/headlight_cmd", 20);
25  horn_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/horn_cmd", 20);
26  wiper_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/wiper_cmd", 20);
27  shift_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/shift_cmd", 20);
28  accelerator_cmd_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/accel_cmd", 20);
29  steering_set_position_with_speed_limit_pub = n.advertise<pacmod_msgs::PositionWithSpeed>("/pacmod/as_rx/steer_cmd", 20);
30  brake_set_position_pub = n.advertise<pacmod_msgs::PacmodCmd>("/pacmod/as_rx/brake_cmd", 20);
31 }
32 
33 void PublishControlBoardRev2::publish_steering_message(const sensor_msgs::Joy::ConstPtr& msg)
34 {
35  // Steering
36  // Axis 0 is left thumbstick, axis 3 is right. Speed in rad/sec.
37  pacmod_msgs::PositionWithSpeed steer_msg;
38 
39  float range_scale = fabs(msg->axes[axes[steering_axis]]) * (STEER_OFFSET - ROT_RANGE_SCALER_LB) + ROT_RANGE_SCALER_LB;
40  float speed_scale = 1.0;
41  bool speed_valid = false;
42  float current_speed = 0.0;
43 
44  speed_mutex.lock();
45 
46  if (last_speed_rpt != NULL)
47  speed_valid = last_speed_rpt->vehicle_speed_valid;
48 
49  if (speed_valid)
50  current_speed = last_speed_rpt->vehicle_speed;
51 
52  speed_mutex.unlock();
53 
54  if (speed_valid)
55  speed_scale = STEER_OFFSET - fabs((current_speed / (max_veh_speed * STEER_SCALE_FACTOR))); //Never want to reach 0 speed scale.
56 
57  steer_msg.angular_position = (range_scale * max_rot_rad) * msg->axes[axes[steering_axis]];
58  steer_msg.angular_velocity_limit = steering_max_speed * speed_scale;
60 }
61 
62 void PublishControlBoardRev2::publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr& msg)
63 {
64  pacmod_msgs::PacmodCmd turn_signal_cmd_pub_msg;
65 
66  if (msg->axes[axes[DPAD_LR]] == AXES_MAX)
67  turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_LEFT;
68  else if (msg->axes[axes[DPAD_LR]] == AXES_MIN)
69  turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_RIGHT;
70  else
71  turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_OFF;
72 
73  // Hazard lights (both left and right turn signals)
75  {
76  if(msg->axes[2] < -0.5)
77  turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_HAZARD;
78 
79  if (last_axes.empty() || last_axes[2] != msg->axes[2])
80  turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
81  }
82  else
83  {
84  if (msg->axes[axes[DPAD_UD]] == AXES_MIN)
85  turn_signal_cmd_pub_msg.ui16_cmd = SIGNAL_HAZARD;
86 
87  if (last_axes.empty() ||
88  last_axes[axes[DPAD_LR]] != msg->axes[axes[DPAD_LR]] ||
89  last_axes[axes[DPAD_UD]] != msg->axes[axes[DPAD_UD]])
90  {
91  turn_signal_cmd_pub.publish(turn_signal_cmd_pub_msg);
92  }
93  }
94 }
95 
96 void PublishControlBoardRev2::publish_shifting_message(const sensor_msgs::Joy::ConstPtr& msg)
97 {
98  // Shifting: reverse
99  if (msg->buttons[btns[RIGHT_BTN]] == BUTTON_DOWN)
100  {
101  pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
102  shift_cmd_pub_msg.ui16_cmd = SHIFT_REVERSE;
103  shift_cmd_pub.publish(shift_cmd_pub_msg);
104  }
105 
106  // Shifting: drive/high
107  if (msg->buttons[btns[BOTTOM_BTN]] == BUTTON_DOWN)
108  {
109  pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
110  shift_cmd_pub_msg.ui16_cmd = SHIFT_LOW;
111  shift_cmd_pub.publish(shift_cmd_pub_msg);
112  }
113 
114  // Shifting: park
115  if (msg->buttons[btns[TOP_BTN]] == BUTTON_DOWN)
116  {
117  pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
118  shift_cmd_pub_msg.ui16_cmd = SHIFT_PARK;
119  shift_cmd_pub.publish(shift_cmd_pub_msg);
120  }
121 
122  // Shifting: neutral
123  if (msg->buttons[btns[LEFT_BTN]] == BUTTON_DOWN)
124  {
125  pacmod_msgs::PacmodCmd shift_cmd_pub_msg;
126  shift_cmd_pub_msg.ui16_cmd = SHIFT_NEUTRAL;
127  shift_cmd_pub.publish(shift_cmd_pub_msg);
128  }
129 }
130 
131 void PublishControlBoardRev2::publish_accelerator_message(const sensor_msgs::Joy::ConstPtr& msg)
132 {
133  pacmod_msgs::PacmodCmd accelerator_cmd_pub_msg;
134 
136  {
137  // Accelerator
138  if (msg->axes[axes[RIGHT_STICK_UD]] >= 0.0)
139  {
140  // only consider center-to-up range as accelerator motion
141  accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (msg->axes[axes[RIGHT_STICK_UD]]) * ACCEL_SCALE_FACTOR
142  + ACCEL_OFFSET;
143  }
144  }
145  else if(controller == LOGITECH_G29)
146  {
147  if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
149 
151  {
152  if ((vehicle_type == LEXUS_RX_450H) ||
153  (vehicle_type == VEHICLE_4))
154  accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0));
155  else
156  accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] + 1.0)) * ACCEL_SCALE_FACTOR
157  + ACCEL_OFFSET;
158  }
159  else
160  {
161  accelerator_cmd_pub_msg.f64_cmd = 0;
162  }
163  }
164  else
165  {
166  if (msg->axes[axes[RIGHT_TRIGGER_AXIS]] != 0)
168 
170  {
171  if ((vehicle_type == LEXUS_RX_450H) ||
172  (vehicle_type == VEHICLE_4))
173  accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0));
174  else
175  accelerator_cmd_pub_msg.f64_cmd = accel_scale_val * (-0.5 * (msg->axes[axes[RIGHT_TRIGGER_AXIS]] - 1.0)) * ACCEL_SCALE_FACTOR + ACCEL_OFFSET;
176  }
177  else
178  {
179  accelerator_cmd_pub_msg.f64_cmd = 0;
180  }
181  }
182 
183  accelerator_cmd_pub.publish(accelerator_cmd_pub_msg);
184 }
185 
186 void PublishControlBoardRev2::publish_brake_message(const sensor_msgs::Joy::ConstPtr& msg)
187 {
188  pacmod_msgs::PacmodCmd brake_msg;
189 
191  {
192  brake_msg.f64_cmd = (msg->axes[axes[RIGHT_STICK_UD]] > 0.0) ? 0.0 : -(brake_scale_val * msg->axes[4]);
193  }
194  else if(controller == LOGITECH_G29)
195  {
196  if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
198 
200  brake_msg.f64_cmd = ((msg->axes[axes[LEFT_TRIGGER_AXIS]] + 1.0) / 2.0) * brake_scale_val;
201  else
202  brake_msg.f64_cmd = 0;
203  }
204  else
205  {
206  if (msg->axes[axes[LEFT_TRIGGER_AXIS]] != 0)
208 
210  brake_msg.f64_cmd = -((msg->axes[axes[LEFT_TRIGGER_AXIS]] - 1.0) / 2.0) * brake_scale_val;
211  else
212  brake_msg.f64_cmd = 0;
213  }
214 
215  brake_set_position_pub.publish(brake_msg);
216 }
217 
218 void PublishControlBoardRev2::publish_lights_horn_wipers_message(const sensor_msgs::Joy::ConstPtr& msg)
219 {
220  static uint16_t headlight_state = 0;
221  static uint16_t wiper_state = 0;
222 
223  if (vehicle_type == 2 && controller != HRI_SAFE_REMOTE)
224  {
225  // Headlights
226  if (msg->axes[axes[DPAD_UD]] == AXES_MAX)
227  {
228  // Rotate through headlight states as button is pressed
229  headlight_state++;
230 
231  if(headlight_state >= NUM_HEADLIGHT_STATES)
232  headlight_state = HEADLIGHT_STATE_START_VALUE;
233 
234  pacmod_msgs::PacmodCmd headlight_cmd_pub_msg;
235  headlight_cmd_pub_msg.ui16_cmd = headlight_state;
236  headlight_cmd_pub.publish(headlight_cmd_pub_msg);
237  }
238 
239  // Horn
240  pacmod_msgs::PacmodCmd horn_cmd_pub_msg;
241 
242  if (msg->buttons[btns[RIGHT_BUMPER]] == BUTTON_DOWN)
243  horn_cmd_pub_msg.ui16_cmd = 1;
244  else
245  horn_cmd_pub_msg.ui16_cmd = 0;
246 
247  horn_cmd_pub.publish(horn_cmd_pub_msg);
248  }
249 
250  if (vehicle_type == 3 && controller != HRI_SAFE_REMOTE) // Semi
251  {
252  // Windshield wipers
253  if (msg->buttons[btns[LEFT_BUMPER]] == BUTTON_DOWN)
254  {
255  // Rotate through wiper states as button is pressed
256  wiper_state++;
257 
258  if(wiper_state >= NUM_WIPER_STATES)
259  wiper_state = WIPER_STATE_START_VALUE;
260 
261  pacmod_msgs::PacmodCmd wiper_cmd_pub_msg;
262  wiper_cmd_pub_msg.ui16_cmd = wiper_state;
263  wiper_cmd_pub.publish(wiper_cmd_pub_msg);
264  }
265  }
266 }
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
void publish_shifting_message(const sensor_msgs::Joy::ConstPtr &msg)
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
static const float AXES_MAX
Definition: globals.h:112
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::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
static const uint16_t WIPER_STATE_START_VALUE
Definition: globals.h:114
ros::Publisher accelerator_cmd_pub
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void publish_steering_message(const sensor_msgs::Joy::ConstPtr &msg)
ros::Publisher brake_set_position_pub
ros::Publisher turn_signal_cmd_pub
void publish_accelerator_message(const sensor_msgs::Joy::ConstPtr &msg)
void publish_lights_horn_wipers_message(const sensor_msgs::Joy::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
void publish_brake_message(const sensor_msgs::Joy::ConstPtr &msg)
static std::unordered_map< JoyAxis, int, EnumHash > axes
static const float STEER_SCALE_FACTOR
Definition: globals.h:104
void publish_turn_signal_message(const sensor_msgs::Joy::ConstPtr &msg)
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