grasp_execution_action_server.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Southwest Research Institute
00003 
00004    Licensed under the Apache License, Version 2.0 (the "License");
00005    you may not use this file except in compliance with the License.
00006    You may obtain a copy of the License at
00007 
00008      http://www.apache.org/licenses/LICENSE-2.0
00009 
00010    Unless required by applicable law or agreed to in writing, software
00011    distributed under the License is distributed on an "AS IS" BASIS,
00012    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013    See the License for the specific language governing permissions and
00014    limitations under the License.
00015  */
00016 
00017 
00018 #include <ros/ros.h>
00019 #include <actionlib/server/action_server.h>
00020 
00021 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
00022 #include <object_manipulation_msgs/GraspHandPostureExecutionGoal.h>
00023 #include <simple_message/socket/tcp_client.h>
00024 #include <simple_message/smpl_msg_connection.h>
00025 #include <simple_message/simple_message.h>
00026 #include <mtconnect_grasp_action/gripper_message.h>
00027 
00028 using namespace object_manipulation_msgs;
00029 using namespace actionlib;
00030 using namespace industrial::smpl_msg_connection;
00031 using namespace industrial::simple_message;
00032 using namespace mtconnect_cnc_robot_example::gripper_message;
00033 
00034 static const std::string PARAM_IP_ADDRESS = "ip_address";
00035 static const std::string PARAM_PORT_NUMBER = "port_number";
00036 
00037 class GraspExecutionAction
00038 {
00039 private:
00040   typedef ActionServer<GraspHandPostureExecutionAction> GEAS;
00041   typedef GEAS::GoalHandle GoalHandle;
00042 public:
00043   GraspExecutionAction(ros::NodeHandle &n, SmplMsgConnection *robot) :
00044     node_(n),
00045     action_server_(node_, "grasp_execution_action",
00046                    boost::bind(&GraspExecutionAction::goalCB, this, _1),
00047                    boost::bind(&GraspExecutionAction::cancelCB, this, _1),
00048                    false)
00049   {
00050     ros::NodeHandle pn("~");
00051 
00052     robot_ = robot;
00053     robot_->makeConnect();
00054     action_server_.start();
00055     
00056     ROS_INFO("Grasp execution action node started");
00057 
00058     /*
00059     //A little bit of debug
00060     GripperMessage gMsg;
00061     SimpleMessage request;
00062     SimpleMessage reply;
00063 
00064     gMsg.init(GripperOperationTypes::INIT);
00065     gMsg.toRequest(request);
00066     this->robot_->sendAndReceiveMsg(request, reply);
00067     ROS_INFO("Gripper initialized");
00068 
00069     gMsg.init(GripperOperationTypes::CLOSE);
00070     gMsg.toRequest(request);
00071     this->robot_->sendAndReceiveMsg(request, reply);
00072     ROS_INFO("Gripper closed");
00073 
00074     gMsg.init(GripperOperationTypes::OPEN);
00075     gMsg.toRequest(request);
00076     this->robot_->sendAndReceiveMsg(request, reply);
00077     ROS_INFO("Gripper opened");
00078     */
00079   }
00080 
00081   ~GraspExecutionAction()
00082   {
00083   }
00084 
00085 private:
00086 
00087 
00088   void goalCB(GoalHandle gh)
00089   {
00090     GripperMessage gMsg;
00091     SimpleMessage request;
00092     SimpleMessage reply;
00093 
00094     ROS_DEBUG("Received grasping goal");
00095 
00096     while (!(robot_->isConnected()))
00097     {
00098       ROS_DEBUG("Reconnecting");
00099       robot_->makeConnect();
00100     }
00101       
00102 
00103     switch(gh.getGoal()->goal)
00104     {
00105       case GraspHandPostureExecutionGoal::PRE_GRASP:
00106 
00107         gh.setAccepted();
00108         ROS_WARN("Pre-grasp is not supported by this gripper");
00109         gh.setSucceeded();
00110         break;
00111 
00112       case GraspHandPostureExecutionGoal::GRASP:
00113       case GraspHandPostureExecutionGoal::RELEASE:
00114 
00115         gh.setAccepted();
00116         switch(gh.getGoal()->goal)
00117         {
00118           case GraspHandPostureExecutionGoal::GRASP:
00119             ROS_INFO("Executing a gripper grasp");
00120             gMsg.init(GripperOperationTypes::CLOSE);
00121             break;
00122           case GraspHandPostureExecutionGoal::RELEASE:
00123             ROS_INFO("Executing a gripper release");
00124             gMsg.init(GripperOperationTypes::OPEN);
00125             break;
00126         }
00127         gMsg.toRequest(request);
00128         this->robot_->sendAndReceiveMsg(request, reply);
00129 
00130         switch(reply.getReplyCode())
00131         {
00132           case ReplyTypes::SUCCESS:
00133             ROS_INFO("Robot gripper returned success");
00134             gh.setSucceeded();
00135             break;
00136           case ReplyTypes::FAILURE:
00137             ROS_ERROR("Robot gripper returned failure");
00138             gh.setCanceled();
00139             break;
00140         }
00141         break;
00142 
00143           default:
00144             gh.setRejected();
00145             break;
00146 
00147     }
00148   }
00149 
00150   void cancelCB(GoalHandle gh)
00151   {
00152     ROS_ERROR("Action cancel not supported for grasp execution action");
00153     gh.setRejected();
00154   }
00155 
00156 
00157   ros::NodeHandle node_;
00158   GEAS action_server_;
00159 
00160   industrial::smpl_msg_connection::SmplMsgConnection *robot_;
00161 
00162 };
00163 
00164 int main(int argc, char** argv)
00165 {
00166         ros::init(argc, argv, "grasp_execution_action_node");
00167         ros::NodeHandle nh("~");
00168         std::string ip_address;
00169         int port_number;
00170 
00171         // reading parameters and proceeding
00172         if(nh.getParam(PARAM_IP_ADDRESS,ip_address) && nh.getParam(PARAM_PORT_NUMBER,port_number))
00173         {
00174                 ROS_INFO("Grasp action connecting to IP address: %s and port: %i", ip_address.c_str(),port_number);
00175                 industrial::tcp_client::TcpClient robot;
00176 
00177                 robot.init(const_cast<char*>(ip_address.c_str()), port_number);
00178                 GraspExecutionAction ge(nh, &robot);
00179 
00180                 ros::spin();
00181         }
00182         else
00183         {
00184                 ROS_ERROR("Missing 'ip_address' and 'port_number' private parameters");
00185         }
00186 
00187 
00188   return 0;
00189 }
00190 
00191 
00192 
00193 


mtconnect_grasp_action
Author(s): Shaun M. Edwards
autogenerated on Mon Jan 6 2014 11:31:10