JoystickDemo.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015-2019, Dataspeed Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Dataspeed Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 #include "JoystickDemo.h"
36 
38 {
39  joy_.axes.resize(AXIS_COUNT_X, 0);
40  joy_.buttons.resize(BTN_COUNT_X, 0);
41 
42  brake_ = true;
43  throttle_ = true;
44  steer_ = true;
45  shift_ = true;
46  signal_ = true;
47  priv_nh.getParam("brake", brake_);
48  priv_nh.getParam("throttle", throttle_);
49  priv_nh.getParam("steer", steer_);
50  priv_nh.getParam("shift", shift_);
51  priv_nh.getParam("signal", signal_);
52 
53  brake_gain_ = 1.0;
54  throttle_gain_ = 1.0;
55  priv_nh.getParam("brake_gain", brake_gain_);
56  priv_nh.getParam("throttle_gain", throttle_gain_);
57  brake_gain_ = std::min(std::max(brake_gain_, (float)0), (float)1);
58  throttle_gain_ = std::min(std::max(throttle_gain_, (float)0), (float)1);
59 
60  ignore_ = false;
61  enable_ = true;
62  count_ = false;
63  strq_ = false;
64  svel_ = 0.0;
66  priv_nh.getParam("ignore", ignore_);
67  priv_nh.getParam("enable", enable_);
68  priv_nh.getParam("count", count_);
69  priv_nh.getParam("strq", strq_);
70  priv_nh.getParam("svel", svel_);
71 
72  sub_joy_ = node.subscribe("/joy", 1, &JoystickDemo::recvJoy, this);
73 
74  data_.brake_joy = 0.0;
75  data_.gear_cmd = dbw_mkz_msgs::Gear::NONE;
76  data_.steering_joy = 0.0;
77  data_.steering_mult = false;
78  data_.throttle_joy = 0.0;
79  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::NONE;
80  data_.joy_throttle_valid = false;
81  data_.joy_brake_valid = false;
82 
83  if (brake_) {
84  pub_brake_ = node.advertise<dbw_mkz_msgs::BrakeCmd>("brake_cmd", 1);
85  }
86  if (throttle_) {
87  pub_throttle_ = node.advertise<dbw_mkz_msgs::ThrottleCmd>("throttle_cmd", 1);
88  }
89  if (steer_) {
90  pub_steering_ = node.advertise<dbw_mkz_msgs::SteeringCmd>("steering_cmd", 1);
91  }
92  if (shift_) {
93  pub_gear_ = node.advertise<dbw_mkz_msgs::GearCmd>("gear_cmd", 1);
94  }
95  if (signal_) {
96  pub_turn_signal_ = node.advertise<dbw_mkz_msgs::TurnSignalCmd>("turn_signal_cmd", 1);
97  }
98  if (enable_) {
99  pub_enable_ = node.advertise<std_msgs::Empty>("enable", 1);
100  pub_disable_ = node.advertise<std_msgs::Empty>("disable", 1);
101  }
102 
104 }
105 
107 {
108  // Detect joy timeouts and reset
109  if (event.current_real - data_.stamp > ros::Duration(0.1)) {
110  data_.joy_throttle_valid = false;
111  data_.joy_brake_valid = false;
113  return;
114  }
115 
116  // Optional watchdog counter
117  if (count_) {
118  counter_++;
119  }
120 
121  // Brake
122  if (brake_) {
123  dbw_mkz_msgs::BrakeCmd msg;
124  msg.enable = true;
125  msg.ignore = ignore_;
126  msg.count = counter_;
127  msg.pedal_cmd_type = dbw_mkz_msgs::BrakeCmd::CMD_PERCENT;
128  msg.pedal_cmd = data_.brake_joy * brake_gain_;
129  pub_brake_.publish(msg);
130  }
131 
132  // Throttle
133  if (throttle_) {
134  dbw_mkz_msgs::ThrottleCmd msg;
135  msg.enable = true;
136  msg.ignore = ignore_;
137  msg.count = counter_;
138  msg.pedal_cmd_type = dbw_mkz_msgs::ThrottleCmd::CMD_PERCENT;
139  msg.pedal_cmd = data_.throttle_joy * throttle_gain_;
140  pub_throttle_.publish(msg);
141  }
142 
143  // Steering
144  if (steer_) {
145  dbw_mkz_msgs::SteeringCmd msg;
146  msg.enable = true;
147  msg.ignore = ignore_;
148  msg.count = counter_;
149  if (!strq_) {
150  msg.cmd_type = dbw_mkz_msgs::SteeringCmd::CMD_ANGLE;
151 
152  float raw_steering_cmd;
153  if (data_.steering_mult) {
154  raw_steering_cmd = dbw_mkz_msgs::SteeringCmd::ANGLE_MAX * data_.steering_joy;
155  } else {
156  raw_steering_cmd = 0.5 * dbw_mkz_msgs::SteeringCmd::ANGLE_MAX * data_.steering_joy;
157  }
158 
159  float tau = 0.1;
160  float filtered_steering_cmd = 0.02 / tau * raw_steering_cmd + (1 - 0.02 / tau) * last_steering_filt_output_;
161  last_steering_filt_output_ = filtered_steering_cmd;
162 
163  msg.steering_wheel_angle_velocity = svel_;
164  msg.steering_wheel_angle_cmd = filtered_steering_cmd;
165  } else {
166  msg.cmd_type = dbw_mkz_msgs::SteeringCmd::CMD_TORQUE;
167  msg.steering_wheel_torque_cmd = dbw_mkz_msgs::SteeringCmd::TORQUE_MAX * data_.steering_joy;
168  }
169  pub_steering_.publish(msg);
170  }
171 
172  // Gear
173  if (shift_) {
174  if (data_.gear_cmd != dbw_mkz_msgs::Gear::NONE) {
175  dbw_mkz_msgs::GearCmd msg;
176  msg.cmd.gear = data_.gear_cmd;
177  pub_gear_.publish(msg);
178  }
179  }
180 
181  // Turn signal
182  if (signal_) {
183  dbw_mkz_msgs::TurnSignalCmd msg;
184  msg.cmd.value = data_.turn_signal_cmd;
186  }
187 }
188 
189 void JoystickDemo::recvJoy(const sensor_msgs::Joy::ConstPtr& msg)
190 {
191  // Check for expected sizes
192  if (msg->axes.size() != (size_t)AXIS_COUNT_X && msg->buttons.size() != (size_t)BTN_COUNT_X) {
193  if (msg->axes.size() == (size_t)AXIS_COUNT_D && msg->buttons.size() == (size_t)BTN_COUNT_D) {
194  ROS_ERROR_THROTTLE(2.0, "Detected Logitech Gamepad F310 in DirectInput (D) mode. Please select (X) with the switch on the back to select XInput mode.");
195  }
196  if (msg->axes.size() != (size_t)AXIS_COUNT_X) {
197  ROS_ERROR_THROTTLE(2.0, "Expected %zu joy axis count, received %zu", (size_t)AXIS_COUNT_X, msg->axes.size());
198  }
199  if (msg->buttons.size() != (size_t)BTN_COUNT_X) {
200  ROS_ERROR_THROTTLE(2.0, "Expected %zu joy button count, received %zu", (size_t)BTN_COUNT_X, msg->buttons.size());
201  }
202  return;
203  }
204 
205  // Handle joystick startup
206  if (msg->axes[AXIS_THROTTLE] != 0.0) {
207  data_.joy_throttle_valid = true;
208  }
209  if (msg->axes[AXIS_BRAKE] != 0.0) {
210  data_.joy_brake_valid = true;
211  }
212 
213  // Throttle
215  data_.throttle_joy = 0.5 - 0.5 * msg->axes[AXIS_THROTTLE];
216  }
217 
218  // Brake
219  if (data_.joy_brake_valid) {
220  data_.brake_joy = 0.5 - 0.5 * msg->axes[AXIS_BRAKE];
221  }
222 
223  // Gear
224  if (msg->buttons[BTN_PARK]) {
225  data_.gear_cmd = dbw_mkz_msgs::Gear::PARK;
226  } else if (msg->buttons[BTN_REVERSE]) {
227  data_.gear_cmd = dbw_mkz_msgs::Gear::REVERSE;
228  } else if (msg->buttons[BTN_DRIVE]) {
229  data_.gear_cmd = dbw_mkz_msgs::Gear::DRIVE;
230  } else if (msg->buttons[BTN_NEUTRAL]) {
231  data_.gear_cmd = dbw_mkz_msgs::Gear::NEUTRAL;
232  } else {
233  data_.gear_cmd = dbw_mkz_msgs::Gear::NONE;
234  }
235 
236  // Steering
237  data_.steering_joy = (fabs(msg->axes[AXIS_STEER_1]) > fabs(msg->axes[AXIS_STEER_2])) ? msg->axes[AXIS_STEER_1] : msg->axes[AXIS_STEER_2];
238  data_.steering_mult = msg->buttons[BTN_STEER_MULT_1] || msg->buttons[BTN_STEER_MULT_2];
239 
240  // Turn signal
241  if (msg->axes[AXIS_TURN_SIG] != joy_.axes[AXIS_TURN_SIG]) {
242  switch (data_.turn_signal_cmd) {
243  case dbw_mkz_msgs::TurnSignal::NONE:
244  if (msg->axes[AXIS_TURN_SIG] < -0.5) {
245  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::RIGHT;
246  } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
247  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::LEFT;
248  }
249  break;
250  case dbw_mkz_msgs::TurnSignal::LEFT:
251  if (msg->axes[AXIS_TURN_SIG] < -0.5) {
252  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::RIGHT;
253  } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
254  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::NONE;
255  }
256  break;
257  case dbw_mkz_msgs::TurnSignal::RIGHT:
258  if (msg->axes[AXIS_TURN_SIG] < -0.5) {
259  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::NONE;
260  } else if (msg->axes[AXIS_TURN_SIG] > 0.5) {
261  data_.turn_signal_cmd = dbw_mkz_msgs::TurnSignal::LEFT;
262  }
263  break;
264  }
265  }
266 
267  // Optional enable and disable buttons
268  if (enable_) {
269  const std_msgs::Empty empty;
270  if (msg->buttons[BTN_ENABLE]) {
271  pub_enable_.publish(empty);
272  }
273  if (msg->buttons[BTN_DISABLE]) {
274  pub_disable_.publish(empty);
275  }
276  }
277 
279  joy_ = *msg;
280 }
281 
msg
ros::Publisher pub_steering_
Definition: JoystickDemo.h:71
ros::Publisher pub_gear_
Definition: JoystickDemo.h:72
void publish(const boost::shared_ptr< M > &message) const
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
sensor_msgs::Joy joy_
Definition: JoystickDemo.h:98
ros::Publisher pub_brake_
Definition: JoystickDemo.h:69
JoystickDataStruct data_
Definition: JoystickDemo.h:97
ros::Timer timer_
Definition: JoystickDemo.h:96
#define ROS_ERROR_THROTTLE(rate,...)
JoystickDemo(ros::NodeHandle &node, ros::NodeHandle &priv_nh)
ros::Publisher pub_enable_
Definition: JoystickDemo.h:74
ros::Publisher pub_disable_
Definition: JoystickDemo.h:75
ros::Subscriber sub_joy_
Definition: JoystickDemo.h:68
void recvJoy(const sensor_msgs::Joy::ConstPtr &msg)
uint8_t counter_
Definition: JoystickDemo.h:99
Timer createTimer(Rate r, Handler h, Obj o, bool oneshot=false, bool autostart=true) const
ros::Publisher pub_throttle_
Definition: JoystickDemo.h:70
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
void cmdCallback(const ros::TimerEvent &event)
bool getParam(const std::string &key, std::string &s) const
static Time now()
ros::Publisher pub_turn_signal_
Definition: JoystickDemo.h:73
float last_steering_filt_output_
Definition: JoystickDemo.h:100
float throttle_gain_
Definition: JoystickDemo.h:86
float brake_gain_
Definition: JoystickDemo.h:85


dbw_mkz_joystick_demo
Author(s): Micho Radovnikovich
autogenerated on Fri May 14 2021 02:47:11