simple_keyop.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2013, Yujin Robot.
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 Yujin Robot 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 
37 /*****************************************************************************
38  ** Includes
39  *****************************************************************************/
40 
41 #include <string>
42 #include <csignal>
43 #include <termios.h> // for keyboard input
44 #include <ecl/console.hpp>
45 #include <ecl/exceptions.hpp>
47 #include <ecl/linear_algebra.hpp>
48 #include <ecl/sigslots.hpp>
49 #include <ecl/time.hpp>
50 #include <ecl/threads.hpp>
51 
52 #include "kobuki_driver/kobuki.hpp"
53 
54 /*****************************************************************************
55 ** Classes
56 *****************************************************************************/
57 
62 class KobukiManager
63 {
64 public:
65  /*********************
66  ** C&D
67  **********************/
68  KobukiManager();
70  bool init();
71 
72  /*********************
73  ** Runtime
74  **********************/
75  void spin();
76 
77  /*********************
78  ** Callbacks
79  **********************/
80  void processStreamData();
81 
82  /*********************
83  ** Accessor
84  **********************/
86  bool isShutdown() const { return quit_requested || kobuki.isShutdown(); }
87 
88 private:
89  double vx, wz;
94 
97  std::string name;
98 
99  /*********************
100  ** Commands
101  **********************/
106  void resetVelocity();
107 
108  /*********************
109  ** Logging
110  **********************/
111  void relayWarnings(const std::string& message);
112  void relayErrors(const std::string& message);
113 
114  /*********************
115  ** Keylogging
116  **********************/
117 
118  void keyboardInputLoop();
119  void processKeyboardInput(char c);
120  void restoreTerminal();
121  bool quit_requested;
123  struct termios original_terminal_state;
124  ecl::Thread thread;
125 };
126 
127 /*****************************************************************************
128  ** Implementation
129  *****************************************************************************/
130 
135  linear_vel_step(0.05),
136  linear_vel_max(1.0),
137  angular_vel_step(0.33),
138  angular_vel_max(6.6),
139  quit_requested(false),
140  key_file_descriptor(0),
141  vx(0.0), wz(0.0),
142  slot_debug_error(&KobukiManager::relayErrors, *this),
143  slot_debug_warning(&KobukiManager::relayWarnings, *this),
144  slot_stream_data(&KobukiManager::processStreamData, *this)
145 {
146  tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
147 }
148 
150 {
151  kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
152  kobuki.disable();
153  tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
154 }
155 
160 {
161  /*********************
162  ** Parameters
163  **********************/
164  std::cout << "Parameters" << std::endl;
165  std::cout << "----------" << std::endl;
166  std::cout << " - linear_vel_max [" << linear_vel_max << "]" << std::endl;
167  std::cout << " - linear_vel_step [" << linear_vel_step << "]" << std::endl;
168  std::cout << " - angular_vel_max [" << angular_vel_max << "]" << std::endl;
169  std::cout << " - angular_vel_step [" << angular_vel_step << "]" << std::endl;
170  std::cout << std::endl;
171 
172  /*********************
173  ** Velocities
174  **********************/
175  vx = 0.0;
176  wz = 0.0;
177 
178  /*********************
179  ** Kobuki
180  **********************/
181  kobuki::Parameters parameters;
182  parameters.sigslots_namespace = "/kobuki";
183  parameters.device_port = "/dev/kobuki";
184  parameters.enable_acceleration_limiter = true;
185 
186  kobuki.init(parameters);
187  kobuki.enable();
188  slot_debug_warning.connect("/kobuki/ros_warn");
189  slot_debug_error.connect("/kobuki/ros_error");
190  slot_stream_data.connect("/kobuki/stream_data");
191 
192  /*********************
193  ** Wait for connection
194  **********************/
196  return true;
197 }
198 
199 /*****************************************************************************
200  ** Implementation [Spin]
201  *****************************************************************************/
202 
208 void KobukiManager::spin()
209 {
210 /*
211  {
212  // just in case we got here not via a keyboard quit request
213  quit_requested = true;
214  thread.cancel();
215  }
216 */
217  ecl::Sleep sleep(ecl::Duration(0.1));
218  while (!quit_requested){
219  sleep();
220  }
221  thread.join();
222 }
223 
224 /*****************************************************************************
225  ** Implementation [Logging]
226  *****************************************************************************/
227 
228 void KobukiManager::relayWarnings(const std::string& message) {
229  std::cout << ecl::yellow << "[WARNING] " << message << ecl::reset << std::endl;
230 }
231 
232 void KobukiManager::relayErrors(const std::string& message) {
233  std::cout << ecl::red << "[ERROR] " << message << ecl::reset << std::endl;
234 }
235 
236 /*****************************************************************************
237  ** Implementation [Keyboard]
238  *****************************************************************************/
239 
247 {
248  struct termios raw;
249  memcpy(&raw, &original_terminal_state, sizeof(struct termios));
250 
251  raw.c_lflag &= ~(ICANON | ECHO);
252  // Setting a new line, then end of file
253  raw.c_cc[VEOL] = 1;
254  raw.c_cc[VEOF] = 2;
255  tcsetattr(key_file_descriptor, TCSANOW, &raw);
256 
257  std::cout << "Reading from keyboard" << std::endl;
258  std::cout << "---------------------" << std::endl;
259  std::cout << "Forward/back arrows : linear velocity incr/decr." << std::endl;
260  std::cout << "Right/left arrows : angular velocity incr/decr." << std::endl;
261  std::cout << "Spacebar : reset linear/angular velocities." << std::endl;
262  std::cout << "q : quit." << std::endl;
263  std::cout << std::endl;
264  char c;
265  while (!quit_requested)
266  {
267  if (read(key_file_descriptor, &c, 1) < 0)
268  {
269  perror("read char failed():");
270  exit(-1);
271  }
273  }
274 }
275 
282 {
283  /*
284  * Arrow keys are a bit special, they are escape characters - meaning they
285  * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
286  * ignore the esc-[ and just parse the last one. So long as we avoid using
287  * the last one for its actual purpose (e.g. left arrow corresponds to
288  * esc-[-D) we can keep the parsing simple.
289  */
290  switch (c)
291  {
292  case 68://kobuki_msgs::KeyboardInput::KeyCode_Left:
293  {
295  break;
296  }
297  case 67://kobuki_msgs::KeyboardInput::KeyCode_Right:
298  {
300  break;
301  }
302  case 65://kobuki_msgs::KeyboardInput::KeyCode_Up:
303  {
305  break;
306  }
307  case 66://kobuki_msgs::KeyboardInput::KeyCode_Down:
308  {
310  break;
311  }
312  case 32://kobuki_msgs::KeyboardInput::KeyCode_Space:
313  {
314  resetVelocity();
315  break;
316  }
317  case 'q':
318  {
319  quit_requested = true;
320  break;
321  }
322  default:
323  {
324  break;
325  }
326  }
327 }
328 
329 /*****************************************************************************
330  ** Implementation [Commands]
331  *****************************************************************************/
332 
337 {
338  if (vx <= linear_vel_max)
339  {
340  vx += linear_vel_step;
341  }
342 // ROS_INFO_STREAM("KeyOp: linear velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
343 }
344 
349 {
350  if (vx >= -linear_vel_max)
351  {
352  vx -= linear_vel_step;
353  }
354 // ROS_INFO_STREAM("KeyOp: linear velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
355 }
356 
361 {
362  if (wz <= angular_vel_max)
363  {
364  wz += angular_vel_step;
365  }
366 // ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
367 }
368 
373 {
374  if (wz >= -angular_vel_max)
375  {
376  wz -= angular_vel_step;
377  }
378 // ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
379 }
380 
382 {
383  vx = 0.0;
384  wz = 0.0;
385 // ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
386 }
387 
389  ecl::LegacyPose2D<double> pose_update;
390  ecl::linear_algebra::Vector3d pose_update_rates;
391  kobuki.updateOdometry(pose_update, pose_update_rates);
392  pose *= pose_update;
393 // dx += pose_update.x();
394 // dth += pose_update.heading();
395  //std::cout << dx << ", " << dth << std::endl;
396  //std::cout << kobuki.getHeading() << ", " << pose.heading() << std::endl;
397  //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
398 
399  kobuki.setBaseControl(vx, wz);
400 }
401 
403  return pose;
404 }
405 
406 /*****************************************************************************
407 ** Signal Handler
408 *****************************************************************************/
409 
410 bool shutdown_req = false;
411 void signalHandler(int signum) {
412  shutdown_req = true;
413 }
414 
415 /*****************************************************************************
416 ** Main
417 *****************************************************************************/
418 
419 int main(int argc, char** argv)
420 {
421  signal(SIGINT, signalHandler);
422 
423  std::cout << ecl::bold << "\nSimple Keyop : Utility for driving kobuki by keyboard.\n" << ecl::reset << std::endl;
424  KobukiManager kobuki_manager;
425  kobuki_manager.init();
426 
427  ecl::Sleep sleep(1);
429  try {
430  while (!shutdown_req && !kobuki_manager.isShutdown()){
431  sleep();
432  pose = kobuki_manager.getPose();
433  std::cout << ecl::green;
434  std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
435  std::cout << ecl::reset;
436  }
437  } catch ( ecl::StandardException &e ) {
438  std::cout << e.what();
439  }
440  return 0;
441 }
KobukiManager::vx
double vx
Definition: simple_keyop.cpp:93
threads.hpp
kobuki::Parameters::sigslots_namespace
std::string sigslots_namespace
The first part of a sigslot connection namespace ["/kobuki"].
Definition: parameters.hpp:69
KobukiManager::relayWarnings
void relayWarnings(const std::string &message)
Definition: simple_keyop.cpp:228
ecl::Slot< const std::string & >
KobukiManager::restoreTerminal
void restoreTerminal()
KobukiManager::angular_vel_step
double angular_vel_step
Definition: simple_keyop.cpp:100
KobukiManager::quit_requested
bool quit_requested
Definition: simple_keyop.cpp:125
KobukiManager::decrementAngularVelocity
void decrementAngularVelocity()
If not already mined, decrement the angular velocities..
Definition: simple_keyop.cpp:372
kobuki::Parameters::enable_acceleration_limiter
bool enable_acceleration_limiter
Enable or disable the acceleration limiter [true].
Definition: parameters.hpp:71
kobuki
Definition: command.hpp:31
KobukiManager::decrementLinearVelocity
void decrementLinearVelocity()
If not already minned, decrement the linear velocities..
Definition: simple_keyop.cpp:348
sigslots.hpp
KobukiManager::slot_debug_warning
ecl::Slot< const std::string & > slot_debug_warning
Definition: simple_keyop.cpp:96
linear_algebra.hpp
ecl::LegacyPose2D< double >
KobukiManager::resetVelocity
void resetVelocity()
Definition: simple_keyop.cpp:381
KobukiManager::getPose
ecl::LegacyPose2D< double > getPose()
Definition: simple_loop.cpp:68
KobukiManager::processKeyboardInput
void processKeyboardInput(char c)
Process individual keyboard inputs.
Definition: simple_keyop.cpp:281
KobukiManager::pose
ecl::LegacyPose2D< double > pose
Definition: simple_loop.cpp:74
legacy_pose2d.hpp
KobukiManager::angular_vel_max
double angular_vel_max
Definition: simple_keyop.cpp:100
KobukiManager::linear_vel_max
double linear_vel_max
Definition: simple_keyop.cpp:99
KobukiManager::spin
void spin()
Worker thread loop; sends current velocity command at a fixed rate.
Definition: sigslots.cpp:36
KobukiManager::keyboardInputLoop
void keyboardInputLoop()
The worker thread function that accepts input keyboard commands.
Definition: simple_keyop.cpp:246
console.hpp
KobukiManager::name
std::string name
Definition: simple_keyop.cpp:101
KobukiManager::slot_debug_error
ecl::Slot< const std::string & > slot_debug_error
Definition: simple_keyop.cpp:96
signalHandler
void signalHandler(int signum)
Definition: simple_keyop.cpp:411
KobukiManager::kobuki
kobuki::Kobuki kobuki
Definition: initialisation.cpp:42
KobukiManager
Keyboard remote control for our robot core (mobile base).
Definition: initialisation.cpp:14
KobukiManager::incrementAngularVelocity
void incrementAngularVelocity()
If not already maxxed, increment the angular velocities..
Definition: simple_keyop.cpp:360
KobukiManager::linear_vel_step
double linear_vel_step
Definition: simple_keyop.cpp:99
ecl::StandardException
KobukiManager::original_terminal_state
struct termios original_terminal_state
Definition: simple_keyop.cpp:127
ecl::Slot::connect
void connect(const std::string &topic)
KobukiManager::key_file_descriptor
int key_file_descriptor
Definition: simple_keyop.cpp:126
ecl::Duration
TimeStamp Duration
kobuki::Parameters
Parameter list and validator for the kobuki.
Definition: parameters.hpp:42
kobuki.hpp
Device driver core interface.
KobukiManager::processStreamData
void processStreamData()
Definition: sigslots.cpp:50
KobukiManager::~KobukiManager
~KobukiManager()
Definition: simple_loop.cpp:43
kobuki::Kobuki
The core kobuki driver class.
Definition: kobuki.hpp:95
ecl::StandardException::what
const char * what() const
KobukiManager::KobukiManager
KobukiManager()
Default constructor, needs initialisation.
Definition: initialisation.cpp:18
time.hpp
KobukiManager::incrementLinearVelocity
void incrementLinearVelocity()
If not already maxxed, increment the command velocities..
Definition: simple_keyop.cpp:336
KobukiManager::slot_stream_data
ecl::Slot slot_stream_data
Definition: sigslots.cpp:57
shutdown_req
bool shutdown_req
Definition: simple_keyop.cpp:410
KobukiManager::isShutdown
bool isShutdown() const
Definition: simple_keyop.cpp:90
KobukiManager::thread
ecl::Thread thread
Definition: simple_keyop.cpp:128
KobukiManager::init
bool init()
Initialises the node.
Definition: simple_keyop.cpp:159
KobukiManager::relayErrors
void relayErrors(const std::string &message)
Definition: simple_keyop.cpp:232
main
int main(int argc, char **argv)
Definition: simple_keyop.cpp:419
kobuki::Parameters::device_port
std::string device_port
The serial device port name [/dev/kobuki].
Definition: parameters.hpp:68
KobukiManager::wz
double wz
Definition: simple_keyop.cpp:93
exceptions.hpp


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Wed Mar 2 2022 00:26:14