katana_teleop_ps3joy.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_ps3.cpp
00020  *
00021  *  Created on: 03.06.2011
00022  *  Author: Henning Deeken <hdeeken@uos.de>
00023  *
00024  **/
00025 
00026 #include <katana_teleop/katana_teleop_ps3.h>
00027 #include <math.h>
00028 
00029 namespace katana
00030 {
00031 
00032 KatanaTeleopPS3::KatanaTeleopPS3() :
00033   action_client("katana_arm_controller/joint_movement_action", true)
00034 {
00035 
00036   ROS_INFO("KatanaTeleopPS3 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, "get_constraint_aware_ik");
00045   n_private.param<std::string> ("fk_service", fk_service, "get_fk");
00046   n_private.param<std::string> ("ik_solver_info", ik_solver_info, "get_ik_solver_info");
00047 
00048   ros::service::waitForService(ik_service);
00049   ros::service::waitForService(fk_service);
00050   ros::service::waitForService(ik_solver_info);
00051 
00052   ik_client = n_.serviceClient<kinematics_msgs::GetConstraintAwarePositionIK> (ik_service);
00053   fk_client = n_.serviceClient<kinematics_msgs::GetPositionFK> (fk_service);
00054   info_client = n_.serviceClient<kinematics_msgs::GetKinematicSolverInfo> (ik_solver_info);
00055 
00056   js_sub_ = n_.subscribe("joint_states", 1000, &KatanaTeleopPS3::jointStateCallback, this);
00057 
00058   ps3joy_sub = n_.subscribe("joy", 100, &KatanaTeleopPS3::ps3joyCallback, this);
00059 
00060   active = true;
00061 
00062 }
00063 
00064 void KatanaTeleopPS3::jointStateCallback(const sensor_msgs::JointState::ConstPtr& js)
00065 {
00066 
00067   sensor_msgs::JointState incoming_joint_state_;
00068   incoming_joint_state_.header = js->header;
00069   incoming_joint_state_.name = js->name;
00070   incoming_joint_state_.position = js->position;
00071 
00072   kinematics_msgs::GetPositionFK::Request fk_request;
00073   kinematics_msgs::GetPositionFK::Response fk_response;
00074 
00075   fk_request.header.frame_id = "katana_base_link";
00076   fk_request.fk_link_names.resize(1);
00077   fk_request.fk_link_names[0] = "katana_gripper_tool_frame";
00078   fk_request.robot_state.joint_state = incoming_joint_state_;
00079 
00080   if (fk_client.call(fk_request, fk_response))
00081   {
00082 
00083     if (fk_response.error_code.val == fk_response.error_code.SUCCESS)
00084     {
00085 
00086       // update the internal variables
00087       currentState = incoming_joint_state_;
00088 
00089       //TODO: read pose of katana_gripper_tool_frame
00090       //currentPose = fk_response.pose_stamped;
00091 
00092       for (unsigned int i = 0; i < fk_response.pose_stamped.size(); i++)
00093       {
00094 
00095         ROS_DEBUG("       Link: %s", fk_response.fk_link_names[i].c_str());
00096         ROS_DEBUG("   Position: %f %f %f",
00097             fk_response.pose_stamped[i].pose.position.x,
00098             fk_response.pose_stamped[i].pose.position.y,
00099             fk_response.pose_stamped[i].pose.position.z);
00100         ROS_DEBUG("Orientation: %f %f %f %f",
00101             fk_response.pose_stamped[i].pose.orientation.x,
00102             fk_response.pose_stamped[i].pose.orientation.y,
00103             fk_response.pose_stamped[i].pose.orientation.z,
00104             fk_response.pose_stamped[i].pose.orientation.w);
00105       }
00106 
00107     }
00108     else
00109     {
00110       ROS_ERROR("Forward kinematics failed");
00111     }
00112   }
00113   else
00114   {
00115     ROS_ERROR("Forward kinematics service call failed");
00116   }
00117 }
00118 
00119 void KatanaTeleopPS3::ps3joyCallback(const sensor_msgs::Joy::ConstPtr& joy)
00120 {
00121   bool execute_action = false;
00122 
00123 
00124   if (joy->buttons[16] == 1)
00125   {
00126     active = false;
00127   }
00128 
00129   if (joy->buttons[16] == 0)
00130   {
00131     savedState = currentState;
00132 
00133   }
00134 
00135   if (joy->buttons[3] == 0)
00136   {
00137 
00138     goal.jointGoal = savedState;
00139     execute_action = true;
00140 
00141   }
00142 
00143   else
00144 
00145   {
00146      if (joy->buttons[8] == 1)
00147     {
00148       addGripperGoalPosition("katana_l_finger_joint", -((joy->axes[8] - 1.0) / -2.0));
00149       execute_action = true;
00150     }
00151     else if (joy->buttons[9] == 1)
00152     {
00153       addGripperGoalPosition("katana_l_finger_joint", -((joy->axes[8] - 1.0) / -2.0));
00154       execute_action = true;
00155     }
00156 
00157     goalPose = currentPose;
00158     if (abs(joy->axes[0]) > 0 || abs(joy->axes[1]) > 0 || abs(joy->axes[3]) > 0)
00159     {
00160       goalPose.pose.position.y += joy->axes[0];
00161 
00162       goalPose.pose.position.x += joy->axes[1];
00163 
00164       goalPose.pose.position.z += joy->axes[3];
00165     }
00166 
00167   }
00168 
00169   // define the service messages
00170   kinematics_msgs::GetConstraintAwarePositionIK::Request gcapik_req;
00171   kinematics_msgs::GetConstraintAwarePositionIK::Response gcapik_res;
00172   gcapik_req.timeout = ros::Duration(5.0);
00173 
00174   gcapik_req.ik_request.pose_stamped = goalPose;
00175   gcapik_req.ik_request.robot_state.joint_state = currentState;
00176   gcapik_req.ik_request.ik_seed_state.joint_state = currentState;
00177 
00178   if (ik_client.call(gcapik_req, gcapik_res))
00179   {
00180     if (gcapik_res.error_code.val == gcapik_res.error_code.SUCCESS)
00181     {
00182 
00183       for (size_t i = 0; i < gcapik_res.solution.joint_state.name.size(); i++)
00184       {
00185         goal.jointGoal.name.push_back(gcapik_res.solution.joint_state.name[i]);
00186         goal.jointGoal.position.push_back(gcapik_res.solution.joint_state.position[i]);
00187       }
00188 
00189       execute_action = true;
00190 
00191     }
00192     else
00193     {
00194       ROS_ERROR("IK failed, error code: %d", gcapik_res.error_code.val);
00195     }
00196   }
00197   else
00198   {
00199     ROS_ERROR("IK service call failed");
00200   }
00201 
00202   if (execute_action)
00203   {
00204 
00205     bool finished_within_time = false;
00206 
00207     action_client.sendGoal(goal);
00208     finished_within_time = action_client.waitForResult(ros::Duration(1.0));
00209 
00210     if (!finished_within_time)
00211     {
00212       action_client.cancelGoal();
00213       ROS_INFO("Timed out achieving goal!");
00214     }
00215     else
00216     {
00217       actionlib::SimpleClientGoalState state = action_client.getState();
00218       bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
00219 
00220       if (success)
00221         ROS_INFO("Action finished: %s", state.toString().c_str());
00222       else
00223         ROS_INFO("Action failed: %s", state.toString().c_str());
00224 
00225       ros::spinOnce();
00226     }
00227   }
00228 
00229 }
00230 
00231 void KatanaTeleopPS3::addGripperGoalPosition(std::string name, float increment)
00232 {
00233 
00234 
00235   float gripper_pos;
00236   if (getCurrentJointPosition(currentState, name, gripper_pos))
00237     if ((gripper_pos + increment) >= 0.44 && (gripper_pos + increment) <= 0.30)
00238     {
00239       goal.jointGoal.name.push_back(name);
00240       goal.jointGoal.position.push_back(gripper_pos + increment);
00241     }
00242     else
00243       ROS_WARN("gripper position would exceed limits");
00244   else
00245     ROS_WARN("could not access gripper joint");
00246 
00247 }
00248 
00249 bool KatanaTeleopPS3::getCurrentJointPosition(sensor_msgs::JointState &joint_state, std::string &name, float &position)
00250 {
00251 
00252   for (size_t i = 0; i < joint_state.name.size(); i++)
00253   {
00254 
00255     if (joint_state.name[i] == name)
00256     {
00257       position = joint_state.position[i];
00258       return true;
00259     }
00260   }
00261   return false;
00262 }
00263 
00264 void KatanaTeleopPS3::loop(){
00265 
00266   while (active)
00267   {
00268       ros::spin();
00269   }
00270 
00271 }
00272 }
00273 
00274 int main(int argc, char** argv)
00275 {
00276   ros::init(argc, argv, "katana_teleop_ps3");
00277 
00278   katana::KatanaTeleopPS3 ktps3;
00279 
00280   ktps3.loop();
00281 
00282 
00283   return 1;
00284 
00285 }
00286 
00287 
00288 // end namespace
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


katana_teleop
Author(s): Henning Deeken
autogenerated on Tue May 28 2013 14:50:45