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
00038
00039
00040 #include "p_driver.h"
00041
00042
00043
00044 InterfaceActArray::InterfaceActArray( player_devaddr_t addr,
00045 StgDriver* driver,
00046 ConfigFile* cf,
00047 int section )
00048
00049 : InterfaceModel( addr, driver, cf, section, "actuator" )
00050 {
00051
00052 }
00053
00054 int InterfaceActArray::ProcessMessage(QueuePointer &resp_queue,
00055 player_msghdr_t* hdr,
00056 void* data)
00057 {
00058 Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod;
00059
00060
00061 if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
00062 PLAYER_ACTARRAY_REQ_GET_GEOM,
00063 this->addr))
00064 {
00065 Stg::Geom geom = actmod->GetGeom();
00066
00067
00068
00069 player_actarray_actuatorgeom_t actuator = {0};
00070 actuator.min = actmod->GetMinPosition();
00071 actuator.max = actmod->GetMaxPosition();
00072 actuator.type = PLAYER_ACTARRAY_TYPE_LINEAR;
00073
00074 player_actarray_geom_t pgeom = {0};
00075 pgeom.base_pos.px = geom.pose.x;
00076 pgeom.base_pos.py = geom.pose.y;
00077 pgeom.base_pos.pz = geom.pose.z;
00078
00079 pgeom.base_orientation.pyaw = geom.pose.a;
00080
00081 pgeom.actuators_count = 1;
00082 pgeom.actuators = &actuator;
00083
00084 this->driver->Publish( this->addr, resp_queue,
00085 PLAYER_MSGTYPE_RESP_ACK,
00086 PLAYER_ACTARRAY_REQ_GET_GEOM,
00087 (void*)&pgeom);
00088 return 0;
00089 }
00090
00091 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00092 PLAYER_ACTARRAY_CMD_POS,
00093 this->addr))
00094 {
00095 player_actarray_position_cmd_t &cmd = *reinterpret_cast<player_actarray_position_cmd_t*> (data);
00096 actmod->GoTo(cmd.position);
00097 return 0;
00098 }
00099
00100 else if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
00101 PLAYER_ACTARRAY_CMD_SPEED,
00102 this->addr))
00103 {
00104 player_actarray_speed_cmd_t &cmd = *reinterpret_cast<player_actarray_speed_cmd_t*> (data);
00105 actmod->SetSpeed(cmd.speed);
00106 return 0;
00107 }
00108
00109
00110
00111 PRINT_WARN2( "actuator doesn't support msg with type %d subtype %d",
00112 hdr->type, hdr->subtype);
00113 return(-1);
00114 }
00115
00116 void InterfaceActArray::Publish( void )
00117 {
00118 Stg::ModelActuator* actmod = (Stg::ModelActuator*)this->mod;
00119
00120
00121 player_actarray_actuator_t act = {0};
00122 act.position = actmod->GetPosition();
00123 act.speed = actmod->GetSpeed();
00124 if (act.speed != 0)
00125 act.state = PLAYER_ACTARRAY_ACTSTATE_MOVING;
00126 else
00127 act.state = PLAYER_ACTARRAY_ACTSTATE_IDLE;
00128
00129 player_actarray_data_t actdata = {0};
00130 actdata.actuators_count = 1;
00131 actdata.actuators = &act;
00132
00133
00134
00135 this->driver->Publish( this->addr,
00136 PLAYER_MSGTYPE_DATA, PLAYER_ACTARRAY_DATA_STATE,
00137 (void*)&actdata);
00138 }