katana_teleop: katana_teleop: katana_teleop

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_CyborgEvo.cpp
00020  *
00021  *  Created on: 03.06.2011
00022  *  Author: Henning Deeken <hdeeken@uos.de>
00023  *
00024  **/
00025 
00026 #include <katana_teleop/katana_teleop_cyborgevo.h>
00027 #include <math.h>
00028 
00029 namespace katana
00030 {
00031 
00032 KatanaTeleopCyborgEvo::KatanaTeleopCyborgEvo() :
00033   action_client("katana_arm_controller/joint_movement_action", true)
00034 {
00035 
00036   ROS_INFO("KatanaTeleopCyborgEvo starting...");
00037 
00038   ros::NodeHandle n_;
00039   ros::NodeHandle n_private("~");
00040 
00041   // register service and action clients
00042   action_client.waitForServer();
00043 
00044   n_private.param<std::string> ("ik_service", ik_service, "katana_kinematics_constraint_aware/get_openrave_ik");
00045   //n_private.param<std::string> ("ik_service", ik_service, "non_constraint_aware_ik_adapter/get_constraint_aware_ik");
00046   n_private.param<std::string> ("fk_service", fk_service, "get_fk");
00047   n_private.param<std::string> ("ik_solver_info", ik_solver_info, "get_kinematic_solver_info");
00048 
00049   ros::service::waitForService(ik_service);
00050   ros::service::waitForService(fk_service);
00051   ros::service::waitForService(ik_solver_info);
00052 
00053   ROS_INFO("KatanaTeleopCyborgEvo connected to all the services...");
00054 
00055   ik_client = n_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK> (ik_service);
00056   fk_client = n_.serviceClient<kinematics_msgs::GetPositionFK> (fk_service);
00057   // info_client = n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo> (ik_solver_info);
00058 
00059   js_sub_ = n_.subscribe("joint_states", 1000, &KatanaTeleopCyborgEvo::jointStateCallback, this);
00060 
00061   cyborgevo_sub = n_.subscribe("joy", 100, &KatanaTeleopCyborgEvo::cyborgevoCallback, this);
00062 
00063   ROS_INFO("KatanaTeleopCyborgEvo subscribed to all the topics..");
00064 
00065   active = true;
00066   initial = true;
00067 
00068   ROS_INFO("KatanaTeleopCyborgEvo initialized...");
00069 }
00070 
00071 int count = 0;
00072 
00073 void KatanaTeleopCyborgEvo::jointStateCallback(const sensor_msgs::JointState::ConstPtr& js)
00074 {
00075 
00076   count++;
00077 
00078   ROS_DEBUG("KatanaTeleopCyborgEvo recieved a new JointState...");
00079   sensor_msgs::JointState incoming_joint_state_;
00080   incoming_joint_state_.header = js->header;
00081   incoming_joint_state_.name = js->name;
00082   incoming_joint_state_.position = js->position;
00083 
00084   if (initial)
00085     initialState = incoming_joint_state_;
00086 
00087 
00088   kinematics_msgs::GetPositionFK::Request fk_request;
00089   kinematics_msgs::GetPositionFK::Response fk_response;
00090 
00091   fk_request.header.frame_id = "katana_base_link";
00092   fk_request.fk_link_names.resize(1);
00093   fk_request.fk_link_names[0] = "katana_gripper_tool_frame";
00094   fk_request.robot_state.joint_state = incoming_joint_state_;
00095 
00096   if (fk_client.call(fk_request, fk_response))
00097   {
00098 
00099     if (fk_response.error_code.val == fk_response.error_code.SUCCESS)
00100     {
00101 
00102         if(initial){
00103         //      initial_RPY_Orientation = fk_response.pose_stamped[0].orientation;
00104         }
00105       // update the internal variables
00106       currentState = incoming_joint_state_;
00107       currentPose = fk_response.pose_stamped[0];
00108       if (count % 100 == 0)
00109       {
00110         ROS_ERROR("Actual Pose...");
00111         for (unsigned int i = 0; i < fk_response.pose_stamped.size(); i++)
00112         {
00113 
00114           ROS_ERROR("       Link: %s", fk_response.fk_link_names[i].c_str());
00115           ROS_ERROR("   Position: %f %f %f",
00116               fk_response.pose_stamped[i].pose.position.x,
00117               fk_response.pose_stamped[i].pose.position.y,
00118               fk_response.pose_stamped[i].pose.position.z);
00119           ROS_ERROR("Orientation: %f %f %f %f",
00120               fk_response.pose_stamped[i].pose.orientation.x,
00121               fk_response.pose_stamped[i].pose.orientation.y,
00122               fk_response.pose_stamped[i].pose.orientation.z,
00123               fk_response.pose_stamped[i].pose.orientation.w);
00124         }
00125       }
00126     }
00127     else
00128     {
00129       ROS_ERROR("Forward kinematics failed");
00130     }
00131   }
00132   else
00133   {
00134     ROS_ERROR("Forward kinematics service call failed");
00135   }
00136 }
00137 
00138 void KatanaTeleopCyborgEvo::cyborgevoCallback(const joy::Joy::ConstPtr& joy)
00139 {
00140 
00141   ROS_DEBUG("KatanaTeleopCyborgEvo recieved a new Joy command...");
00142 
00143   bool execute_action = false;
00144   bool request_ik = false;
00145 
00146   if (joy->buttons[10] == 1 && joy->buttons[11] == 1)
00147   {
00148     goal.jointGoal = initialState;
00149     active = false;
00150     ROS_WARN("Return to Initial State");
00151     execute_action = true;
00152   }
00153 
00154   if (joy->buttons[6] == 1 && joy->buttons[8] == 1)
00155   {
00156     savedState = currentState;
00157 
00158   }
00159 
00160   if (joy->buttons[7] == 1 && joy->buttons[9] == 1)
00161   {
00162 
00163     goal.jointGoal = savedState;
00164     ROS_WARN("Return to Saved State");
00165     execute_action = true;
00166 
00167   }
00168 
00169   else
00170 
00171   {
00172     if (joy->axes[5] == 1.0)
00173     {
00174       addGripperGoalPosition("katana_l_finger_joint", 5 * 0.017453293);
00175       ROS_WARN("Close Gripper");
00176       execute_action = true;
00177     }
00178     else if (joy->axes[5] == -1.0)
00179     {
00180       addGripperGoalPosition("katana_l_finger_joint", 5 * -0.017453293);
00181       ROS_WARN("Open Gripper");
00182       execute_action = true;
00183     }
00184 
00185     goalPose = currentPose;
00186     if (abs(joy->axes[0]) > 0.1 ||
00187         abs(joy->axes[1]) > 0.1 ||
00188         abs(joy->axes[3]) > 0.1
00189     //  abs(joy->axes[R]) > 0.1 ||
00190     //  abs(joy->axes[P]) > 0.1 ||
00191     //  abs(joy->axes[Y]) > 0.1
00192         )
00193     {
00194       goalPose.pose.position.y += 0.001 * joy->axes[0];
00195       goalPose.pose.position.x += 0.001 * joy->axes[1];
00196       goalPose.pose.position.z += 0.001 * joy->axes[3];
00197 
00198 
00199 /*
00200  *    add to RPY convert to quad -> orientation = quad
00201       goalPose.pose.position.y += 0.001 * joy->axes[0];
00202       goalPose.pose.position.x += 0.001 * joy->axes[1];
00203       goalPose.pose.position.z += 0.001 * joy->axes[3];
00204 */
00205 
00206       ROS_WARN("Want to modify Pose");
00207       request_ik = true;
00208     }
00209   }
00210 /*
00211   ROS_ERROR("Desired Pose...");
00212   ROS_ERROR("       Link: %s", "katana_gripper_tool_frame");
00213   ROS_ERROR("   Position: %f %f %f",
00214       goalPose.pose.position.x,
00215       goalPose.pose.position.y,
00216       goalPose.pose.position.z);
00217   ROS_ERROR("Orientation: %f %f %f %f",
00218       goalPose.pose.orientation.x,
00219       goalPose.pose.orientation.y,
00220       goalPose.pose.orientation.z,
00221       goalPose.pose.orientation.w);
00222 */
00223   if (request_ik)
00224   {
00225 
00226     // define the service messages
00227     kinematics_msgs::GetConstraintAwarePositionIK::Request gcapik_req;
00228     kinematics_msgs::GetConstraintAwarePositionIK::Response gcapik_res;
00229 
00230     gcapik_req.ik_request.ik_link_name = "katana_gripper_tool_frame";
00231     gcapik_req.ik_request.pose_stamped = goalPose;
00232     gcapik_req.ik_request.robot_state.joint_state = currentState;
00233     gcapik_req.ik_request.ik_seed_state.joint_state = currentState;
00234     gcapik_req.timeout = ros::Duration(5.0);
00235 
00236     ROS_INFO("KatanaTeleopCyborgEvo calls the IK Client...");
00237 
00238     if (ik_client.call(gcapik_req, gcapik_res))
00239     {
00240       if (gcapik_res.error_code.val == gcapik_res.error_code.SUCCESS)
00241       {
00242         ROS_WARN("IK succeeded...");
00243         for (size_t i = 0; i < gcapik_res.solution.joint_state.name.size(); i++)
00244         {
00245           goal.jointGoal.name.push_back(gcapik_res.solution.joint_state.name[i]);
00246           goal.jointGoal.position.push_back(gcapik_res.solution.joint_state.position[i]);
00247 
00248           ROS_WARN("Joint: %s - %f", gcapik_res.solution.joint_state.name[i].c_str(), gcapik_res.solution.joint_state.position[i]);
00249         }
00250         ROS_WARN("Modify Pose 2");
00251         execute_action = true;
00252 
00253       }
00254       else
00255       {
00256         ROS_ERROR("IK failed, error code: %d", gcapik_res.error_code.val);
00257       }
00258     }
00259     else
00260     {
00261       ROS_ERROR("IK service call failed");
00262     }
00263   }
00264 
00265   if (execute_action)
00266   {
00267     ROS_WARN("Send Goal...");
00268     bool finished_within_time = false;
00269 
00270     for (size_t i = 0; i < goal.jointGoal.name.size(); i++)
00271     {
00272       ROS_WARN("Joint: %s - %f", goal.jointGoal.name[i].c_str(), goal.jointGoal.position[i]);
00273     }
00274 
00275     action_client.sendGoal(goal);
00276 
00277     goal.jointGoal.name.clear();
00278     goal.jointGoal.position.clear();
00279 
00280     finished_within_time = action_client.waitForResult(ros::Duration(1.0));
00281 
00282     if (!finished_within_time)
00283     {
00284       action_client.cancelGoal();
00285       ROS_INFO("Timed out achieving goal!");
00286     }
00287     else
00288     {
00289       actionlib::SimpleClientGoalState state = action_client.getState();
00290       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00291 
00292       if (success)
00293         ROS_INFO("Action finished: %s", state.toString().c_str());
00294       else
00295         ROS_INFO("Action failed: %s", state.toString().c_str());
00296 
00297       //ros::spinOnce();
00298     }
00299   }
00300 
00301 }
00302 
00303 void KatanaTeleopCyborgEvo::addGripperGoalPosition(std::string name, float increment)
00304 {
00305 
00306   float gripper_pos;
00307   if (getCurrentJointPosition(currentState, name, gripper_pos))
00308   {
00309     // if ((gripper_pos + increment) >= -0.44 && (gripper_pos + increment) <= 0.30)
00310     // {
00311     goal.jointGoal.name.push_back(name);
00312     goal.jointGoal.position.push_back(gripper_pos + increment);
00313     // }
00314     // else
00315     //   ROS_WARN("gripper position would exceed limits: %f", gripper_pos + increment);
00316   }
00317   else
00318     ROS_WARN("could not access gripper joint");
00319 
00320 }
00321 
00322 bool KatanaTeleopCyborgEvo::getCurrentJointPosition(sensor_msgs::JointState &joint_state, std::string &name,
00323                                                     float &position)
00324 {
00325 
00326   for (size_t i = 0; i < joint_state.name.size(); i++)
00327   {
00328 
00329     if (joint_state.name[i] == name)
00330     {
00331       ROS_ERROR("joint: %s - %f", joint_state.name[i].c_str(), joint_state.position[i]);
00332       position = joint_state.position[i];
00333       return true;
00334     }
00335   }
00336   return false;
00337 }
00338 
00339 void KatanaTeleopCyborgEvo::loop()
00340 {
00341 
00342   ROS_INFO("KatanaTeleopCyborgEvo loops...");
00343   while (active)
00344   {
00345     ros::spin();
00346   }
00347 
00348 }
00349 }
00350 
00351 int main(int argc, char** argv)
00352 {
00353   ros::init(argc, argv, "katana_teleop_cyborgevo");
00354 
00355   katana::KatanaTeleopCyborgEvo ktc;
00356 
00357   ktc.loop();
00358 
00359   return 1;
00360 
00361 }
katana_teleop_cyborgevo.cpp
