hand_posture_execution.cpp
Go to the documentation of this file.
00001 
00010 #include "sr_move_arm/hand_posture_execution.hpp"
00011 
00012 namespace shadowrobot
00013 {
00014 
00015   SrHandPostureExecutionSimpleAction::SrHandPostureExecutionSimpleAction() :
00016     nh_tilde("~"), hand_occupied(false)
00017   {
00018     is_hand_occupied_client = nh.serviceClient<sr_robot_msgs::is_hand_occupied>("is_hand_occupied");
00019 
00020     action_server = boost::shared_ptr<actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction> >(new actionlib::SimpleActionServer<object_manipulation_msgs::GraspHandPostureExecutionAction>("/right_arm/hand_posture_execution", boost::bind(&SrHandPostureExecutionSimpleAction::execute, this, _1), false));
00021 
00022     get_status_server = nh.advertiseService("/right_arm/grasp_status", &SrHandPostureExecutionSimpleAction::getStatusCallback, this);
00023 
00024     shadowhand_ros_lib = boost::shared_ptr<shadowrobot::HandCommander>( new shadowrobot::HandCommander() );
00025 
00026     action_server->start();
00027   }
00028 
00029   SrHandPostureExecutionSimpleAction::~SrHandPostureExecutionSimpleAction()
00030   {}
00031 
00032   bool SrHandPostureExecutionSimpleAction::getStatusCallback(object_manipulation_msgs::GraspStatus::Request &request,
00033                                                              object_manipulation_msgs::GraspStatus::Response &response)
00034   {
00035     response.is_hand_occupied = false;
00036 
00037     sr_robot_msgs::is_hand_occupied srv;
00038     if( is_hand_occupied_client.call(srv) )
00039     {
00040       response.is_hand_occupied = srv.response.hand_occupied;
00041       if( srv.response.hand_occupied)
00042         ROS_INFO("The hand is occupied");
00043       else
00044         ROS_INFO("The hand is empty");
00045     }
00046     else
00047     {
00048       ROS_ERROR("Called to is_hand_occupied service failed.");
00049       return false;
00050     }
00051     return true;
00052   }
00053 
00054   void SrHandPostureExecutionSimpleAction::execute(const object_manipulation_msgs::GraspHandPostureExecutionGoalConstPtr& goal)
00055   {
00056     if(action_server->isPreemptRequested() || !ros::ok())
00057     {
00058       ROS_INFO("Change Hand Pose action preempted.");
00059       //set action state to preempted
00060       action_server->setPreempted();
00061     }
00062 
00063 
00064     std::vector<std::string> joint_names;
00065     if (goal->goal == object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP)
00066       joint_names = goal->grasp.grasp_posture.name;
00067     else
00068       joint_names = goal->grasp.pre_grasp_posture.name;
00069     
00070     if (joint_names.empty())
00071     {
00072       ROS_ERROR("Shadow Robot grasp execution: names empty in requested grasp");
00073       action_server->setAborted();
00074       return;
00075     }
00076 
00077     joint_vector.clear();
00078     for(unsigned int i = 0; i < joint_names.size(); ++i)
00079     {
00080       sr_robot_msgs::joint joint;
00081       joint.joint_name = joint_names[i];
00082       joint_vector.push_back(joint);
00083     }
00084 
00085     object_manipulation_msgs::GraspHandPostureExecutionResult result;
00086 
00087     switch (goal->goal)
00088     {
00089     case object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP:
00090       ROS_DEBUG("GRASP!");
00091 
00092       if (goal->grasp.grasp_posture.position.empty())
00093       {
00094         ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested grasp");
00095         action_server->setAborted();
00096         return;
00097       }
00098       for(unsigned int i = 0; i < goal->grasp.grasp_posture.position.size(); ++i)
00099       {
00100         joint_vector[i].joint_target = goal->grasp.grasp_posture.position[i]*180.0/M_PI;
00101         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
00102       }
00103 
00104       shadowhand_ros_lib->sendCommands(joint_vector);
00105       ROS_DEBUG("Hand in grasp position");
00106 
00107       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
00108       action_server->setSucceeded( result );
00109       hand_occupied = true;
00110 
00111       break;
00112 
00113     case object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP:
00114       ROS_DEBUG("PREGRASP!");
00115 
00116       if (goal->grasp.pre_grasp_posture.position.empty())
00117       {
00118         ROS_ERROR("Shadow Robot grasp execution: position vector empty in requested pre_grasp");
00119         action_server->setAborted();
00120         return;
00121       }
00122       //move to pregrasp
00123       for(unsigned int i = 0; i < goal->grasp.pre_grasp_posture.position.size(); ++i)
00124       {
00125         joint_vector[i].joint_target = goal->grasp.pre_grasp_posture.position[i]*180.0/M_PI;
00126         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
00127       }
00128 
00129       shadowhand_ros_lib->sendCommands(joint_vector);
00130       ROS_DEBUG("Hand in pregrasp position");
00131 
00132       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
00133       action_server->setSucceeded( result );
00134 
00135       hand_occupied = false;
00136 
00137       break;
00138     case object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE:
00139       ROS_DEBUG("RELEASE!");
00140 
00141       //open the hand completely
00142       for(unsigned int i = 0; i < goal->grasp.pre_grasp_posture.position.size(); ++i)
00143       {
00144         joint_vector[i].joint_target = 0.0;
00145         ROS_DEBUG("[%s]: %f", joint_names[i].c_str(), joint_vector[i].joint_target);
00146       }
00147 
00148       shadowhand_ros_lib->sendCommands(joint_vector);
00149       ROS_DEBUG("Hand opened");
00150 
00151       result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
00152       action_server->setSucceeded( result );
00153 
00154       hand_occupied = false;
00155 
00156       break;
00157     default:
00158       ROS_ERROR("Shadow Robot grasp execution: unknown goal code (%d)", goal->goal);
00159       action_server->setAborted();
00160       return;
00161     }
00162 
00163     //TODO: check the actual state of the hand and compare to sent targets?
00164     //object_manipulation_msgs::GraspHandPostureExecutionResult result;
00165     //result.result.value = object_manipulation_msgs::ManipulationResult::SUCCESS;
00166     //action_server->setSucceeded( result );
00167   }
00168 }
00169 
00170 int main(int argc, char** argv)
00171 {
00172   ros::init(argc, argv, "hand_posture_execution");
00173 
00174   //ros::AsyncSpinner spinner(1); //Use 1 thread
00175   //spinner.start();
00176   shadowrobot::SrHandPostureExecutionSimpleAction move_arm;
00177   ros::spin();
00178   //ros::waitForShutdown();
00179 
00180   return 0;
00181 }
00182 
00183 /* For the emacs weenies in the crowd.
00184 Local Variables:
00185    c-basic-offset: 2
00186 End:
00187 */
00188 // vim: expandtab:ts=2:sw=2


sr_move_arm
Author(s): Ugo Cupcic
autogenerated on Fri Jan 3 2014 12:07:38