keyboard_teleop.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2015, PickNik LLC
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of the PickNik LLC nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Dave Coleman <dave@dav.ee>, Andy McEvoy
00036    Desc:   Tweak a TF transform using a keyboard
00037 */
00038 
00039 #include <termios.h>
00040 #include <signal.h>
00041 #include <stdio.h>
00042 #include <stdlib.h>
00043 
00044 #include <ros/ros.h>
00045 #include <sensor_msgs/JointState.h>
00046 #include <std_msgs/Float64MultiArray.h>
00047 
00048 #define KEYCODE_a 0x61
00049 #define KEYCODE_b 0x62
00050 #define KEYCODE_c 0x63
00051 #define KEYCODE_d 0x64
00052 #define KEYCODE_e 0x65
00053 #define KEYCODE_f 0x66
00054 #define KEYCODE_g 0x67
00055 #define KEYCODE_h 0x68
00056 #define KEYCODE_i 0x69
00057 #define KEYCODE_j 0x6a
00058 #define KEYCODE_k 0x6b
00059 #define KEYCODE_l 0x6c
00060 #define KEYCODE_m 0x6d
00061 #define KEYCODE_n 0x6e
00062 #define KEYCODE_o 0x6f
00063 #define KEYCODE_p 0x70
00064 #define KEYCODE_q 0x71
00065 #define KEYCODE_r 0x72
00066 #define KEYCODE_s 0x73
00067 #define KEYCODE_t 0x74
00068 #define KEYCODE_u 0x75
00069 #define KEYCODE_v 0x76
00070 #define KEYCODE_w 0x77
00071 #define KEYCODE_x 0x78
00072 #define KEYCODE_y 0x79
00073 #define KEYCODE_z 0x7a
00074 #define KEYCODE_ESCAPE  0x1B
00075 
00076 int kfd = 0;
00077 struct termios cooked, raw;
00078 
00079 void quit(int sig)
00080 {
00081   tcsetattr(kfd, TCSANOW, &cooked);
00082   exit(0);
00083 }
00084 
00085 class TeleopJointsKeyboard
00086 {
00087 public:
00088 
00089   TeleopJointsKeyboard()
00090     : has_recieved_joints_(false)
00091   {
00092     std::cout << "init " << std::endl;
00093     // TODO: make this robot agonistic
00094     joints_sub_ = nh_.subscribe<sensor_msgs::JointState>("/iiwa_7_r800/joint_states", 1,
00095                                                          &TeleopJointsKeyboard::stateCallback, this);
00096     joints_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/iiwa_7_r800/joints_position_controller/command", 1);
00097     cmd_.data.resize(7);
00098   }
00099 
00100   ~TeleopJointsKeyboard()
00101   { }
00102 
00103   void stateCallback(const sensor_msgs::JointStateConstPtr& msg)
00104   {
00105     if(msg->position.size() != 7)
00106     {
00107       ROS_ERROR_STREAM("Not enough joints!");
00108       exit(-1);
00109     }
00110 
00111     // Copy latest joint positions to our output message
00112     if (!has_recieved_joints_)
00113       cmd_.data = msg->position;
00114 
00115     // Debug
00116     //std::copy(cmd_.data.begin(), cmd_.data.end(), std::ostream_iterator<double>(std::cout, " "));
00117     //std::cout << std::endl;
00118 
00119     // Important safety feature
00120     has_recieved_joints_ = true;
00121   }
00122 
00123   void keyboardLoop()
00124   {
00125     char c;
00126     bool dirty=false;
00127 
00128     // get the console in raw mode
00129     tcgetattr(kfd, &cooked);
00130     memcpy(&raw, &cooked, sizeof(struct termios));
00131     raw.c_lflag &=~ (ICANON | ECHO);
00132     // Setting a new line, then end of file
00133     raw.c_cc[VEOL] = 1;
00134     raw.c_cc[VEOF] = 2;
00135     tcsetattr(kfd, TCSANOW, &raw);
00136 
00137     puts("Reading from keyboard");
00138     puts("---------------------------");
00139     puts("Use 'QA' to for joint 1");
00140     puts("Use 'WS' to for joint 2");
00141     puts("Use 'ED' to for joint 3");
00142     puts("Use 'RF' to for joint 4");
00143     puts("Use 'TG' to for joint 5");
00144     puts("Use 'YH' to for joint 6");
00145     puts("Use 'UJ' to for joint 7");
00146     puts("ESC to end");
00147 
00148     double delta_dist = 0.005;
00149 
00150     for(;;)
00151     {
00152       // get the next event from the keyboard
00153       if(read(kfd, &c, 1) < 0)
00154       {
00155         perror("read():");
00156         exit(-1);
00157       }
00158 
00159       dirty = true;
00160       switch(c)
00161       {
00162         case KEYCODE_q:
00163           cmd_.data[0] = cmd_.data[0] + delta_dist; // radians
00164           break;
00165         case KEYCODE_a:
00166           cmd_.data[0] = cmd_.data[0] - delta_dist; // radians
00167           break;
00168 
00169         case KEYCODE_w:
00170           cmd_.data[1] = cmd_.data[1] + delta_dist; // radians
00171           break;
00172         case KEYCODE_s:
00173           cmd_.data[1] = cmd_.data[1] - delta_dist; // radians
00174           break;
00175 
00176         case KEYCODE_e:
00177           cmd_.data[2] = cmd_.data[2] + delta_dist; // radians
00178           break;
00179         case KEYCODE_d:
00180           cmd_.data[2] = cmd_.data[2] - delta_dist; // radians
00181           break;
00182 
00183         case KEYCODE_r:
00184           cmd_.data[3] = cmd_.data[3] + delta_dist; // radians
00185           break;
00186         case KEYCODE_f:
00187           cmd_.data[3] = cmd_.data[3] - delta_dist; // radians
00188           break;
00189 
00190         case KEYCODE_t:
00191           cmd_.data[4] = cmd_.data[4] + delta_dist; // radians
00192           break;
00193         case KEYCODE_g:
00194           cmd_.data[4] = cmd_.data[4] - delta_dist; // radians
00195           break;
00196 
00197         case KEYCODE_y:
00198           cmd_.data[5] = cmd_.data[5] + delta_dist; // radians
00199           break;
00200         case KEYCODE_h:
00201           cmd_.data[5] = cmd_.data[5] - delta_dist; // radians
00202           break;
00203 
00204         case KEYCODE_u:
00205           cmd_.data[6] = cmd_.data[6] + delta_dist; // radians
00206           break;
00207         case KEYCODE_j:
00208           cmd_.data[6] = cmd_.data[6] - delta_dist; // radians
00209           break;
00210 
00211         case  KEYCODE_ESCAPE:
00212           std::cout << std::endl;
00213           std::cout << "Exiting " << std::endl;
00214           quit(0);
00215           break;
00216 
00217         default:
00218           std::cout << "CODE: "  << c << std::endl;
00219           dirty = false;
00220       }
00221 
00222       // Publish command
00223       if (dirty)
00224       {
00225         // Important safety feature
00226         if (!has_recieved_joints_)
00227         {
00228           ROS_ERROR_STREAM_NAMED("joint_teleop","Unable to send joint commands because robot state is invalid");
00229         } else {
00230           std::cout << ".";
00231           joints_pub_.publish(cmd_);
00232         }
00233       }
00234     }
00235   }
00236 
00237 private:
00238 
00239   ros::NodeHandle nh_;
00240   ros::Publisher joints_pub_;
00241   ros::Subscriber joints_sub_;
00242   std_msgs::Float64MultiArray cmd_;
00243   bool has_recieved_joints_;
00244 
00245 };
00246 
00247 int main(int argc, char** argv)
00248 {
00249   ros::init(argc, argv, "joints_teleop_keyboard");
00250   signal(SIGINT,quit);
00251 
00252   // NOTE: We run the ROS loop in a separate thread as external calls such
00253   // as service callbacks to load controllers can block the (main) control loop
00254   ros::AsyncSpinner spinner(1);
00255   spinner.start();
00256 
00257   TeleopJointsKeyboard teleop;
00258   teleop.keyboardLoop();
00259 
00260   return(0);
00261 }


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sun Apr 24 2016 04:39:29