test_joystick_interrupt.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2018, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <string>
31 
32 #include <ros/ros.h>
33 #include <geometry_msgs/Twist.h>
34 #include <sensor_msgs/Joy.h>
35 #include <std_msgs/Int32.h>
36 
37 #include <gtest/gtest.h>
38 
39 class JoystickInterruptTest : public ::testing::Test
40 {
41 protected:
42  ros::NodeHandle nh_;
43  ros::Publisher pub_cmd_vel_;
44  ros::Publisher pub_joy_;
45  ros::Subscriber sub_cmd_vel_;
46 
47  geometry_msgs::Twist::ConstPtr cmd_vel_;
48 
49  void cbCmdVel(const geometry_msgs::Twist::ConstPtr& msg)
50  {
51  cmd_vel_ = msg;
52  }
53 
54 public:
55  explicit JoystickInterruptTest(const std::string& cmd_vel_topic = "cmd_vel")
56  : nh_()
57  {
58  pub_cmd_vel_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel_input", 1);
59  pub_joy_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
60  sub_cmd_vel_ = nh_.subscribe(cmd_vel_topic, 1, &JoystickInterruptTest::cbCmdVel, this);
61 
62  ros::Rate wait(10);
63  for (size_t i = 0; i < 100; ++i)
64  {
65  wait.sleep();
66  ros::spinOnce();
67  if (i > 5 && pub_cmd_vel_.getNumSubscribers() > 0)
68  break;
69  }
70  }
71  void publishCmdVel(
72  const float lin,
73  const float ang)
74  {
75  geometry_msgs::Twist cmd_vel_out;
76  cmd_vel_out.linear.x = lin;
77  cmd_vel_out.angular.z = ang;
78  pub_cmd_vel_.publish(cmd_vel_out);
79  }
80  void publishJoy(
81  const int button,
82  const int high_speed,
83  const float lin0,
84  const float ang0,
85  const float lin1,
86  const float ang1)
87  {
88  sensor_msgs::Joy joy;
89  joy.header.stamp = ros::Time::now();
90  joy.buttons.resize(2);
91  joy.buttons[0] = button;
92  joy.buttons[1] = high_speed;
93  joy.axes.resize(6);
94  joy.axes[0] = lin0;
95  joy.axes[1] = ang0;
96  joy.axes[2] = lin1;
97  joy.axes[3] = ang1;
98  joy.axes[4] = 0;
99  joy.axes[5] = 0;
100  pub_joy_.publish(joy);
101  }
102 };
103 
104 TEST_F(JoystickInterruptTest, NoInterrupt)
105 {
106  ros::Duration(1.0).sleep();
107  ros::Rate rate(20);
108  for (size_t i = 0; i < 25; ++i)
109  {
110  publishCmdVel(0.1, 0.2);
111  if (i < 5)
112  publishJoy(0, 0, 0, 0, 0, 0);
113  else if (i < 10)
114  publishJoy(0, 0, 1, 1, 0, 0);
115  else if (i < 15)
116  publishJoy(0, 0, 0, 0, 1, 1);
117  else if (i < 20)
118  publishJoy(0, 1, 1, 1, 0, 0);
119  else
120  publishJoy(0, 1, 0, 0, 1, 1);
121 
122  rate.sleep();
123  ros::spinOnce();
124  if (i < 3)
125  continue;
126  ASSERT_TRUE(static_cast<bool>(cmd_vel_));
127  ASSERT_NEAR(cmd_vel_->linear.x, 0.1, 1e-3);
128  ASSERT_NEAR(cmd_vel_->angular.z, 0.2, 1e-3);
129  }
130 }
131 
132 TEST_F(JoystickInterruptTest, Interrupt)
133 {
134  ros::Duration(1.0).sleep();
135  ros::Rate rate(20);
136  for (size_t i = 0; i < 25; ++i)
137  {
138  publishCmdVel(0.1, 0.2);
139  if (i < 5)
140  publishJoy(0, 0, 0, 0, 0, 0);
141  else if (i < 10)
142  publishJoy(1, 0, 1, 0, 0, 0);
143  else if (i < 15)
144  publishJoy(1, 0, 0, 0, 1, 0);
145  else if (i < 20)
146  publishJoy(1, 0, 0, 1, 0, 0);
147  else
148  publishJoy(1, 0, 0, 0, 0, 1);
149 
150  rate.sleep();
151  ros::spinOnce();
152  if (i < 3)
153  continue;
154  ASSERT_TRUE(static_cast<bool>(cmd_vel_));
155  if (i < 5)
156  {
157  ASSERT_NEAR(cmd_vel_->linear.x, 0.1, 1e-3);
158  ASSERT_NEAR(cmd_vel_->angular.z, 0.2, 1e-3);
159  }
160  else if (i < 15)
161  {
162  ASSERT_NEAR(cmd_vel_->linear.x, 1.0, 1e-3);
163  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
164  }
165  else
166  {
167  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
168  ASSERT_NEAR(cmd_vel_->angular.z, 1.0, 1e-3);
169  }
170  }
171 }
172 
173 TEST_F(JoystickInterruptTest, InterruptNoTwistInput)
174 {
175  ros::Duration(1.0).sleep();
176  // make sure the internal state of the joystick interrupt node
177  // (i.e. last_input_twist_) is set back to a zero twist.
178  publishCmdVel(0, 0);
179  ros::Rate rate(20);
180  for (size_t i = 0; i < 20; ++i)
181  {
182  if (i < 5)
183  publishJoy(0, 0, 0, 0, 0, 0);
184  else if (i < 10)
185  publishJoy(1, 0, 1, 0.5, 0, 0);
186  else if (i < 15)
187  publishJoy(0, 0, 1, 0, 0, 0);
188  else
189  publishJoy(0, 0, 0, 0.5, 0, 0);
190 
191  rate.sleep();
192  ros::spinOnce();
193  if (i < 3)
194  continue;
195  ASSERT_TRUE(static_cast<bool>(cmd_vel_));
196  if (i < 5)
197  {
198  ASSERT_NEAR(cmd_vel_->linear.x, 0, 1e-3);
199  ASSERT_NEAR(cmd_vel_->angular.z, 0, 1e-3);
200  }
201  else if (i < 10)
202  {
203  ASSERT_NEAR(cmd_vel_->linear.x, 1.0, 1e-3);
204  ASSERT_NEAR(cmd_vel_->angular.z, 0.5, 1e-3);
205  }
206  else
207  {
208  ASSERT_NEAR(cmd_vel_->linear.x, 0, 1e-3);
209  ASSERT_NEAR(cmd_vel_->angular.z, 0, 1e-3);
210  }
211  }
212 }
213 
214 TEST_F(JoystickInterruptTest, InterruptHighSpeed)
215 {
216  ros::Duration(1.0).sleep();
217  ros::Rate rate(20);
218  for (size_t i = 0; i < 25; ++i)
219  {
220  publishCmdVel(0.1, 0.2);
221  if (i < 5)
222  publishJoy(0, 0, 0, 0, 0, 0);
223  else if (i < 10)
224  publishJoy(1, 1, 1, 0, 0, 0);
225  else if (i < 15)
226  publishJoy(1, 1, 0, 0, 1, 0);
227  else if (i < 20)
228  publishJoy(1, 1, 0, 1, 0, 0);
229  else
230  publishJoy(1, 1, 0, 0, 0, 1);
231 
232  rate.sleep();
233  ros::spinOnce();
234  if (i < 3)
235  continue;
236  ASSERT_TRUE(static_cast<bool>(cmd_vel_));
237  if (i < 5)
238  {
239  ASSERT_NEAR(cmd_vel_->linear.x, 0.1, 1e-3);
240  ASSERT_NEAR(cmd_vel_->angular.z, 0.2, 1e-3);
241  }
242  else if (i < 15)
243  {
244  ASSERT_NEAR(cmd_vel_->linear.x, 2.0, 1e-3);
245  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
246  }
247  else
248  {
249  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
250  ASSERT_NEAR(cmd_vel_->angular.z, 2.0, 1e-3);
251  }
252  }
253 }
254 
255 class JoystickInterruptOmniTest : public JoystickInterruptTest
256 {
257 public:
258  JoystickInterruptOmniTest()
259  : JoystickInterruptTest("cmd_vel_omni")
260  {
261  }
262  void publishJoy(
263  const int button,
264  const int high_speed,
265  const float lin0,
266  const float lin_y0,
267  const float ang0,
268  const float lin1,
269  const float lin_y1,
270  const float ang1)
271  {
272  sensor_msgs::Joy joy;
273  joy.header.stamp = ros::Time::now();
274  joy.buttons.resize(2);
275  joy.buttons[0] = button;
276  joy.buttons[1] = high_speed;
277  joy.axes.resize(6);
278  joy.axes[0] = lin0;
279  joy.axes[1] = ang0;
280  joy.axes[2] = lin1;
281  joy.axes[3] = ang1;
282  joy.axes[4] = lin_y0;
283  joy.axes[5] = lin_y1;
284  pub_joy_.publish(joy);
285  }
286 };
287 
288 TEST_F(JoystickInterruptOmniTest, Interrupt)
289 {
290  ros::Duration(1.0).sleep();
291  ros::Rate rate(20);
292  for (size_t i = 0; i < 25; ++i)
293  {
294  publishCmdVel(0.1, 0.2);
295  if (i < 5)
296  publishJoy(0, 0, 0, 0, 0, 0, 0, 0);
297  else if (i < 10)
298  publishJoy(1, 0, 0.5, 0, 0, 0, 0, 0);
299  else if (i < 15)
300  publishJoy(1, 0, 0.5, 0, 0, -1, 0, 0);
301  else if (i < 20)
302  publishJoy(1, 0, 0, 0.3, 0, 0, 0, 0);
303  else if (i < 25)
304  publishJoy(1, 0, 0, 0.3, 0, 0, 1, 0);
305  else if (i < 30)
306  publishJoy(1, 0, 0, 0, -0.7, 0, 0, 0);
307  else
308  publishJoy(1, 0, 0, 0, -0.7, 0, 0, 1);
309 
310  rate.sleep();
311  ros::spinOnce();
312  if (i < 3)
313  continue;
314  ASSERT_TRUE(static_cast<bool>(cmd_vel_));
315  if (i < 5)
316  {
317  ASSERT_NEAR(cmd_vel_->linear.x, 0.1, 1e-3);
318  ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
319  ASSERT_NEAR(cmd_vel_->angular.z, 0.2, 1e-3);
320  }
321  else if (i < 10)
322  {
323  ASSERT_NEAR(cmd_vel_->linear.x, 0.5, 1e-3);
324  ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
325  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
326  }
327  else if (i < 15)
328  {
329  ASSERT_NEAR(cmd_vel_->linear.x, -1.0, 1e-3);
330  ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
331  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
332  }
333  else if (i < 20)
334  {
335  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
336  ASSERT_NEAR(cmd_vel_->linear.y, 0.15, 1e-3);
337  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
338  }
339  else if (i < 25)
340  {
341  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
342  ASSERT_NEAR(cmd_vel_->linear.y, 0.5, 1e-3);
343  ASSERT_NEAR(cmd_vel_->angular.z, 0.0, 1e-3);
344  }
345  else if (i < 30)
346  {
347  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
348  ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
349  ASSERT_NEAR(cmd_vel_->angular.z, -0.7, 1e-3);
350  }
351  else
352  {
353  ASSERT_NEAR(cmd_vel_->linear.x, 0.0, 1e-3);
354  ASSERT_NEAR(cmd_vel_->linear.y, 0.0, 1e-3);
355  ASSERT_NEAR(cmd_vel_->angular.z, 1.0, 1e-3);
356  }
357  }
358 }
359 
360 class JoystickMuxTest : public ::testing::Test
361 {
362 protected:
363  ros::NodeHandle nh_;
364  ros::Publisher pub1_;
365  ros::Publisher pub2_;
366  ros::Publisher pub_joy_;
367  ros::Subscriber sub_;
368 
369  std_msgs::Int32::ConstPtr msg_;
370 
371  void cbMsg(const std_msgs::Int32::ConstPtr& msg)
372  {
373  msg_ = msg;
374  }
375 
376 public:
377  JoystickMuxTest()
378  {
379  pub1_ = nh_.advertise<std_msgs::Int32>("mux_input0", 1);
380  pub2_ = nh_.advertise<std_msgs::Int32>("mux_input1", 1);
381  pub_joy_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
382  sub_ = nh_.subscribe("mux_output", 1, &JoystickMuxTest::cbMsg, this);
383 
384  ros::Rate wait(10);
385  for (size_t i = 0; i < 100; ++i)
386  {
387  wait.sleep();
388  ros::spinOnce();
389  if (i > 5 && pub1_.getNumSubscribers() > 0)
390  break;
391  }
392  }
393  void waitPublisher()
394  {
395  ros::Rate wait(10);
396  for (size_t i = 0; i < 100; ++i)
397  {
398  wait.sleep();
399  ros::spinOnce();
400  if (i > 5 && sub_.getNumPublishers() > 0)
401  break;
402  }
403  }
404  void publish1(const int32_t v)
405  {
406  std_msgs::Int32 msg_out;
407  msg_out.data = v;
408  pub1_.publish(msg_out);
409  }
410  void publish2(const int32_t v)
411  {
412  std_msgs::Int32 msg_out;
413  msg_out.data = v;
414  pub2_.publish(msg_out);
415  }
416  void publishJoy(const int button)
417  {
418  sensor_msgs::Joy joy;
419  joy.header.stamp = ros::Time::now();
420  joy.buttons.resize(1);
421  joy.buttons[0] = button;
422  pub_joy_.publish(joy);
423  }
424  void publishEmptyJoy()
425  {
426  sensor_msgs::Joy joy;
427  joy.header.stamp = ros::Time::now();
428  pub_joy_.publish(joy);
429  }
430 };
431 
432 TEST_F(JoystickMuxTest, Interrupt)
433 {
434  publish1(0);
435  publish2(0);
436  waitPublisher();
437  for (int btn = 0; btn < 2; ++btn)
438  {
439  publishJoy(btn);
440  ros::Duration(1.0).sleep();
441  ros::Rate rate(20);
442  for (int i = 0; i < 15; ++i)
443  {
444  publishJoy(btn);
445  publish1(i);
446  publish2(-i);
447 
448  rate.sleep();
449  ros::spinOnce();
450 
451  if (i < 5)
452  continue;
453 
454  ASSERT_TRUE(static_cast<bool>(msg_)) << "button: " << btn;
455  if (btn)
456  {
457  ASSERT_NEAR(-i, msg_->data, 2) << "button: " << btn;
458  }
459  else
460  {
461  ASSERT_NEAR(i, msg_->data, 2) << "button:" << btn;
462  }
463  }
464  }
465 }
466 /*
467 TEST_F(JoystickMuxTest, Timeout)
468 {
469  publish1(0);
470  publish2(0);
471  waitPublisher();
472  ros::Rate rate(20);
473  publishJoy(1);
474  for (int i = 0; i < 20; ++i)
475  {
476  publish1(i);
477  publish2(-i);
478 
479  rate.sleep();
480  ros::spinOnce();
481 
482  if (i < 5)
483  continue;
484 
485  ASSERT_TRUE(static_cast<bool>(msg_));
486  if (i < 10)
487  {
488  ASSERT_NEAR(-i, msg_->data, 2);
489  }
490  else if (i > 13)
491  {
492  // after timeout
493  ASSERT_NEAR(i, msg_->data, 2);
494  }
495  }
496 }
497 
498 TEST_F(JoystickMuxTest, ButtonNumberInsufficient)
499 {
500  publish1(0);
501  publish2(0);
502  waitPublisher();
503  ros::Rate rate(20);
504  for (int i = 0; i < 20; ++i)
505  {
506  publishEmptyJoy();
507  publish1(i);
508  publish2(-i);
509 
510  rate.sleep();
511  ros::spinOnce();
512 
513  if (i < 3)
514  continue;
515 
516  ASSERT_TRUE(static_cast<bool>(msg_));
517  ASSERT_NEAR(i, msg_->data, 2);
518  }
519 }*/
520 
521 int main(int argc, char** argv)
522 {
523  testing::InitGoogleTest(&argc, argv);
524  ros::init(argc, argv, "test_joystick_interrupt");
525 
526  return RUN_ALL_TESTS();
527 }
TEST_F
TEST_F(JoystickInterruptTest, NoInterrupt)
Definition: test_joystick_interrupt.cpp:104
JoystickMuxTest::publishEmptyJoy
void publishEmptyJoy()
Definition: test_joystick_interrupt.cpp:424
ros::Publisher
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
ros.h
JoystickMuxTest::publish1
void publish1(const int32_t v)
Definition: test_joystick_interrupt.cpp:404
JoystickInterruptTest::sub_cmd_vel_
ros::Subscriber sub_cmd_vel_
Definition: test_joystick_interrupt.cpp:45
ros::spinOnce
ROSCPP_DECL void spinOnce()
JoystickMuxTest::pub1_
ros::Publisher pub1_
Definition: test_joystick_interrupt.cpp:364
JoystickMuxTest::publishJoy
void publishJoy(const int button)
Definition: test_joystick_interrupt.cpp:416
JoystickInterruptTest::JoystickInterruptTest
JoystickInterruptTest(const std::string &cmd_vel_topic="cmd_vel")
Definition: test_joystick_interrupt.cpp:55
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
JoystickInterruptTest::nh_
ros::NodeHandle nh_
Definition: test_joystick_interrupt.cpp:42
JoystickInterruptTest::pub_cmd_vel_
ros::Publisher pub_cmd_vel_
Definition: test_joystick_interrupt.cpp:43
ros::Subscriber::getNumPublishers
uint32_t getNumPublishers() const
JoystickMuxTest::pub_joy_
ros::Publisher pub_joy_
Definition: test_joystick_interrupt.cpp:366
JoystickInterruptOmniTest::JoystickInterruptOmniTest
JoystickInterruptOmniTest()
Definition: test_joystick_interrupt.cpp:258
JoystickInterruptTest::cmd_vel_
geometry_msgs::Twist::ConstPtr cmd_vel_
Definition: test_joystick_interrupt.cpp:47
main
int main(int argc, char **argv)
Definition: test_joystick_interrupt.cpp:521
JoystickMuxTest::nh_
ros::NodeHandle nh_
Definition: test_joystick_interrupt.cpp:363
JoystickMuxTest::msg_
std_msgs::Int32::ConstPtr msg_
Definition: test_joystick_interrupt.cpp:369
JoystickMuxTest::pub2_
ros::Publisher pub2_
Definition: test_joystick_interrupt.cpp:365
JoystickMuxTest::sub_
ros::Subscriber sub_
Definition: test_joystick_interrupt.cpp:367
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
JoystickInterruptTest::publishJoy
void publishJoy(const int button, const int high_speed, const float lin0, const float ang0, const float lin1, const float ang1)
Definition: test_joystick_interrupt.cpp:80
JoystickMuxTest::publish2
void publish2(const int32_t v)
Definition: test_joystick_interrupt.cpp:410
ros::Rate::sleep
bool sleep()
JoystickInterruptTest
Definition: test_joystick_interrupt.cpp:39
JoystickInterruptOmniTest
Definition: test_joystick_interrupt.cpp:255
JoystickMuxTest
Definition: test_joystick_interrupt.cpp:360
JoystickInterruptOmniTest::publishJoy
void publishJoy(const int button, const int high_speed, const float lin0, const float lin_y0, const float ang0, const float lin1, const float lin_y1, const float ang1)
Definition: test_joystick_interrupt.cpp:262
JoystickMuxTest::JoystickMuxTest
JoystickMuxTest()
Definition: test_joystick_interrupt.cpp:377
JoystickInterruptTest::pub_joy_
ros::Publisher pub_joy_
Definition: test_joystick_interrupt.cpp:44
ros::Rate
JoystickInterruptTest::publishCmdVel
void publishCmdVel(const float lin, const float ang)
Definition: test_joystick_interrupt.cpp:71
ros::Duration::sleep
bool sleep() const
ros::Publisher::getNumSubscribers
uint32_t getNumSubscribers() const
JoystickMuxTest::cbMsg
void cbMsg(const std_msgs::Int32::ConstPtr &msg)
Definition: test_joystick_interrupt.cpp:371
JoystickMuxTest::waitPublisher
void waitPublisher()
Definition: test_joystick_interrupt.cpp:393
ros::Duration
ros::NodeHandle
ros::Subscriber
JoystickInterruptTest::cbCmdVel
void cbCmdVel(const geometry_msgs::Twist::ConstPtr &msg)
Definition: test_joystick_interrupt.cpp:49
ros::Time::now
static Time now()

joystick_interrupt
Author(s): Atsushi Watanabe
autogenerated on Wed Mar 19 2025 02:14:07