keyboard_teleop.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Willow Garage, Inc.
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 the Willow Garage, Inc. 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 
00030 #include <ros/ros.h>
00031 #include <geometry_msgs/Twist.h>
00032 #include <signal.h>
00033 #include <termios.h>
00034 #include <stdio.h>
00035 #include "boost/thread/mutex.hpp"
00036 #include "boost/thread/thread.hpp"
00037 
00038 #define KEYCODE_R 0x43
00039 #define KEYCODE_L 0x44
00040 #define KEYCODE_U 0x41
00041 #define KEYCODE_D 0x42
00042 #define KEYCODE_Q 0x71
00043 
00044 class TurtlebotTeleop
00045 {
00046 public:
00047   TurtlebotTeleop();
00048   void keyLoop();
00049   void watchdog();
00050 
00051 private:
00052 
00053 
00054   ros::NodeHandle nh_,ph_;
00055   double linear_, angular_;
00056   ros::Time first_publish_;
00057   ros::Time last_publish_;
00058   double l_scale_, a_scale_;
00059   ros::Publisher vel_pub_;
00060   void publish(double, double);
00061   boost::mutex publish_mutex_;
00062 
00063 };
00064 
00065 TurtlebotTeleop::TurtlebotTeleop():
00066   ph_("~"),
00067   linear_(0),
00068   angular_(0),
00069   l_scale_(1.0),
00070   a_scale_(1.0)
00071 {
00072   ph_.param("scale_angular", a_scale_, a_scale_);
00073   ph_.param("scale_linear", l_scale_, l_scale_);
00074 
00075   vel_pub_ = nh_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
00076 }
00077 
00078 int kfd = 0;
00079 struct termios cooked, raw;
00080 
00081 void quit(int sig)
00082 {
00083   tcsetattr(kfd, TCSANOW, &cooked);
00084   ros::shutdown();
00085   exit(0);
00086 }
00087 
00088 
00089 int main(int argc, char** argv)
00090 {
00091   ros::init(argc, argv, "turtlebot_teleop");
00092   TurtlebotTeleop turtlebot_teleop;
00093   ros::NodeHandle n;
00094 
00095   signal(SIGINT,quit);
00096 
00097   boost::thread my_thread(boost::bind(&TurtlebotTeleop::keyLoop, &turtlebot_teleop));
00098 
00099 
00100   ros::Timer timer = n.createTimer(ros::Duration(0.1), boost::bind(&TurtlebotTeleop::watchdog, &turtlebot_teleop));
00101 
00102   ros::spin();
00103 
00104   my_thread.interrupt() ;
00105   my_thread.join() ;
00106 
00107   return(0);
00108 }
00109 
00110 
00111 void TurtlebotTeleop::watchdog()
00112 {
00113   boost::mutex::scoped_lock lock(publish_mutex_);
00114   if ((ros::Time::now() > last_publish_ + ros::Duration(0.15)) &&
00115       (ros::Time::now() > first_publish_ + ros::Duration(0.50)))
00116     publish(0, 0);
00117 }
00118 
00119 void TurtlebotTeleop::keyLoop()
00120 {
00121   char c;
00122 
00123 
00124   // get the console in raw mode
00125   tcgetattr(kfd, &cooked);
00126   memcpy(&raw, &cooked, sizeof(struct termios));
00127   raw.c_lflag &=~ (ICANON | ECHO);
00128   // Setting a new line, then end of file
00129   raw.c_cc[VEOL] = 1;
00130   raw.c_cc[VEOF] = 2;
00131   tcsetattr(kfd, TCSANOW, &raw);
00132 
00133   puts("Reading from keyboard");
00134   puts("---------------------------");
00135   puts("Use arrow keys to move the turtlebot.");
00136 
00137 
00138   while (ros::ok())
00139   {
00140     // get the next event from the keyboard
00141     if(read(kfd, &c, 1) < 0)
00142     {
00143       perror("read():");
00144       exit(-1);
00145     }
00146 
00147 
00148     linear_=angular_=0;
00149     ROS_DEBUG("value: 0x%02X\n", c);
00150 
00151     switch(c)
00152     {
00153       case KEYCODE_L:
00154         ROS_DEBUG("LEFT");
00155         angular_ = 1.0;
00156         break;
00157       case KEYCODE_R:
00158         ROS_DEBUG("RIGHT");
00159         angular_ = -1.0;
00160         break;
00161       case KEYCODE_U:
00162         ROS_DEBUG("UP");
00163         linear_ = 1.0;
00164         break;
00165       case KEYCODE_D:
00166         ROS_DEBUG("DOWN");
00167         linear_ = -1.0;
00168         break;
00169     }
00170     boost::mutex::scoped_lock lock(publish_mutex_);
00171     if (ros::Time::now() > last_publish_ + ros::Duration(1.0)) {
00172       first_publish_ = ros::Time::now();
00173     }
00174     last_publish_ = ros::Time::now();
00175     publish(angular_, linear_);
00176   }
00177 
00178   return;
00179 }
00180 
00181 void TurtlebotTeleop::publish(double angular, double linear)
00182 {
00183     geometry_msgs::Twist vel;
00184     vel.angular.z = a_scale_*angular;
00185     vel.linear.x = l_scale_*linear;
00186 
00187     vel_pub_.publish(vel);
00188 
00189 
00190   return;
00191 }


gapter
Author(s):
autogenerated on Thu Jun 6 2019 22:05:13