Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030 #include "p_driver.h"
00031
00032
00033
00041 #include "p_driver.h"
00042 using namespace Stg;
00043
00044 InterfaceGripper::InterfaceGripper( player_devaddr_t addr,
00045 StgDriver* driver,
00046 ConfigFile* cf,
00047 int section )
00048 : InterfaceModel( addr, driver, cf, section, "gripper" )
00049 {
00050
00051 }
00052
00053 void InterfaceGripper::Publish( void )
00054 {
00055 ModelGripper * gmod = reinterpret_cast<ModelGripper*> (this->mod);
00056
00057 player_gripper_data_t pdata;
00058 memset( &pdata, 0, sizeof(pdata) );
00059
00060
00061 pdata.beams = 0;
00062 pdata.beams |= gmod->GetConfig().beam[0] ? 0x04 : 0x00;
00063 pdata.beams |= gmod->GetConfig().beam[1] ? 0x08 : 0x00;
00064
00065 switch (gmod->GetConfig().paddles)
00066 {
00067 case ModelGripper::PADDLE_OPEN:
00068 pdata.state = PLAYER_GRIPPER_STATE_OPEN;
00069 break;
00070 case ModelGripper::PADDLE_CLOSED:
00071 pdata.state = PLAYER_GRIPPER_STATE_CLOSED;
00072 break;
00073 case ModelGripper::PADDLE_OPENING:
00074 case ModelGripper::PADDLE_CLOSING:
00075 pdata.state = PLAYER_GRIPPER_STATE_MOVING;
00076 break;
00077 default:
00078 pdata.state = PLAYER_GRIPPER_STATE_ERROR;
00079 }
00080
00081
00082 this->driver->Publish(this->addr,
00083 PLAYER_MSGTYPE_DATA,
00084 PLAYER_GRIPPER_DATA_STATE,
00085 (void*)&pdata);
00086
00087 }
00088
00089 int InterfaceGripper::ProcessMessage(QueuePointer& resp_queue,
00090 player_msghdr_t* hdr,
00091 void* data)
00092 {
00093 ModelGripper * gmod = reinterpret_cast<ModelGripper*> (this->mod);
00094
00095 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_OPEN, this->addr))
00096 {
00097 gmod->CommandOpen();
00098 return 0;
00099 }
00100 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_CLOSE, this->addr))
00101 {
00102 gmod->CommandClose();
00103 return 0;
00104 }
00105
00106 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, PLAYER_GRIPPER_REQ_GET_GEOM, this->addr))
00107 {
00108 Geom geom = this->mod->GetGeom();
00109 Pose pose = this->mod->GetPose();
00110
00111 player_gripper_geom_t pgeom;
00112 memset(&pgeom, 0, sizeof(pgeom));
00113
00114 pgeom.pose.px = pose.x;
00115 pgeom.pose.py = pose.y;
00116 pgeom.pose.pz = pose.z;
00117 pgeom.pose.pyaw = pose.a;
00118
00119 pgeom.outer_size.sl = geom.size.x;
00120 pgeom.outer_size.sw = geom.size.y;
00121 pgeom.outer_size.sh = geom.size.z;
00122
00123 pgeom.num_beams = 2;
00124
00125 this->driver->Publish(this->addr, resp_queue, PLAYER_MSGTYPE_RESP_ACK, PLAYER_GRIPPER_REQ_GET_GEOM, (void*)&pgeom);
00126 return(0);
00127 }
00128
00129 PRINT_WARN2( "stage gripper doesn't support message id:%d/%d", hdr->type, hdr->subtype );
00130 return -1;
00131 }
00132