keyop_core.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2012, 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 <ros/ros.h>
00042 #include <ecl/time.hpp>
00043 #include <ecl/exceptions.hpp>
00044 #include <std_srvs/Empty.h>
00045 #include <kobuki_msgs/MotorPower.h>
00046 #include "../include/keyop_core/keyop_core.hpp"
00047 
00048 /*****************************************************************************
00049  ** Namespaces
00050  *****************************************************************************/
00051 
00052 namespace keyop_core
00053 {
00054 
00055 /*****************************************************************************
00056  ** Implementation
00057  *****************************************************************************/
00058 
00062 KeyOpCore::KeyOpCore() : last_zero_vel_sent(true), // avoid zero-vel messages from the beginning
00063                          accept_incoming(true),
00064                          power_status(false),
00065                          wait_for_connection_(true),
00066                          cmd(new geometry_msgs::Twist()),
00067                          cmd_stamped(new geometry_msgs::TwistStamped()),
00068                          linear_vel_step(0.1),
00069                          linear_vel_max(3.4),
00070                          angular_vel_step(0.02),
00071                          angular_vel_max(1.2),
00072                          quit_requested(false),
00073                          key_file_descriptor(0)
00074 {
00075   tcgetattr(key_file_descriptor, &original_terminal_state); // get terminal properties
00076 }
00077 
00078 KeyOpCore::~KeyOpCore()
00079 {
00080   tcsetattr(key_file_descriptor, TCSANOW, &original_terminal_state);
00081 }
00082 
00086 bool KeyOpCore::init()
00087 {
00088   ros::NodeHandle nh("~");
00089 
00090   name = nh.getUnresolvedNamespace();
00091 
00092   /*********************
00093    ** Parameters
00094    **********************/
00095   nh.getParam("linear_vel_step", linear_vel_step);
00096   nh.getParam("linear_vel_max", linear_vel_max);
00097   nh.getParam("angular_vel_step", angular_vel_step);
00098   nh.getParam("angular_vel_max", angular_vel_max);
00099   nh.getParam("wait_for_connection", wait_for_connection_);
00100 
00101   ROS_INFO_STREAM("KeyOpCore : using linear  vel step [" << linear_vel_step << "].");
00102   ROS_INFO_STREAM("KeyOpCore : using linear  vel max  [" << linear_vel_max << "].");
00103   ROS_INFO_STREAM("KeyOpCore : using angular vel step [" << angular_vel_step << "].");
00104   ROS_INFO_STREAM("KeyOpCore : using angular vel max  [" << angular_vel_max << "].");
00105 
00106   /*********************
00107    ** Subscribers
00108    **********************/
00109   keyinput_subscriber = nh.subscribe("teleop", 1, &KeyOpCore::remoteKeyInputReceived, this);
00110 
00111   /*********************
00112    ** Publishers
00113    **********************/
00114   velocity_publisher_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00115   motor_power_publisher_ = nh.advertise<kobuki_msgs::MotorPower>("motor_power", 1);
00116 
00117   /*********************
00118    ** Velocities
00119    **********************/
00120   cmd->linear.x = 0.0;
00121   cmd->linear.y = 0.0;
00122   cmd->linear.z = 0.0;
00123   cmd->angular.x = 0.0;
00124   cmd->angular.y = 0.0;
00125   cmd->angular.z = 0.0;
00126 
00127   /*********************
00128    ** Wait for connection
00129    **********************/
00130   if (!wait_for_connection_)
00131   {
00132     return true;
00133   }
00134   ecl::MilliSleep millisleep;
00135   int count = 0;
00136   bool connected = false;
00137   while (!connected)
00138   {
00139     if (motor_power_publisher_.getNumSubscribers() > 0)
00140     {
00141       connected = true;
00142       break;
00143     }
00144     if (count == 6)
00145     {
00146       connected = false;
00147       break;
00148     }
00149     else
00150     {
00151       ROS_WARN_STREAM("KeyOp: could not connect, trying again after 500ms...");
00152       try
00153       {
00154         millisleep(500);
00155       }
00156       catch (ecl::StandardException e)
00157       {
00158         ROS_ERROR_STREAM("Waiting has been interrupted.");
00159         ROS_DEBUG_STREAM(e.what());
00160         return false;
00161       }
00162       ++count;
00163     }
00164   }
00165   if (!connected)
00166   {
00167     ROS_ERROR("KeyOp: could not connect.");
00168     ROS_ERROR("KeyOp: check remappings for enable/disable topics).");
00169   }
00170   else
00171   {
00172     kobuki_msgs::MotorPower power_cmd;
00173     power_cmd.state = kobuki_msgs::MotorPower::ON;
00174     motor_power_publisher_.publish(power_cmd);
00175     ROS_INFO("KeyOp: connected.");
00176     power_status = true;
00177   }
00178 
00179   // start keyboard input thread
00180   thread.start(&KeyOpCore::keyboardInputLoop, *this);
00181   return true;
00182 }
00183 
00184 /*****************************************************************************
00185  ** Implementation [Spin]
00186  *****************************************************************************/
00187 
00193 void KeyOpCore::spin()
00194 {
00195   ros::Rate loop_rate(10);
00196 
00197   while (!quit_requested && ros::ok())
00198   {
00199     // Avoid spamming robot with continuous zero-velocity messages
00200     if ((cmd->linear.x  != 0.0) || (cmd->linear.y  != 0.0) || (cmd->linear.z  != 0.0) ||
00201         (cmd->angular.x != 0.0) || (cmd->angular.y != 0.0) || (cmd->angular.z != 0.0))
00202     {
00203       velocity_publisher_.publish(cmd);
00204       last_zero_vel_sent = false;
00205     }
00206     else if (last_zero_vel_sent == false)
00207     {
00208       velocity_publisher_.publish(cmd);
00209       last_zero_vel_sent = true;
00210     }
00211     accept_incoming = true;
00212     ros::spinOnce();
00213     loop_rate.sleep();
00214   }
00215   if (quit_requested)
00216   { // ros node is still ok, send a disable command
00217     disable();
00218   }
00219   else
00220   {
00221     // just in case we got here not via a keyboard quit request
00222     quit_requested = true;
00223     thread.cancel();
00224   }
00225   thread.join();
00226 }
00227 
00228 /*****************************************************************************
00229  ** Implementation [Keyboard]
00230  *****************************************************************************/
00231 
00238 void KeyOpCore::keyboardInputLoop()
00239 {
00240   struct termios raw;
00241   memcpy(&raw, &original_terminal_state, sizeof(struct termios));
00242 
00243   raw.c_lflag &= ~(ICANON | ECHO);
00244   // Setting a new line, then end of file
00245   raw.c_cc[VEOL] = 1;
00246   raw.c_cc[VEOF] = 2;
00247   tcsetattr(key_file_descriptor, TCSANOW, &raw);
00248 
00249   puts("Reading from keyboard");
00250   puts("---------------------------");
00251   puts("Forward/back arrows : linear velocity incr/decr.");
00252   puts("Right/left arrows : angular velocity incr/decr.");
00253   puts("Spacebar : reset linear/angular velocities.");
00254   puts("d : disable motors.");
00255   puts("e : enable motors.");
00256   puts("q : quit.");
00257   char c;
00258   while (!quit_requested)
00259   {
00260     if (read(key_file_descriptor, &c, 1) < 0)
00261     {
00262       perror("read char failed():");
00263       exit(-1);
00264     }
00265     processKeyboardInput(c);
00266   }
00267 }
00268 
00272 void KeyOpCore::remoteKeyInputReceived(const kobuki_msgs::KeyboardInput& key)
00273 {
00274   processKeyboardInput(key.pressedKey);
00275 }
00276 
00282 void KeyOpCore::processKeyboardInput(char c)
00283 {
00284   /*
00285    * Arrow keys are a bit special, they are escape characters - meaning they
00286    * trigger a sequence of keycodes. In this case, 'esc-[-Keycode_xxx'. We
00287    * ignore the esc-[ and just parse the last one. So long as we avoid using
00288    * the last one for its actual purpose (e.g. left arrow corresponds to
00289    * esc-[-D) we can keep the parsing simple.
00290    */
00291   switch (c)
00292   {
00293     case kobuki_msgs::KeyboardInput::KeyCode_Left:
00294     {
00295       incrementAngularVelocity();
00296       break;
00297     }
00298     case kobuki_msgs::KeyboardInput::KeyCode_Right:
00299     {
00300       decrementAngularVelocity();
00301       break;
00302     }
00303     case kobuki_msgs::KeyboardInput::KeyCode_Up:
00304     {
00305       incrementLinearVelocity();
00306       break;
00307     }
00308     case kobuki_msgs::KeyboardInput::KeyCode_Down:
00309     {
00310       decrementLinearVelocity();
00311       break;
00312     }
00313     case kobuki_msgs::KeyboardInput::KeyCode_Space:
00314     {
00315       resetVelocity();
00316       break;
00317     }
00318     case 'q':
00319     {
00320       quit_requested = true;
00321       break;
00322     }
00323     case 'd':
00324     {
00325       disable();
00326       break;
00327     }
00328     case 'e':
00329     {
00330       enable();
00331       break;
00332     }
00333     default:
00334     {
00335       break;
00336     }
00337   }
00338 }
00339 
00340 /*****************************************************************************
00341  ** Implementation [Commands]
00342  *****************************************************************************/
00351 void KeyOpCore::disable()
00352 {
00353   cmd->linear.x = 0.0;
00354   cmd->angular.z = 0.0;
00355   velocity_publisher_.publish(cmd);
00356   accept_incoming = false;
00357 
00358   if (power_status)
00359   {
00360     ROS_INFO("KeyOp: die, die, die (disabling power to the device's motor system).");
00361     kobuki_msgs::MotorPower power_cmd;
00362     power_cmd.state = kobuki_msgs::MotorPower::OFF;
00363     motor_power_publisher_.publish(power_cmd);
00364     power_status = false;
00365   }
00366   else
00367   {
00368     ROS_WARN("KeyOp: Motor system has already been powered down.");
00369   }
00370 }
00371 
00378 void KeyOpCore::enable()
00379 {
00380   accept_incoming = false;
00381 
00382   cmd->linear.x = 0.0;
00383   cmd->angular.z = 0.0;
00384   velocity_publisher_.publish(cmd);
00385 
00386   if (!power_status)
00387   {
00388     ROS_INFO("KeyOp: Enabling power to the device subsystem.");
00389     kobuki_msgs::MotorPower power_cmd;
00390     power_cmd.state = kobuki_msgs::MotorPower::ON;
00391     motor_power_publisher_.publish(power_cmd);
00392     power_status = true;
00393   }
00394   else
00395   {
00396     ROS_WARN("KeyOp: Device has already been powered up.");
00397   }
00398 }
00399 
00403 void KeyOpCore::incrementLinearVelocity()
00404 {
00405   if (power_status)
00406   {
00407     if (cmd->linear.x <= linear_vel_max)
00408     {
00409       cmd->linear.x += linear_vel_step;
00410     }
00411     ROS_INFO_STREAM("KeyOp: linear  velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00412   }
00413   else
00414   {
00415     ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
00416   }
00417 }
00418 
00422 void KeyOpCore::decrementLinearVelocity()
00423 {
00424   if (power_status)
00425   {
00426     if (cmd->linear.x >= -linear_vel_max)
00427     {
00428       cmd->linear.x -= linear_vel_step;
00429     }
00430     ROS_INFO_STREAM("KeyOp: linear  velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00431   }
00432   else
00433   {
00434     ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
00435   }
00436 }
00437 
00441 void KeyOpCore::incrementAngularVelocity()
00442 {
00443   if (power_status)
00444   {
00445     if (cmd->angular.z <= angular_vel_max)
00446     {
00447       cmd->angular.z += angular_vel_step;
00448     }
00449     ROS_INFO_STREAM("KeyOp: angular velocity incremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00450   }
00451   else
00452   {
00453     ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
00454   }
00455 }
00456 
00460 void KeyOpCore::decrementAngularVelocity()
00461 {
00462   if (power_status)
00463   {
00464     if (cmd->angular.z >= -angular_vel_max)
00465     {
00466       cmd->angular.z -= angular_vel_step;
00467     }
00468     ROS_INFO_STREAM("KeyOp: angular velocity decremented [" << cmd->linear.x << "|" << cmd->angular.z << "]");
00469   }
00470   else
00471   {
00472     ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
00473   }
00474 }
00475 
00476 void KeyOpCore::resetVelocity()
00477 {
00478   if (power_status)
00479   {
00480     cmd->angular.z = 0.0;
00481     cmd->linear.x = 0.0;
00482     ROS_INFO_STREAM("KeyOp: reset linear/angular velocities.");
00483   }
00484   else
00485   {
00486     ROS_WARN_STREAM("KeyOp: motors are not yet powered up.");
00487   }
00488 }
00489 
00490 } // namespace keyop_core


kobuki_keyop
Author(s): Daniel Stonier
autogenerated on Mon Oct 6 2014 01:32:18