sick_key.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * Copyright (c) 2013, Intelligent Robotics Lab, DLUT.
00004  * All rights reserved.
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  *     * Redistributions of source code must retain the above copyright
00010  *       notice, this list of conditions and the following disclaimer.
00011  *     * Redistributions in binary form must reproduce the above copyright
00012  *       notice, this list of conditions and the following disclaimer in the
00013  *       documentation and/or other materials provided with the distribution.
00014  *     * Neither the name of the Willow Garage, Inc. and Intelligent Robotics 
00015  *        Lab, DLUT nor the names of its contributors may be used to endorse or
00016  *       promote products derived from this software without specific prior written 
00017  *       permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00020  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00021  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00022  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00023  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00024  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00025  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00026  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00027  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00028  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00029  * POSSIBILITY OF SUCH DAMAGE.
00030 *********************************************************************/
00031 
00032 
00033 #include <ros/ros.h>
00034 #include <dlut_move_base/Velocity.h>
00035 #include <signal.h>
00036 #include <termios.h>
00037 #include <stdio.h>
00038 #include <math.h>
00039 #include <std_msgs/Bool.h>
00040 
00041 #define KEYCODE_R 0x43 
00042 #define KEYCODE_L 0x44
00043 #define KEYCODE_U 0x41
00044 #define KEYCODE_D 0x42
00045 #define KEYCODE_Q 0x71
00046 #define KEYCODE_SPACE 0x20
00047 #define KEYCODE_0 0x30
00048 #define KEYCODE_ENTER 0x0A
00049 
00050 class TeleopTurtle
00051 {
00052 public:
00053   TeleopTurtle();
00054   void keyLoop(dlut_move_base::Velocity &vel);
00055 
00056 private:  
00057   ros::NodeHandle nh_;
00058   double linear_, angular_, l_scale_, a_scale_;
00059   ros::Publisher vel_pub_;
00060   ros::Publisher is_run_pub1_,is_run_pub2_;
00061   std_msgs::Bool is_motor_run_;  
00062 };
00063 
00064 TeleopTurtle::TeleopTurtle():
00065   linear_(0.0),
00066   angular_(0.0),
00067   l_scale_(0.1),
00068   a_scale_(0.1)
00069 {
00070   nh_.param("scale_angular", a_scale_, a_scale_);
00071   nh_.param("scale_linear", l_scale_, l_scale_);
00072 
00073   vel_pub_ = nh_.advertise<dlut_move_base::Velocity>("cmd_velocity", 1);
00074   is_run_pub1_ = nh_.advertise<std_msgs::Bool>("is_run", 1000);
00075   is_run_pub2_ = nh_.advertise<std_msgs::Bool>("is_run_pch", 1000);
00076 }
00077 
00078 int g_kfd = 0;
00079 struct termios g_cooked, g_raw;
00080 
00081 void quit(int sig)
00082 {
00083   tcsetattr(g_kfd, TCSANOW, &g_cooked);
00084   ros::shutdown();
00085   exit(0);
00086 }
00087 
00088 int main(int argc, char** argv)
00089 {
00090   ros::init(argc, argv, "teleop_key");
00091   TeleopTurtle teleop_turtle;
00092   dlut_move_base::Velocity vel;
00093   vel.angular = 0.0;
00094   vel.linear = 0.0;
00095 
00096   signal(SIGINT,quit);
00097 
00098   teleop_turtle.keyLoop(vel);
00099   
00100   return(0);
00101 }
00102 
00103 void TeleopTurtle::keyLoop(dlut_move_base::Velocity &vel)
00104 {
00105   char c;                         //the value of the key pressed
00106   bool is_control_command = false;//is control command or not
00107   bool is_data_command = false;   //is data command or not
00108 
00109   // get the console in g_raw mode                                                              
00110   tcgetattr(g_kfd, &g_cooked);
00111   memcpy(&g_raw, &g_cooked, sizeof(struct termios));
00112   g_raw.c_lflag &= ~ (ICANON | ECHO);
00113   // Setting a new line, then end of file                         
00114   g_raw.c_cc[VEOL] = 1;
00115   g_raw.c_cc[VEOF] = 2;
00116   tcsetattr(g_kfd, TCSANOW, &g_raw);
00117 
00118   puts("Reading from keyboard");
00119   puts("---------------------------");
00120   puts("Use arrow keys to move the turtle.");
00121 
00122   //the robot stopped moving or not
00123   bool is_stop_move = 0;
00124 
00125   //the robot begin getting the laser data or not
00126   int begin_get_data = 0; 
00127 
00128   //the robot stop getting the laser data or not
00129   int stop_get_data = 0;  
00130 
00131   for(;;)
00132   {
00133     // get the next event from the keyboard  
00134     if(read(g_kfd, &c, 1) < 0)
00135     {
00136       perror("read():");
00137       exit(-1);
00138     }
00139 
00140     linear_ = angular_ = 0.0;
00141     ROS_DEBUG("value: 0x%02X\n", c);
00142  
00143     switch(c)
00144     {
00145       case KEYCODE_L:
00146         ROS_DEBUG("LEFT");
00147         angular_ = 1.0;
00148         is_control_command = true;
00149         break;
00150       case KEYCODE_R:
00151         ROS_DEBUG("RIGHT");
00152         angular_ = -1.0;
00153         is_control_command = true;
00154         break;
00155       case KEYCODE_U:
00156         ROS_DEBUG("UP");
00157         linear_ = 1.0;
00158         is_control_command = true;
00159         break;
00160       case KEYCODE_D:
00161         ROS_DEBUG("DOWN");
00162         linear_ = -1.0;
00163         is_control_command = true;
00164         break;
00165       case KEYCODE_SPACE:
00166         ROS_DEBUG("STOP");
00167         is_stop_move = 1;
00168         is_control_command = true;
00169         break;
00170       case KEYCODE_0:
00171         ROS_DEBUG("BEGIN");
00172         begin_get_data = 1;
00173         is_data_command = true;
00174         break;
00175       case KEYCODE_ENTER:
00176         ROS_DEBUG("END");
00177         stop_get_data = 1;
00178         is_data_command = true;
00179         break;
00180       default :
00181         break;  
00182     }
00183 
00184     vel.angular = vel.angular + a_scale_*angular_;
00185     vel.linear = vel.linear + l_scale_*linear_;
00186 
00187     //add the following two if in case the function gcvt convert 0 to a numner very near to 0
00188     if(vel.angular > -0.001 && vel.angular < 0.001)
00189     {
00190       vel.angular = 0.0;        
00191     }
00192 
00193     if(vel.linear > -0.001 && vel.linear < 0.001)//the robot runs faster at 0.05 than                                                           at 0.1,it's abnormal
00194     {
00195       vel.linear = 0.0; 
00196     }
00197 
00198     if(is_stop_move == 1)
00199     {
00200       vel.angular = 0.0;
00201       vel.linear = 0.0; 
00202       is_stop_move = 0; 
00203     }
00204 
00205     if(begin_get_data == 1)
00206     {
00207       is_motor_run_.data = true;
00208       begin_get_data = 0;
00209     }
00210 
00211     if(stop_get_data == 1)
00212     {
00213       is_motor_run_.data = false;
00214       stop_get_data = 0;
00215     }
00216 
00217     if(is_control_command == true)//is control command, so publish it
00218     {
00219       vel_pub_.publish(vel); 
00220       is_control_command = false;
00221     }
00222 
00223     if(is_data_command == true)
00224     {
00225       is_run_pub1_.publish(is_motor_run_);//tell the motor under the sick to rotate
00226       is_run_pub2_.publish(is_motor_run_);//publish the signal to save the laser data or not
00227       is_data_command = false;
00228     }
00229   }
00230 
00231   return;
00232 }


dlut_move_base
Author(s): Zhuang Yan , Yan Fei, Dong Bing Bing
autogenerated on Sat Jun 8 2019 19:55:39