katana_teleop_key.cpp
Go to the documentation of this file.
00001 /*
00002  * UOS-ROS packages - Robot Operating System code by the University of Osnabrück
00003  * Copyright (C) 2011  University of Osnabrück
00004  *
00005  * This program is free software; you can redistribute it and/or
00006  * modify it under the terms of the GNU General Public License
00007  * as published by the Free Software Foundation; either version 2
00008  * of the License, or (at your option) any later version.
00009  *
00010  * This program is distributed in the hope that it will be useful,
00011  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00012  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00013  * GNU General Public License for more details.
00014  *
00015  * You should have received a copy of the GNU General Public License
00016  * along with this program; if not, write to the Free Software
00017  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00018  *
00019  * katana_teleop_key.cpp
00020  *
00021  *  Created on: 21.04.2011
00022  *  Author: Henning Deeken <hdeeken@uos.de>
00023  *
00024  * based on a pr2 teleop by Kevin Watts
00025  */
00026 
00027 #include <katana_teleop/katana_teleop_key.h>
00028 
00029 namespace katana
00030 {
00031 
00032 KatanaTeleopKey::KatanaTeleopKey() :
00033   action_client("katana_arm_controller/joint_movement_action", true), gripper_("gripper_grasp_posture_controller", true)
00034 {
00035   ROS_INFO("KatanaTeleopKey starting...");
00036   ros::NodeHandle n_;
00037   ros::NodeHandle n_private("~");
00038 
00039   n_.param("increment", increment, 0.017453293); // default increment = 1°
00040   n_.param("increment_step", increment_step, 0.017453293); // default step_increment = 1°
00041   n_.param("increment_step_scaling", increment_step_scaling, 1.0); // default scaling = 1
00042 
00043   js_sub_ = n_.subscribe("joint_states", 1000, &KatanaTeleopKey::jointStateCallback, this);
00044 
00045   got_joint_states_ = false;
00046 
00047   jointIndex = 0;
00048 
00049   action_client.waitForServer();
00050   gripper_.waitForServer();
00051 
00052   // Gets all of the joints
00053   XmlRpc::XmlRpcValue joint_names;
00054 
00055   // Gets all of the joints
00056   if (!n_.getParam("katana_joints", joint_names))
00057   {
00058     ROS_ERROR("No joints given. (namespace: %s)", n_.getNamespace().c_str());
00059   }
00060   joint_names_.resize(joint_names.size());
00061 
00062   if (joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00063   {
00064     ROS_ERROR("Malformed joint specification.  (namespace: %s)", n_.getNamespace().c_str());
00065   }
00066 
00067   for (size_t i = 0; (int)i < joint_names.size(); ++i)
00068   {
00069     XmlRpc::XmlRpcValue &name_value = joint_names[i];
00070 
00071     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00072     {
00073       ROS_ERROR("Array of joint names should contain all strings.  (namespace: %s)",
00074           n_.getNamespace().c_str());
00075     }
00076 
00077     joint_names_[i] = (std::string)name_value;
00078   }
00079 
00080   // Gets all of the gripper joints
00081   XmlRpc::XmlRpcValue gripper_joint_names;
00082 
00083   // Gets all of the joints
00084   if (!n_.getParam("katana_gripper_joints", gripper_joint_names))
00085   {
00086     ROS_ERROR("No gripper joints given. (namespace: %s)", n_.getNamespace().c_str());
00087   }
00088 
00089   gripper_joint_names_.resize(gripper_joint_names.size());
00090 
00091   if (gripper_joint_names.getType() != XmlRpc::XmlRpcValue::TypeArray)
00092   {
00093     ROS_ERROR("Malformed gripper joint specification.  (namespace: %s)", n_.getNamespace().c_str());
00094   }
00095   for (size_t i = 0; (int)i < gripper_joint_names.size(); ++i)
00096   {
00097     XmlRpc::XmlRpcValue &name_value = gripper_joint_names[i];
00098     if (name_value.getType() != XmlRpc::XmlRpcValue::TypeString)
00099     {
00100       ROS_ERROR("Array of gripper joint names should contain all strings.  (namespace: %s)",
00101           n_.getNamespace().c_str());
00102     }
00103 
00104     gripper_joint_names_[i] = (std::string)name_value;
00105   }
00106 
00107   combined_joints_.resize(joint_names_.size() + gripper_joint_names_.size());
00108 
00109   for (unsigned int i = 0; i < joint_names_.size(); i++)
00110   {
00111     combined_joints_[i] = joint_names_[i];
00112   }
00113 
00114   for (unsigned int i = 0; i < gripper_joint_names_.size(); i++)
00115   {
00116     combined_joints_[joint_names_.size() + i] = gripper_joint_names_[i];
00117   }
00118 
00119   giveInfo();
00120 
00121 }
00122 
00123 void KatanaTeleopKey::giveInfo()
00124 {
00125   ROS_INFO("---------------------------");
00126   ROS_INFO("Use 'WS' to increase/decrease the joint position about one increment");
00127   ROS_INFO("Current increment is set to: %f", increment);
00128   ROS_INFO("Use '+#' to alter the increment by a increment/decrement of: %f", increment_step);
00129   ROS_INFO("Use ',.' to alter the increment_step_size altering the scaling factor by -/+ 1.0");
00130   ROS_INFO("Current scaling is set to: %f" , increment_step_scaling);
00131   ROS_INFO("---------------------------");
00132   ROS_INFO("Use 'R' to return to the arm's initial pose");
00133   ROS_INFO("Use 'I' to display this manual and the current joint state");
00134   ROS_INFO("---------------------------");
00135   ROS_INFO("Use 'AD' to switch to the next/previous joint");
00136   ROS_INFO("Use '0-9' to select a joint by number");
00137   ROS_INFO("---------------------------");
00138   ROS_INFO("Use 'OC' to open/close gripper");
00139 
00140   for (unsigned int i = 0; i < joint_names_.size(); i++)
00141   {
00142     ROS_INFO("Use '%d' to switch to Joint: '%s'",i, joint_names_[i].c_str());
00143   }
00144 
00145   for (unsigned int i = 0; i < gripper_joint_names_.size(); i++)
00146   {
00147     ROS_INFO("Use '%zu' to switch to Gripper Joint: '%s'",i + joint_names_.size(), gripper_joint_names_[i].c_str());
00148   }
00149 
00150   if (!current_pose_.name.empty())
00151   {
00152     ROS_INFO("---------------------------");
00153     ROS_INFO("Current Joint Positions:");
00154 
00155     for (unsigned int i = 0; i < current_pose_.position.size(); i++)
00156     {
00157       ROS_INFO("Joint %d - %s: %f", i, current_pose_.name[i].c_str(), current_pose_.position[i]);
00158     }
00159   }
00160 }
00161 
00162 void KatanaTeleopKey::jointStateCallback(const sensor_msgs::JointState::ConstPtr& js)
00163 {
00164   // ROS_INFO("KatanaTeleopKeyboard received a new JointState");
00165 
00166   current_pose_.name = js->name;
00167   current_pose_.position = js->position;
00168 
00169   if (!got_joint_states_)
00170   {
00171     // ROS_INFO("KatanaTeleopKeyboard received initial JointState");
00172     initial_pose_.name = js->name;
00173     initial_pose_.position = js->position;
00174     got_joint_states_ = true;
00175   }
00176 }
00177 
00178 bool KatanaTeleopKey::matchJointGoalRequest(double increment)
00179 {
00180   bool found_match = false;
00181 
00182   for (unsigned int i = 0; i < current_pose_.name.size(); i++)
00183   {
00184     if (current_pose_.name[i] == combined_joints_[jointIndex])
00185     {
00186       //ROS_DEBUG("incoming inc: %f - curren_pose: %f - resulting pose: %f ",increment, current_pose_.position[i], current_pose_.position[i] + increment);
00187       movement_goal_.position.push_back(current_pose_.position[i] + increment);
00188       found_match = true;
00189       break;
00190 
00191     }
00192   }
00193 
00194   return found_match;
00195 }
00196 
00197 void KatanaTeleopKey::keyboardLoop()
00198 {
00199 
00200   char c;
00201   bool dirty = true;
00202   bool shutdown = false;
00203 
00204   // get the console in raw mode
00205   tcgetattr(kfd, &cooked);
00206   memcpy(&raw, &cooked, sizeof(struct termios));
00207   raw.c_lflag &= ~(ICANON | ECHO);
00208   // Setting a new line, then end of file
00209   raw.c_cc[VEOL] = 1;
00210   raw.c_cc[VEOF] = 2;
00211   tcsetattr(kfd, TCSANOW, &raw);
00212 
00213   ros::Rate r(50.0); // 50 Hz
00214 
00215   while (ros::ok() && !shutdown)
00216   {
00217     r.sleep();
00218     ros::spinOnce();
00219 
00220     if (!got_joint_states_)
00221       continue;
00222 
00223     dirty = false;
00224 
00225     // get the next event from the keyboard
00226     if (read(kfd, &c, 1) < 0)
00227     {
00228       perror("read():");
00229       exit(-1);
00230     }
00231 
00232     size_t selected_joint_index;
00233     switch (c)
00234     {
00235       // Increasing/Decreasing JointPosition
00236       case KEYCODE_W:
00237         if (matchJointGoalRequest(increment))
00238         {
00239           movement_goal_.name.push_back(combined_joints_[jointIndex]);
00240           dirty = true;
00241         }
00242         else
00243         {
00244           ROS_WARN("movement with the desired joint: %s failed due to a mismatch with the current joint state", combined_joints_[jointIndex].c_str());
00245         }
00246 
00247         break;
00248 
00249       case KEYCODE_S:
00250         if (matchJointGoalRequest(-increment))
00251         {
00252           movement_goal_.name.push_back(combined_joints_[jointIndex]);
00253           dirty = true;
00254         }
00255         else
00256         {
00257           ROS_WARN("movement with the desired joint: %s failed due to a mismatch with the current joint state", combined_joints_[jointIndex].c_str());
00258         }
00259 
00260         break;
00261 
00262         // Switching active Joint
00263       case KEYCODE_D:
00264         // use this line if you want to use "the gripper" instead of the single gripper joints
00265         jointIndex = (jointIndex + 1) % (joint_names_.size() + 1);
00266 
00267         // use this line if you want to select specific gripper joints
00268         //jointIndex = (jointIndex + 1) % combined_joints_.size();
00269         break;
00270 
00271       case KEYCODE_A:
00272         // use this line if you want to use "the gripper" instead of the single gripper joints
00273         jointIndex = (jointIndex - 1) % (joint_names_.size() + 1);
00274 
00275         // use this line if you want to select specific gripper joints
00276         //jointIndex = (jointIndex - 1) % combined_joints_.size();
00277 
00278         break;
00279 
00280       case KEYCODE_R:
00281         ROS_INFO("Resetting arm to its initial pose..");
00282 
00283         movement_goal_.name = initial_pose_.name;
00284         movement_goal_.position = initial_pose_.position;
00285         dirty = true;
00286         break;
00287 
00288       case KEYCODE_Q:
00289         // in case of shutting down the teleop node the arm is moved back into it's initial pose
00290         // assuming that this is a proper resting pose for the arm
00291 
00292         ROS_INFO("Shutting down the Katana Teleoperation node...");
00293         shutdown = true;
00294         break;
00295 
00296       case KEYCODE_I:
00297         giveInfo();
00298         break;
00299 
00300       case KEYCODE_0:
00301       case KEYCODE_1:
00302       case KEYCODE_2:
00303       case KEYCODE_3:
00304       case KEYCODE_4:
00305       case KEYCODE_5:
00306       case KEYCODE_6:
00307       case KEYCODE_7:
00308       case KEYCODE_8:
00309       case KEYCODE_9:
00310         selected_joint_index = c - KEYCODE_0;
00311 
00312         if (combined_joints_.size() > jointIndex)
00313         {
00314           ROS_DEBUG("You choose to adress joint no. %zu: %s", selected_joint_index, combined_joints_[9].c_str());
00315           jointIndex = selected_joint_index;
00316         }
00317         else
00318         {
00319           ROS_WARN("Joint Index No. %zu can not be adressed!", jointIndex);
00320         }
00321         break;
00322 
00323       case KEYCODE_PLUS:
00324         increment += (increment_step * increment_step_scaling);
00325         ROS_DEBUG("Increment increased to: %f",increment);
00326         break;
00327 
00328       case KEYCODE_NUMBER:
00329         increment -= (increment_step * increment_step_scaling);
00330         if (increment < 0)
00331         {
00332           increment = 0.0;
00333         }
00334         ROS_DEBUG("Increment decreased to: %f",increment);
00335         break;
00336 
00337       case KEYCODE_POINT:
00338         increment_step_scaling += 1.0;
00339         ROS_DEBUG("Increment_Scaling increased to: %f",increment_step_scaling);
00340         break;
00341 
00342       case KEYCODE_COMMA:
00343         increment_step_scaling -= 1.0;
00344         ROS_DEBUG("Increment_Scaling decreased to: %f",increment_step_scaling);
00345         break;
00346 
00347       case KEYCODE_C:
00348         send_gripper_action(GRASP);
00349         break;
00350 
00351       case KEYCODE_O:
00352         send_gripper_action(RELEASE);
00353         break;
00354 
00355     } // end switch case
00356 
00357     if (dirty)
00358     {
00359       ROS_INFO("Sending new JointMovementActionGoal..");
00360 
00361       katana_msgs::JointMovementGoal goal;
00362       goal.jointGoal = movement_goal_;
00363 
00364       for (size_t i = 0; i < goal.jointGoal.name.size(); i++)
00365       {
00366         ROS_DEBUG("Joint: %s to %f rad", goal.jointGoal.name[i].c_str(), goal.jointGoal.position[i]);
00367       }
00368 
00369       action_client.sendGoal(goal);
00370       bool finished_within_time = action_client.waitForResult(ros::Duration(10.0));
00371       if (!finished_within_time)
00372       {
00373         action_client.cancelGoal();
00374         ROS_INFO("Timed out achieving goal!");
00375       }
00376       else
00377       {
00378         actionlib::SimpleClientGoalState state = action_client.getState();
00379         if (state == actionlib::SimpleClientGoalState::SUCCEEDED)
00380           ROS_INFO("Action finished: %s",state.toString().c_str());
00381         else
00382           ROS_INFO("Action failed: %s", state.toString().c_str());
00383 
00384       }
00385 
00386       movement_goal_.name.clear();
00387       movement_goal_.position.clear();
00388 
00389     } // end if dirty
00390   }
00391 }
00392 
00393 bool KatanaTeleopKey::send_gripper_action(int goal_type)
00394 {
00395   GCG goal;
00396 
00397   switch (goal_type)
00398   {
00399     case GRASP:
00400       goal.command.position = -0.44; 
00401       // leave velocity and effort empty
00402       break;
00403 
00404     case RELEASE:
00405       goal.command.position = 0.3; 
00406       // leave velocity and effort empty
00407       break;
00408 
00409     default:
00410       ROS_ERROR("unknown goal code (%d)", goal_type);
00411       return false;
00412 
00413   }
00414 
00415 
00416   bool finished_within_time = false;
00417   gripper_.sendGoal(goal);
00418   finished_within_time = gripper_.waitForResult(ros::Duration(10.0));
00419   if (!finished_within_time)
00420   {
00421     gripper_.cancelGoal();
00422     ROS_WARN("Timed out achieving goal!");
00423     return false;
00424   }
00425   else
00426   {
00427     actionlib::SimpleClientGoalState state = gripper_.getState();
00428     bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00429     if (success)
00430       ROS_INFO("Action finished: %s",state.toString().c_str());
00431     else
00432       ROS_WARN("Action failed: %s",state.toString().c_str());
00433 
00434     return success;
00435   }
00436 
00437 }
00438 }// end namespace "katana"
00439 
00440 void quit(int sig)
00441 {
00442   tcsetattr(kfd, TCSANOW, &cooked);
00443   exit(0);
00444 }
00445 
00446 int main(int argc, char** argv)
00447 {
00448   ros::init(argc, argv, "katana_teleop_key");
00449 
00450   katana::KatanaTeleopKey ktk;
00451 
00452   signal(SIGINT, quit);
00453 
00454   ktk.keyboardLoop();
00455 
00456   return 0;
00457 }
00458 


katana_teleop
Author(s): Henning Deeken
autogenerated on Mon Oct 6 2014 10:47:32