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/time.hpp>
45 #include <ecl/threads.hpp>
46 #include <ecl/sigslots.hpp>
47 #include <ecl/exceptions.hpp>
48 #include <ecl/linear_algebra.hpp>
49 #include <ecl/geometry/pose2d.hpp>
50 #include "xbot_driver/xbot.hpp"
51 
52 /*****************************************************************************
53 ** Classes
54 *****************************************************************************/
55 
60 class XbotManager
61 {
62 public:
63  /*********************
64  ** C&D
65  **********************/
66  XbotManager();
67  ~XbotManager();
68  bool init();
69 
70  /*********************
71  ** Runtime
72  **********************/
73  void spin();
74 
75  /*********************
76  ** Callbacks
77  **********************/
78  void processStreamData();
79 
80  /*********************
81  ** Accessor
82  **********************/
84 
85 private:
86  float vx, wz;
90 
93  std::string name;
94 
95  /*********************
96  ** Commands
97  **********************/
102  void resetVelocity();
103 
104  /*********************
105  ** Keylogging
106  **********************/
107 
108  void keyboardInputLoop();
109  void processKeyboardInput(char c);
110  void restoreTerminal();
113  struct termios original_terminal_state;
114  ecl::Thread thread;
115 };
116 
117 /*****************************************************************************
118  ** Implementation
119  *****************************************************************************/
120 
125  linear_vel_step(0.05),
126  linear_vel_max(1.0),
127  angular_vel_step(0.33),
128  angular_vel_max(6.6),
129  quit_requested(false),
131  vx(0.0), wz(0.0),
133 {
134  tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
135 }
136 
138 {
139  xbot.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
140  xbot.disable();
141  tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
142 }
143 
148 {
149  /*********************
150  ** Parameters
151  **********************/
152  std::cout << "XbotManager : using linear vel step [" << linear_vel_step << "]." << std::endl;
153  std::cout << "XbotManager : using linear vel max [" << linear_vel_max << "]." << std::endl;
154  std::cout << "XbotManager : using angular vel step [" << angular_vel_step << "]." << std::endl;
155  std::cout << "XbotManager : using angular vel max [" << angular_vel_max << "]." << std::endl;
156 
157  /*********************
158  ** Velocities
159  **********************/
160  vx = 0.0;
161  wz = 0.0;
162 
163  /*********************
164  ** Xbot
165  **********************/
166  xbot::Parameters parameters;
167  parameters.sigslots_namespace = "/xbot";
168  parameters.base_port = "/dev/xbot";
169  parameters.enable_acceleration_limiter = true;
170 
171  xbot.init(parameters);
172  xbot.enable();
173  slot_stream_data.connect("/xbot/stream_data");
174 
175  /*********************
176  ** Wait for connection
177  **********************/
178  thread.start(&XbotManager::keyboardInputLoop, *this);
179  return true;
180 }
181 
182 /*****************************************************************************
183  ** Implementation [Spin]
184  *****************************************************************************/
185 
191 void XbotManager::spin()
192 {
193 /*
194  {
195  // just in case we got here not via a keyboard quit request
196  quit_requested = true;
197  thread.cancel();
198  }
199 */
200  ecl::Sleep sleep(0.1);
201  while (!quit_requested){
202  sleep();
203  }
204  thread.join();
205 }
206 
207 /*****************************************************************************
208  ** Implementation [Keyboard]
209  *****************************************************************************/
210 
218 {
219  struct termios raw;
220  memcpy(&raw, &original_terminal_state, sizeof(struct termios));
221 
222  raw.c_lflag &= ~(ICANON | ECHO);
223  // Setting a new line, then end of file
224  raw.c_cc[VEOL] = 1;
225  raw.c_cc[VEOF] = 2;
226  tcsetattr(key_file_descriptor, TCSANOW, &raw);
227 
228  puts("Reading from keyboard");
229  puts("---------------------------");
230  puts("Forward/back arrows : linear velocity incr/decr.");
231  puts("Right/left arrows : angular velocity incr/decr.");
232  puts("Spacebar : reset linear/angular velocities.");
233  puts("q : quit.");
234  char c;
235  while (!quit_requested)
236  {
237  if (read(key_file_descriptor, &c, 1) < 0)
238  {
239  perror("read char failed():");
240  exit(-1);
241  }
243  }
244 }
245 
252 {
253  /*
254  * Arrow keys are a bit special, they are escape characters - meaning they
255  * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
256  * ignore the esc-[ and just parse the last one. So long as we avoid using
257  * the last one for its actual purpose (e.g. left arrow corresponds to
258  * esc-[-D) we can keep the parsing simple.
259  */
260  switch (c)
261  {
262  case 68://xbot_msgs::KeyboardInput::KeyCode_Left:
263  {
265  break;
266  }
267  case 67://xbot_msgs::KeyboardInput::KeyCode_Right:
268  {
270  break;
271  }
272  case 65://xbot_msgs::KeyboardInput::KeyCode_Up:
273  {
275  break;
276  }
277  case 66://xbot_msgs::KeyboardInput::KeyCode_Down:
278  {
280  break;
281  }
282  case 32://xbot_msgs::KeyboardInput::KeyCode_Space:
283  {
284  resetVelocity();
285  break;
286  }
287  case 'q':
288  {
289  quit_requested = true;
290  break;
291  }
292  default:
293  {
294  break;
295  }
296  }
297 }
298 
299 /*****************************************************************************
300  ** Implementation [Commands]
301  *****************************************************************************/
302 
307 {
308  if (vx <= linear_vel_max)
309  {
310  vx += linear_vel_step;
311  }
312 // ROS_INFO_STREAM("KeyOp: linear velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
313 }
314 
319 {
320  if (vx >= -linear_vel_max)
321  {
322  vx -= linear_vel_step;
323  }
324 // ROS_INFO_STREAM("KeyOp: linear velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
325 }
326 
331 {
332  if (wz <= angular_vel_max)
333  {
334  wz += angular_vel_step;
335  }
336 // ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
337 }
338 
343 {
344  if (wz >= -angular_vel_max)
345  {
346  wz -= angular_vel_step;
347  }
348 // ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
349 }
350 
352 {
353  vx = 0.0;
354  wz = 0.0;
355 // ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
356 }
357 
359  ecl::Pose2D<double> pose_update;
360  ecl::linear_algebra::Vector3d pose_update_rates;
361  xbot.updateOdometry(pose_update, pose_update_rates);
362  pose *= pose_update;
363 // dx += pose_update.x();
364 // dth += pose_update.heading();
365  //std::cout << dx << ", " << dth << std::endl;
366  //std::cout << xbot.getHeading() << ", " << pose.heading() << std::endl;
367  //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
368 
369  xbot.setBaseControl(vx, wz);
370 }
371 
373  return pose;
374 }
375 
376 /*****************************************************************************
377 ** Signal Handler
378 *****************************************************************************/
379 
380 bool shutdown_req = false;
381 void signalHandler(int signum) {
382  shutdown_req = true;
383 }
384 
385 /*****************************************************************************
386 ** Main
387 *****************************************************************************/
388 
389 int main(int argc, char** argv)
390 {
391  signal(SIGINT, signalHandler);
392 
393  std::cout << "Simple Keyop : Utility for driving xbot by keyboard." << std::endl;
394  XbotManager xbot_manager;
395  xbot_manager.init();
396 
397  ecl::Sleep sleep(1);
399  try {
400  while (!shutdown_req){
401  sleep();
402  pose = xbot_manager.getPose();
403  std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
404  }
405  } catch ( ecl::StandardException &e ) {
406  std::cout << e.what();
407  }
408  return 0;
409 }
void incrementAngularVelocity()
If not already maxxed, increment the angular velocities..
int main(int argc, char **argv)
std::string base_port
The serial device port name [/dev/xbot].
Definition: parameters.hpp:51
const char * what() const
bool init()
Initialises the node.
float angular_vel_step
XbotManager()
Default constructor, needs initialisation.
Parameter list and validator for the xbot.
Definition: parameters.hpp:35
void signalHandler(int signum)
void incrementLinearVelocity()
If not already maxxed, increment the command velocities..
void spin()
Worker thread loop; sends current velocity command at a fixed rate.
Definition: sigslots.cpp:32
std::string sigslots_namespace
The first part of a sigslot connection namespace ["/xbot"].
Definition: parameters.hpp:53
void processKeyboardInput(char c)
Process individual keyboard inputs.
float linear_vel_max
xbot::Xbot xbot
ecl::Pose2D< double > pose
Definition: simple_loop.cpp:75
void keyboardInputLoop()
The worker thread function that accepts input keyboard commands.
bool shutdown_req
EndOfLine endl
Device driver core interface.
Standard exception type, provides code location and error string.
Keyboard remote control for our robot core (mobile base).
std::string name
ecl::Slot slot_stream_data
Definition: sigslots.cpp:53
void processStreamData()
Definition: sigslots.cpp:46
void connect(const std::string &topic)
ecl::Pose2D< double > getPose()
Definition: simple_loop.cpp:69
struct termios original_terminal_state
void decrementAngularVelocity()
If not already mined, decrement the angular velocities..
int key_file_descriptor
float angular_vel_max
void resetVelocity()
Definition: command.hpp:30
ecl::Thread thread
bool enable_acceleration_limiter
Enable or disable the acceleration limiter [true].
Definition: parameters.hpp:55
float linear_vel_step
Function loading component of a callback system.
Definition: slot.hpp:48
The core xbot driver class.
Definition: xbot.hpp:86
void decrementLinearVelocity()
If not already minned, decrement the linear velocities..
void restoreTerminal()


xbot_driver
Author(s): Roc, wangpeng@droid.ac.cn
autogenerated on Sat Oct 10 2020 03:27:38