simple_keyop.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Yujin Robot.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Yujin Robot nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00037 /*****************************************************************************
00038  ** Includes
00039  *****************************************************************************/
00040 
00041 #include <string>
00042 #include <csignal>
00043 #include <termios.h> // for keyboard input
00044 #include <ecl/time.hpp>
00045 #include <ecl/threads.hpp>
00046 #include <ecl/sigslots.hpp>
00047 #include <ecl/exceptions.hpp>
00048 #include <ecl/linear_algebra.hpp>
00049 #include <ecl/geometry/legacy_pose2d.hpp>
00050 #include "kobuki_driver/kobuki.hpp"
00051 
00052 /*****************************************************************************
00053 ** Classes
00054 *****************************************************************************/
00055 
00060 class KobukiManager
00061 {
00062 public:
00063   /*********************
00064    ** C&D
00065    **********************/
00066   KobukiManager();
00067   ~KobukiManager();
00068   bool init();
00069 
00070   /*********************
00071    ** Runtime
00072    **********************/
00073   void spin();
00074 
00075   /*********************
00076    ** Callbacks
00077    **********************/
00078   void processStreamData();
00079 
00080   /*********************
00081    ** Accessor
00082    **********************/
00083   ecl::LegacyPose2D<double> getPose();
00084 
00085 private:
00086   double vx, wz;
00087   ecl::LegacyPose2D<double> pose;
00088   kobuki::Kobuki kobuki;
00089   ecl::Slot<> slot_stream_data;
00090 
00091   double linear_vel_step, linear_vel_max;
00092   double angular_vel_step, angular_vel_max;
00093   std::string name;
00094 
00095   /*********************
00096    ** Commands
00097    **********************/
00098   void incrementLinearVelocity();
00099   void decrementLinearVelocity();
00100   void incrementAngularVelocity();
00101   void decrementAngularVelocity();
00102   void resetVelocity();
00103 
00104   /*********************
00105    ** Keylogging
00106    **********************/
00107 
00108   void keyboardInputLoop();
00109   void processKeyboardInput(char c);
00110   void restoreTerminal();
00111   bool quit_requested;
00112   int key_file_descriptor;
00113   struct termios original_terminal_state;
00114   ecl::Thread thread;
00115 };
00116 
00117 /*****************************************************************************
00118  ** Implementation
00119  *****************************************************************************/
00120 
00124 KobukiManager::KobukiManager() :
00125                          linear_vel_step(0.05),
00126                          linear_vel_max(1.0),
00127                          angular_vel_step(0.33),
00128                          angular_vel_max(6.6),
00129                          quit_requested(false),
00130                          key_file_descriptor(0),
00131                          vx(0.0), wz(0.0),
00132                          slot_stream_data(&KobukiManager::processStreamData, *this)
00133 {
00134   tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
00135 }
00136 
00137 KobukiManager::~KobukiManager()
00138 {
00139   kobuki.setBaseControl(0,0); // linear_velocity, angular_velocity in (m/s), (rad/s)
00140   kobuki.disable();
00141   tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
00142 }
00143 
00147 bool KobukiManager::init()
00148 {
00149   /*********************
00150    ** Parameters
00151    **********************/
00152   std::cout << "KobukiManager : using linear  vel step [" << linear_vel_step << "]." << std::endl;
00153   std::cout << "KobukiManager : using linear  vel max  [" << linear_vel_max << "]." << std::endl;
00154   std::cout << "KobukiManager : using angular vel step [" << angular_vel_step << "]." << std::endl;
00155   std::cout << "KobukiManager : using angular vel max  [" << angular_vel_max << "]." << std::endl;
00156 
00157   /*********************
00158    ** Velocities
00159    **********************/
00160   vx = 0.0;
00161   wz = 0.0;
00162 
00163   /*********************
00164    ** Kobuki
00165    **********************/
00166   kobuki::Parameters parameters;
00167   parameters.sigslots_namespace = "/kobuki";
00168   parameters.device_port = "/dev/kobuki";
00169   parameters.enable_acceleration_limiter = true;
00170 
00171   kobuki.init(parameters);
00172   kobuki.enable();
00173   slot_stream_data.connect("/kobuki/stream_data");
00174 
00175   /*********************
00176    ** Wait for connection
00177    **********************/
00178   thread.start(&KobukiManager::keyboardInputLoop, *this);
00179   return true;
00180 }
00181 
00182 /*****************************************************************************
00183  ** Implementation [Spin]
00184  *****************************************************************************/
00185 
00191 void KobukiManager::spin()
00192 {
00193 /*
00194   {
00195     // just in case we got here not via a keyboard quit request
00196     quit_requested = true;
00197     thread.cancel();
00198   }
00199 */
00200   ecl::Sleep sleep(0.1);
00201   while (!quit_requested){
00202     sleep();
00203   }
00204   thread.join();
00205 }
00206 
00207 /*****************************************************************************
00208  ** Implementation [Keyboard]
00209  *****************************************************************************/
00210 
00217 void KobukiManager::keyboardInputLoop()
00218 {
00219   struct termios raw;
00220   memcpy(&raw, &original_terminal_state, sizeof(struct termios));
00221 
00222   raw.c_lflag &= ~(ICANON | ECHO);
00223   // Setting a new line, then end of file
00224   raw.c_cc[VEOL] = 1;
00225   raw.c_cc[VEOF] = 2;
00226   tcsetattr(key_file_descriptor, TCSANOW, &raw);
00227 
00228   puts("Reading from keyboard");
00229   puts("---------------------------");
00230   puts("Forward/back arrows : linear velocity incr/decr.");
00231   puts("Right/left arrows : angular velocity incr/decr.");
00232   puts("Spacebar : reset linear/angular velocities.");
00233   puts("q : quit.");
00234   char c;
00235   while (!quit_requested)
00236   {
00237     if (read(key_file_descriptor, &c, 1) < 0)
00238     {
00239       perror("read char failed():");
00240       exit(-1);
00241     }
00242     processKeyboardInput(c);
00243   }
00244 }
00245 
00251 void KobukiManager::processKeyboardInput(char c)
00252 {
00253   /*
00254    * Arrow keys are a bit special, they are escape characters - meaning they
00255    * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
00256    * ignore the esc-[ and just parse the last one. So long as we avoid using
00257    * the last one for its actual purpose (e.g. left arrow corresponds to
00258    * esc-[-D) we can keep the parsing simple.
00259    */
00260   switch (c)
00261   {
00262     case 68://kobuki_msgs::KeyboardInput::KeyCode_Left:
00263     {
00264       incrementAngularVelocity();
00265       break;
00266     }
00267     case 67://kobuki_msgs::KeyboardInput::KeyCode_Right:
00268     {
00269       decrementAngularVelocity();
00270       break;
00271     }
00272     case 65://kobuki_msgs::KeyboardInput::KeyCode_Up:
00273     {
00274       incrementLinearVelocity();
00275       break;
00276     }
00277     case 66://kobuki_msgs::KeyboardInput::KeyCode_Down:
00278     {
00279       decrementLinearVelocity();
00280       break;
00281     }
00282     case 32://kobuki_msgs::KeyboardInput::KeyCode_Space:
00283     {
00284       resetVelocity();
00285       break;
00286     }
00287     case 'q':
00288     {
00289       quit_requested = true;
00290       break;
00291     }
00292     default:
00293     {
00294       break;
00295     }
00296   }
00297 }
00298 
00299 /*****************************************************************************
00300  ** Implementation [Commands]
00301  *****************************************************************************/
00302 
00306 void KobukiManager::incrementLinearVelocity()
00307 {
00308   if (vx <= linear_vel_max)
00309   {
00310     vx += linear_vel_step;
00311   }
00312 //  ROS_INFO_STREAM("KeyOp: linear  velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00313 }
00314 
00318 void KobukiManager::decrementLinearVelocity()
00319 {
00320   if (vx >= -linear_vel_max)
00321   {
00322     vx -= linear_vel_step;
00323   }
00324 //  ROS_INFO_STREAM("KeyOp: linear  velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00325 }
00326 
00330 void KobukiManager::incrementAngularVelocity()
00331 {
00332   if (wz <= angular_vel_max)
00333   {
00334     wz += angular_vel_step;
00335   }
00336 //  ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00337 }
00338 
00342 void KobukiManager::decrementAngularVelocity()
00343 {
00344   if (wz >= -angular_vel_max)
00345   {
00346     wz -= angular_vel_step;
00347   }
00348 //    ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00349 }
00350 
00351 void KobukiManager::resetVelocity()
00352 {
00353   vx = 0.0;
00354   wz = 0.0;
00355 //    ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
00356 }
00357 
00358 void KobukiManager::processStreamData() {
00359   ecl::LegacyPose2D<double> pose_update;
00360   ecl::linear_algebra::Vector3d pose_update_rates;
00361   kobuki.updateOdometry(pose_update, pose_update_rates);
00362   pose *= pose_update;
00363 //  dx += pose_update.x();
00364 //  dth += pose_update.heading();
00365   //std::cout << dx << ", " << dth << std::endl;
00366   //std::cout << kobuki.getHeading() << ", " << pose.heading() << std::endl;
00367   //std::cout << "[" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
00368 
00369   kobuki.setBaseControl(vx, wz);
00370 }
00371 
00372 ecl::LegacyPose2D<double> KobukiManager::getPose() {
00373   return pose;
00374 }
00375 
00376 /*****************************************************************************
00377 ** Signal Handler
00378 *****************************************************************************/
00379 
00380 bool shutdown_req = false;
00381 void signalHandler(int signum) {
00382   shutdown_req = true;
00383 }
00384 
00385 /*****************************************************************************
00386 ** Main
00387 *****************************************************************************/
00388 
00389 int main(int argc, char** argv)
00390 {
00391   signal(SIGINT, signalHandler);
00392 
00393   std::cout << "Simple Keyop : Utility for driving kobuki by keyboard." << std::endl;
00394   KobukiManager kobuki_manager;
00395   kobuki_manager.init();
00396 
00397   ecl::Sleep sleep(1);
00398   ecl::LegacyPose2D<double> pose;
00399   try {
00400     while (!shutdown_req){
00401       sleep();
00402       pose = kobuki_manager.getPose();
00403       std::cout << "current pose: [" << pose.x() << ", " << pose.y() << ", " << pose.heading() << "]" << std::endl;
00404     }
00405   } catch ( ecl::StandardException &e ) {
00406     std::cout << e.what();
00407   }
00408   return 0;
00409 }


kobuki_driver
Author(s): Daniel Stonier , Younghun Ju , Jorge Santos Simon
autogenerated on Thu Jun 6 2019 20:24:37