katana_teleop_key.cpp
Go to the documentation of this file.
1 /*
2  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
3  * Copyright (C) 2011 University of Osnabrück
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software
17  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
18  *
19  * katana_teleop_key.cpp
20  *
21  * Created on: 21.04.2011
22  * Author: Henning Deeken <hdeeken@uos.de>
23  *
24  * based on a pr2 teleop by Kevin Watts
25  */
26 
28 
29 namespace katana
30 {
31 
33  action_client("katana_arm_controller/joint_movement_action", true), gripper_("gripper_grasp_posture_controller", true)
34 {
35  ROS_INFO("KatanaTeleopKey starting...");
36  ros::NodeHandle n_;
37  ros::NodeHandle n_private("~");
38 
39  n_.param("increment", increment, 0.017453293); // default increment = 1°
40  n_.param("increment_step", increment_step, 0.017453293); // default step_increment = 1°
41  n_.param("increment_step_scaling", increment_step_scaling, 1.0); // default scaling = 1
42 
43  js_sub_ = n_.subscribe("joint_states", 1000, &KatanaTeleopKey::jointStateCallback, this);
44 
45  got_joint_states_ = false;
46 
47  jointIndex = 0;
48 
51 
52  // Gets all of the joints
53  XmlRpc::XmlRpcValue joint_names;
54 
55  // Gets all of the joints
56  if (!n_.getParam("katana_joints", joint_names))
57  {
58  ROS_ERROR("No joints given. (namespace: %s)", n_.getNamespace().c_str());
59  }
60  joint_names_.resize(joint_names.size());
61 
62  if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
63  {
64  ROS_ERROR("Malformed joint specification. (namespace: %s)", n_.getNamespace().c_str());
65  }
66 
67  for (size_t i = 0; (int)i < joint_names.size(); ++i)
68  {
69  XmlRpc::XmlRpcValue &name_value = joint_names[i];
70 
71  if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
72  {
73  ROS_ERROR("Array of joint names should contain all strings. (namespace: %s)",
74  n_.getNamespace().c_str());
75  }
76 
77  joint_names_[i] = (std::string)name_value;
78  }
79 
80  // Gets all of the gripper joints
81  XmlRpc::XmlRpcValue gripper_joint_names;
82 
83  // Gets all of the joints
84  if (!n_.getParam("katana_gripper_joints", gripper_joint_names))
85  {
86  ROS_ERROR("No gripper joints given. (namespace: %s)", n_.getNamespace().c_str());
87  }
88 
89  gripper_joint_names_.resize(gripper_joint_names.size());
90 
91  if (gripper_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
92  {
93  ROS_ERROR("Malformed gripper joint specification. (namespace: %s)", n_.getNamespace().c_str());
94  }
95  for (size_t i = 0; (int)i < gripper_joint_names.size(); ++i)
96  {
97  XmlRpc::XmlRpcValue &name_value = gripper_joint_names[i];
98  if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
99  {
100  ROS_ERROR("Array of gripper joint names should contain all strings. (namespace: %s)",
101  n_.getNamespace().c_str());
102  }
103 
104  gripper_joint_names_[i] = (std::string)name_value;
105  }
106 
107  combined_joints_.resize(joint_names_.size() + gripper_joint_names_.size());
108 
109  for (unsigned int i = 0; i < joint_names_.size(); i++)
110  {
112  }
113 
114  for (unsigned int i = 0; i < gripper_joint_names_.size(); i++)
115  {
117  }
118 
119  giveInfo();
120 
121 }
122 
124 {
125  ROS_INFO("---------------------------");
126  ROS_INFO("Use 'WS' to increase/decrease the joint position about one increment");
127  ROS_INFO("Current increment is set to: %f", increment);
128  ROS_INFO("Use '+#' to alter the increment by a increment/decrement of: %f", increment_step);
129  ROS_INFO("Use ',.' to alter the increment_step_size altering the scaling factor by -/+ 1.0");
130  ROS_INFO("Current scaling is set to: %f" , increment_step_scaling);
131  ROS_INFO("---------------------------");
132  ROS_INFO("Use 'R' to return to the arm's initial pose");
133  ROS_INFO("Use 'I' to display this manual and the current joint state");
134  ROS_INFO("---------------------------");
135  ROS_INFO("Use 'AD' to switch to the next/previous joint");
136  ROS_INFO("Use '0-9' to select a joint by number");
137  ROS_INFO("---------------------------");
138  ROS_INFO("Use 'OC' to open/close gripper");
139 
140  for (unsigned int i = 0; i < joint_names_.size(); i++)
141  {
142  ROS_INFO("Use '%d' to switch to Joint: '%s'",i, joint_names_[i].c_str());
143  }
144 
145  for (unsigned int i = 0; i < gripper_joint_names_.size(); i++)
146  {
147  ROS_INFO("Use '%zu' to switch to Gripper Joint: '%s'",i + joint_names_.size(), gripper_joint_names_[i].c_str());
148  }
149 
150  if (!current_pose_.name.empty())
151  {
152  ROS_INFO("---------------------------");
153  ROS_INFO("Current Joint Positions:");
154 
155  for (unsigned int i = 0; i < current_pose_.position.size(); i++)
156  {
157  ROS_INFO("Joint %d - %s: %f", i, current_pose_.name[i].c_str(), current_pose_.position[i]);
158  }
159  }
160 }
161 
162 void KatanaTeleopKey::jointStateCallback(const sensor_msgs::JointState::ConstPtr& js)
163 {
164  // ROS_INFO("KatanaTeleopKeyboard received a new JointState");
165 
166  current_pose_.name = js->name;
167  current_pose_.position = js->position;
168 
169  if (!got_joint_states_)
170  {
171  // ROS_INFO("KatanaTeleopKeyboard received initial JointState");
172  initial_pose_.name = js->name;
173  initial_pose_.position = js->position;
174  got_joint_states_ = true;
175  }
176 }
177 
179 {
180  bool found_match = false;
181 
182  for (unsigned int i = 0; i < current_pose_.name.size(); i++)
183  {
184  if (current_pose_.name[i] == combined_joints_[jointIndex])
185  {
186  //ROS_DEBUG("incoming inc: %f - curren_pose: %f - resulting pose: %f ",increment, current_pose_.position[i], current_pose_.position[i] + increment);
187  movement_goal_.position.push_back(current_pose_.position[i] + increment);
188  found_match = true;
189  break;
190 
191  }
192  }
193 
194  return found_match;
195 }
196 
198 {
199 
200  char c;
201  bool dirty = true;
202  bool shutdown = false;
203 
204  // get the console in raw mode
205  tcgetattr(kfd, &cooked);
206  memcpy(&raw, &cooked, sizeof(struct termios));
207  raw.c_lflag &= ~(ICANON | ECHO);
208  // Setting a new line, then end of file
209  raw.c_cc[VEOL] = 1;
210  raw.c_cc[VEOF] = 2;
211  tcsetattr(kfd, TCSANOW, &raw);
212 
213  ros::Rate r(50.0); // 50 Hz
214 
215  while (ros::ok() && !shutdown)
216  {
217  r.sleep();
218  ros::spinOnce();
219 
220  if (!got_joint_states_)
221  continue;
222 
223  dirty = false;
224 
225  // get the next event from the keyboard
226  if (read(kfd, &c, 1) < 0)
227  {
228  perror("read():");
229  exit(-1);
230  }
231 
232  size_t selected_joint_index;
233  switch (c)
234  {
235  // Increasing/Decreasing JointPosition
236  case KEYCODE_W:
238  {
239  movement_goal_.name.push_back(combined_joints_[jointIndex]);
240  dirty = true;
241  }
242  else
243  {
244  ROS_WARN("movement with the desired joint: %s failed due to a mismatch with the current joint state", combined_joints_[jointIndex].c_str());
245  }
246 
247  break;
248 
249  case KEYCODE_S:
251  {
252  movement_goal_.name.push_back(combined_joints_[jointIndex]);
253  dirty = true;
254  }
255  else
256  {
257  ROS_WARN("movement with the desired joint: %s failed due to a mismatch with the current joint state", combined_joints_[jointIndex].c_str());
258  }
259 
260  break;
261 
262  // Switching active Joint
263  case KEYCODE_D:
264  // use this line if you want to use "the gripper" instead of the single gripper joints
265  jointIndex = (jointIndex + 1) % (joint_names_.size() + 1);
266 
267  // use this line if you want to select specific gripper joints
268  //jointIndex = (jointIndex + 1) % combined_joints_.size();
269  break;
270 
271  case KEYCODE_A:
272  // use this line if you want to use "the gripper" instead of the single gripper joints
273  jointIndex = (jointIndex - 1) % (joint_names_.size() + 1);
274 
275  // use this line if you want to select specific gripper joints
276  //jointIndex = (jointIndex - 1) % combined_joints_.size();
277 
278  break;
279 
280  case KEYCODE_R:
281  ROS_INFO("Resetting arm to its initial pose..");
282 
283  movement_goal_.name = initial_pose_.name;
284  movement_goal_.position = initial_pose_.position;
285  dirty = true;
286  break;
287 
288  case KEYCODE_Q:
289  // in case of shutting down the teleop node the arm is moved back into it's initial pose
290  // assuming that this is a proper resting pose for the arm
291 
292  ROS_INFO("Shutting down the Katana Teleoperation node...");
293  shutdown = true;
294  break;
295 
296  case KEYCODE_I:
297  giveInfo();
298  break;
299 
300  case KEYCODE_0:
301  case KEYCODE_1:
302  case KEYCODE_2:
303  case KEYCODE_3:
304  case KEYCODE_4:
305  case KEYCODE_5:
306  case KEYCODE_6:
307  case KEYCODE_7:
308  case KEYCODE_8:
309  case KEYCODE_9:
310  selected_joint_index = c - KEYCODE_0;
311 
312  if (combined_joints_.size() > jointIndex)
313  {
314  ROS_DEBUG("You choose to adress joint no. %zu: %s", selected_joint_index, combined_joints_[9].c_str());
315  jointIndex = selected_joint_index;
316  }
317  else
318  {
319  ROS_WARN("Joint Index No. %zu can not be adressed!", jointIndex);
320  }
321  break;
322 
323  case KEYCODE_PLUS:
325  ROS_DEBUG("Increment increased to: %f",increment);
326  break;
327 
328  case KEYCODE_NUMBER:
330  if (increment < 0)
331  {
332  increment = 0.0;
333  }
334  ROS_DEBUG("Increment decreased to: %f",increment);
335  break;
336 
337  case KEYCODE_POINT:
338  increment_step_scaling += 1.0;
339  ROS_DEBUG("Increment_Scaling increased to: %f",increment_step_scaling);
340  break;
341 
342  case KEYCODE_COMMA:
343  increment_step_scaling -= 1.0;
344  ROS_DEBUG("Increment_Scaling decreased to: %f",increment_step_scaling);
345  break;
346 
347  case KEYCODE_C:
349  break;
350 
351  case KEYCODE_O:
353  break;
354 
355  } // end switch case
356 
357  if (dirty)
358  {
359  ROS_INFO("Sending new JointMovementActionGoal..");
360 
361  katana_msgs::JointMovementGoal goal;
362  goal.jointGoal = movement_goal_;
363 
364  for (size_t i = 0; i < goal.jointGoal.name.size(); i++)
365  {
366  ROS_DEBUG("Joint: %s to %f rad", goal.jointGoal.name[i].c_str(), goal.jointGoal.position[i]);
367  }
368 
369  action_client.sendGoal(goal);
370  bool finished_within_time = action_client.waitForResult(ros::Duration(10.0));
371  if (!finished_within_time)
372  {
374  ROS_INFO("Timed out achieving goal!");
375  }
376  else
377  {
380  ROS_INFO("Action finished: %s",state.toString().c_str());
381  else
382  ROS_INFO("Action failed: %s", state.toString().c_str());
383 
384  }
385 
386  movement_goal_.name.clear();
387  movement_goal_.position.clear();
388 
389  } // end if dirty
390  }
391 }
392 
394 {
395  GCG goal;
396 
397  switch (goal_type)
398  {
399  case GRASP:
400  goal.command.position = -0.44;
401  // leave velocity and effort empty
402  break;
403 
404  case RELEASE:
405  goal.command.position = 0.3;
406  // leave velocity and effort empty
407  break;
408 
409  default:
410  ROS_ERROR("unknown goal code (%d)", goal_type);
411  return false;
412 
413  }
414 
415 
416  bool finished_within_time = false;
417  gripper_.sendGoal(goal);
418  finished_within_time = gripper_.waitForResult(ros::Duration(10.0));
419  if (!finished_within_time)
420  {
422  ROS_WARN("Timed out achieving goal!");
423  return false;
424  }
425  else
426  {
428  bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
429  if (success)
430  ROS_INFO("Action finished: %s",state.toString().c_str());
431  else
432  ROS_WARN("Action failed: %s",state.toString().c_str());
433 
434  return success;
435  }
436 
437 }
438 }// end namespace "katana"
439 
440 void quit(int sig)
441 {
442  tcsetattr(kfd, TCSANOW, &cooked);
443  exit(0);
444 }
445 
446 int main(int argc, char** argv)
447 {
448  ros::init(argc, argv, "katana_teleop_key");
449 
451 
452  signal(SIGINT, quit);
453 
454  ktk.keyboardLoop();
455 
456  return 0;
457 }
458 
#define KEYCODE_3
#define GRASP
#define KEYCODE_S
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
#define KEYCODE_2
#define KEYCODE_NUMBER
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
int size() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
#define KEYCODE_I
#define KEYCODE_0
int main(int argc, char **argv)
Type const & getType() const
#define ROS_WARN(...)
#define KEYCODE_Q
#define KEYCODE_9
bool send_gripper_action(int32_t goal_type)
std::string toString() const
std::vector< std::string > gripper_joint_names_
#define KEYCODE_D
void jointStateCallback(const sensor_msgs::JointState::ConstPtr &js)
#define KEYCODE_6
sensor_msgs::JointState current_pose_
#define KEYCODE_8
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
#define KEYCODE_4
void quit(int sig)
ROSCPP_DECL bool ok()
const std::string & getNamespace() const
#define KEYCODE_R
#define KEYCODE_7
#define KEYCODE_A
#define KEYCODE_POINT
bool sleep()
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
ROSCONSOLE_DECL void shutdown()
control_msgs::GripperCommandGoal GCG
bool getParam(const std::string &key, std::string &s) const
sensor_msgs::JointState initial_pose_
#define KEYCODE_C
std::vector< std::string > combined_joints_
#define KEYCODE_O
#define KEYCODE_PLUS
sensor_msgs::JointState movement_goal_
SimpleClientGoalState getState() const
std::vector< std::string > joint_names_
#define RELEASE
actionlib::SimpleActionClient< control_msgs::GripperCommandAction > gripper_
struct termios cooked raw
#define KEYCODE_5
#define KEYCODE_1
#define KEYCODE_W
ROSCPP_DECL void spinOnce()
#define ROS_ERROR(...)
#define KEYCODE_COMMA
bool matchJointGoalRequest(double increment)
int kfd
bool got_joint_states_
#define ROS_DEBUG(...)


katana_teleop
Author(s): Henning Deeken
autogenerated on Fri Jan 3 2020 04:01:13