Update of /cvsroot/playerstage/code/gazebo/player
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv8594
Modified Files:
SimulationInterface.cc
Log Message:
Added ability to get property from simulator via player
Index: SimulationInterface.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/SimulationInterface.cc,v
retrieving revision 1.4
retrieving revision 1.5
diff -C2 -d -r1.4 -r1.5
*** SimulationInterface.cc 7 Jan 2007 00:26:05 -0000 1.4
--- SimulationInterface.cc 20 Apr 2007 01:00:43 -0000 1.5
***************
*** 82,86 ****
// Locking for a new request
sem_wait(&GazeboClient::sim->data->pending_request);
!
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr))
{
--- 82,86 ----
// Locking for a new request
sem_wait(&GazeboClient::sim->data->pending_request);
!
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D, this->device_addr))
{
***************
*** 88,92 ****
player_simulation_pose2d_req_t reply;
memcpy(&reply,req,sizeof(player_simulation_pose2d_req_t));
!
req_item.type = GAZEBO_SIM_REQ_GETPOSE2D;
req_item.status = GAZEBO_SIM_REQ_STATUS_PENDING;
--- 88,92 ----
player_simulation_pose2d_req_t reply;
memcpy(&reply,req,sizeof(player_simulation_pose2d_req_t));
!
req_item.type = GAZEBO_SIM_REQ_GETPOSE2D;
req_item.status = GAZEBO_SIM_REQ_STATUS_PENDING;
***************
*** 102,106 ****
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE2D, NULL, 0, NULL);
}
else
--- 102,106 ----
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE2D, NULL, 0, NULL);
}
else
***************
*** 113,117 ****
reply.pose.pa = req_item.rot[2];
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_GET_POSE2D, (void*)&reply,
sizeof(reply), NULL);
}
}
--- 113,117 ----
reply.pose.pa = req_item.rot[2];
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_GET_POSE2D, (void*)&reply, sizeof(reply),
NULL);
}
}
***************
*** 137,146 ****
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_POSE2D, NULL, 0, NULL);
}
else
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_POSE2D, NULL, 0, NULL);
}
}
--- 137,146 ----
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_POSE2D, NULL, 0, NULL);
}
else
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_POSE2D, NULL, 0, NULL);
}
}
***************
*** 164,168 ****
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE3D, NULL, 0, NULL);
}
else
--- 164,168 ----
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE3D, NULL, 0, NULL);
}
else
***************
*** 179,183 ****
reply.simtime = req_item.simtime;
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_GET_POSE3D, (void*)&reply,
sizeof(reply), NULL);
}
}
--- 179,183 ----
reply.simtime = req_item.simtime;
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_GET_POSE3D, (void*)&reply, sizeof(reply),
NULL);
}
}
***************
*** 205,214 ****
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_POSE3D, NULL, 0, NULL);
}
else
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_POSE3D, NULL, 0, NULL);
}
}
--- 205,214 ----
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_POSE3D, NULL, 0, NULL);
}
else
{
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_POSE3D, NULL, 0, NULL);
}
}
***************
*** 228,232 ****
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0,
NULL);
}
else if (!strncmp(req->prop,"RESET",req->prop_count))
--- 228,232 ----
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0, NULL);
}
else if (!strncmp(req->prop,"RESET",req->prop_count))
***************
*** 238,242 ****
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0,
NULL);
}
else if (!strncmp(req->prop,"SAVE",req->prop_count))
--- 238,242 ----
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0, NULL);
}
else if (!strncmp(req->prop,"SAVE",req->prop_count))
***************
*** 248,252 ****
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0,
NULL);
}
else
--- 248,252 ----
gz_sim_unlock(GazeboClient::sim);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0, NULL);
}
else
***************
*** 254,258 ****
PLAYER_WARN2("Request for unknown property \"%s\" of object
\"%s\"",req->prop, req->name);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0,
NULL);
}
}
--- 254,258 ----
PLAYER_WARN2("Request for unknown property \"%s\" of object
\"%s\"",req->prop, req->name);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0, NULL);
}
}
***************
*** 261,265 ****
PLAYER_WARN1("Request to set property for unknown object
\"%s\"",req->name);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0,
NULL);
}
}
--- 261,298 ----
PLAYER_WARN1("Request to set property for unknown object
\"%s\"",req->name);
this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_SET_PROPERTY, NULL, 0, NULL);
! }
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_PROPERTY, this->device_addr))
! {
! player_simulation_property_req_t* req =
(player_simulation_property_req_t*)data;
! player_simulation_property_req_t reply;
!
! memcpy(&reply,req,sizeof(player_simulation_property_req_t));
!
! printf("received a property request[%s][%s]\n",req->name, req->prop);
! if (!strncmp(req->name,"__GAZEBO__",req->name_count))
! {
! if (!strncmp(req->prop,"SIM_TIME",req->prop_count))
! {
! gz_sim_lock(GazeboClient::sim,1);
! memcpy(reply.value, &GazeboClient::sim->data->sim_time,
sizeof(double)) ;
! gz_sim_unlock(GazeboClient::sim);
! }
! else if (!strncmp(req->prop,"REAL_TIME",req->prop_count))
! {
! gz_sim_lock(GazeboClient::sim,1);
! memcpy(reply.value,
&GazeboClient::sim->data->real_time,sizeof(double)) ;
! gz_sim_unlock(GazeboClient::sim);
! }
! else if (!strncmp(req->prop,"PAUSE_TIME",req->prop_count))
! {
! gz_sim_lock(GazeboClient::sim,1);
! memcpy(reply.value,
&GazeboClient::sim->data->pause_time,sizeof(double)) ;
! gz_sim_unlock(GazeboClient::sim);
! }
! this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_PROPERTY, (void*)&reply,
sizeof(reply), NULL);
!
!
}
}
-------------------------------------------------------------------------
This SF.net email is sponsored by DB2 Express
Download DB2 Express C - the FREE version of DB2 express and take
control of your XML. No limits. Just data. Click to get it now.
http://sourceforge.net/powerbar/db2/
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
