keyboard_teleop.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2015, PickNik LLC
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the PickNik LLC nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Dave Coleman <dave@dav.ee>, Andy McEvoy
36  Desc: Tweak a TF transform using a keyboard
37 */
38 
39 #include <termios.h>
40 #include <signal.h>
41 #include <stdio.h>
42 #include <stdlib.h>
43 
44 #include <ros/ros.h>
45 #include <sensor_msgs/JointState.h>
46 #include <std_msgs/Float64MultiArray.h>
47 
48 #define KEYCODE_a 0x61
49 #define KEYCODE_b 0x62
50 #define KEYCODE_c 0x63
51 #define KEYCODE_d 0x64
52 #define KEYCODE_e 0x65
53 #define KEYCODE_f 0x66
54 #define KEYCODE_g 0x67
55 #define KEYCODE_h 0x68
56 #define KEYCODE_i 0x69
57 #define KEYCODE_j 0x6a
58 #define KEYCODE_k 0x6b
59 #define KEYCODE_l 0x6c
60 #define KEYCODE_m 0x6d
61 #define KEYCODE_n 0x6e
62 #define KEYCODE_o 0x6f
63 #define KEYCODE_p 0x70
64 #define KEYCODE_q 0x71
65 #define KEYCODE_r 0x72
66 #define KEYCODE_s 0x73
67 #define KEYCODE_t 0x74
68 #define KEYCODE_u 0x75
69 #define KEYCODE_v 0x76
70 #define KEYCODE_w 0x77
71 #define KEYCODE_x 0x78
72 #define KEYCODE_y 0x79
73 #define KEYCODE_z 0x7a
74 #define KEYCODE_ESCAPE 0x1B
75 
76 int kfd = 0;
77 struct termios cooked, raw;
78 
79 void quit(int sig)
80 {
81  tcsetattr(kfd, TCSANOW, &cooked);
82  exit(0);
83 }
84 
86 {
87 public:
88 
90  : has_recieved_joints_(false)
91  {
92  std::cout << "init " << std::endl;
93  // TODO: make this robot agonistic
94  joints_sub_ = nh_.subscribe<sensor_msgs::JointState>("/iiwa_7_r800/joint_states", 1,
96  joints_pub_ = nh_.advertise<std_msgs::Float64MultiArray>("/iiwa_7_r800/joints_position_controller/command", 1);
97  cmd_.data.resize(7);
98  }
99 
101  { }
102 
103  void stateCallback(const sensor_msgs::JointStateConstPtr& msg)
104  {
105  if(msg->position.size() != 7)
106  {
107  ROS_ERROR_STREAM("Not enough joints!");
108  exit(-1);
109  }
110 
111  // Copy latest joint positions to our output message
113  cmd_.data = msg->position;
114 
115  // Debug
116  //std::copy(cmd_.data.begin(), cmd_.data.end(), std::ostream_iterator<double>(std::cout, " "));
117  //std::cout << std::endl;
118 
119  // Important safety feature
120  has_recieved_joints_ = true;
121  }
122 
124  {
125  char c;
126  bool dirty=false;
127 
128  // get the console in raw mode
129  tcgetattr(kfd, &cooked);
130  memcpy(&raw, &cooked, sizeof(struct termios));
131  raw.c_lflag &=~ (ICANON | ECHO);
132  // Setting a new line, then end of file
133  raw.c_cc[VEOL] = 1;
134  raw.c_cc[VEOF] = 2;
135  tcsetattr(kfd, TCSANOW, &raw);
136 
137  puts("Reading from keyboard");
138  puts("---------------------------");
139  puts("Use 'QA' to for joint 1");
140  puts("Use 'WS' to for joint 2");
141  puts("Use 'ED' to for joint 3");
142  puts("Use 'RF' to for joint 4");
143  puts("Use 'TG' to for joint 5");
144  puts("Use 'YH' to for joint 6");
145  puts("Use 'UJ' to for joint 7");
146  puts("ESC to end");
147 
148  double delta_dist = 0.005;
149 
150  for(;;)
151  {
152  // get the next event from the keyboard
153  if(read(kfd, &c, 1) < 0)
154  {
155  perror("read():");
156  exit(-1);
157  }
158 
159  dirty = true;
160  switch(c)
161  {
162  case KEYCODE_q:
163  cmd_.data[0] = cmd_.data[0] + delta_dist; // radians
164  break;
165  case KEYCODE_a:
166  cmd_.data[0] = cmd_.data[0] - delta_dist; // radians
167  break;
168 
169  case KEYCODE_w:
170  cmd_.data[1] = cmd_.data[1] + delta_dist; // radians
171  break;
172  case KEYCODE_s:
173  cmd_.data[1] = cmd_.data[1] - delta_dist; // radians
174  break;
175 
176  case KEYCODE_e:
177  cmd_.data[2] = cmd_.data[2] + delta_dist; // radians
178  break;
179  case KEYCODE_d:
180  cmd_.data[2] = cmd_.data[2] - delta_dist; // radians
181  break;
182 
183  case KEYCODE_r:
184  cmd_.data[3] = cmd_.data[3] + delta_dist; // radians
185  break;
186  case KEYCODE_f:
187  cmd_.data[3] = cmd_.data[3] - delta_dist; // radians
188  break;
189 
190  case KEYCODE_t:
191  cmd_.data[4] = cmd_.data[4] + delta_dist; // radians
192  break;
193  case KEYCODE_g:
194  cmd_.data[4] = cmd_.data[4] - delta_dist; // radians
195  break;
196 
197  case KEYCODE_y:
198  cmd_.data[5] = cmd_.data[5] + delta_dist; // radians
199  break;
200  case KEYCODE_h:
201  cmd_.data[5] = cmd_.data[5] - delta_dist; // radians
202  break;
203 
204  case KEYCODE_u:
205  cmd_.data[6] = cmd_.data[6] + delta_dist; // radians
206  break;
207  case KEYCODE_j:
208  cmd_.data[6] = cmd_.data[6] - delta_dist; // radians
209  break;
210 
211  case KEYCODE_ESCAPE:
212  std::cout << std::endl;
213  std::cout << "Exiting " << std::endl;
214  quit(0);
215  break;
216 
217  default:
218  std::cout << "CODE: " << c << std::endl;
219  dirty = false;
220  }
221 
222  // Publish command
223  if (dirty)
224  {
225  // Important safety feature
227  {
228  ROS_ERROR_STREAM_NAMED("joint_teleop","Unable to send joint commands because robot state is invalid");
229  } else {
230  std::cout << ".";
232  }
233  }
234  }
235  }
236 
237 private:
238 
242  std_msgs::Float64MultiArray cmd_;
244 
245 };
246 
247 int main(int argc, char** argv)
248 {
249  ros::init(argc, argv, "joints_teleop_keyboard");
250  signal(SIGINT,quit);
251 
252  // NOTE: We run the ROS loop in a separate thread as external calls such
253  // as service callbacks to load controllers can block the (main) control loop
255  spinner.start();
256 
257  TeleopJointsKeyboard teleop;
258  teleop.keyboardLoop();
259 
260  return(0);
261 }
#define KEYCODE_j
void publish(const boost::shared_ptr< M > &message) const
#define ROS_ERROR_STREAM_NAMED(name, args)
#define KEYCODE_u
#define KEYCODE_y
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
#define KEYCODE_t
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
std_msgs::Float64MultiArray cmd_
void spinner()
ros::Subscriber joints_sub_
void quit(int sig)
#define KEYCODE_q
#define KEYCODE_h
#define KEYCODE_s
#define KEYCODE_r
void stateCallback(const sensor_msgs::JointStateConstPtr &msg)
struct termios cooked raw
#define KEYCODE_g
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
#define KEYCODE_f
#define KEYCODE_d
#define KEYCODE_e
int kfd
#define KEYCODE_a
ros::Publisher joints_pub_
#define KEYCODE_w
#define KEYCODE_ESCAPE
#define ROS_ERROR_STREAM(args)
int main(int argc, char **argv)


ros_control_boilerplate
Author(s): Dave Coleman
autogenerated on Sat Jun 8 2019 18:06:50