Update of /cvsroot/playerstage/code/gazebo/player
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18792/player
Modified Files:
GazeboDriver.cc SimulationInterface.cc
Log Message:
Player simulation interface support
Index: SimulationInterface.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/SimulationInterface.cc,v
retrieving revision 1.3
retrieving revision 1.4
diff -C2 -d -r1.3 -r1.4
*** SimulationInterface.cc 1 Oct 2006 18:55:14 -0000 1.3
--- SimulationInterface.cc 7 Jan 2007 00:26:05 -0000 1.4
***************
*** 24,29 ****
* CVS: $Id$
*/
! #include "gazebo.h"
#include "GazeboDriver.hh"
#include "SimulationInterface.hh"
--- 24,50 ----
* CVS: $Id$
*/
+ /**
+ @addtogroup player
+ @par Simulation Interface
+ - PLAYER_SIMULATION_REQ_GET_POSE2D (the use of the 3D equivalent is
recommended)
+ - PLAYER_SIMULATION_REQ_SET_POSE2D (the use of the 3D equivalent is
recommended)
+ - PLAYER_SIMULATION_REQ_GET_POSE3D
+ - PLAYER_SIMULATION_REQ_SET_POSE3D
+ - PLAYER_SIMULATION_REQ_SET_PROPERTY. The available object properties are:
+ -# <B>Object name:</B> __GAZEBO__
+ <BR><B>Property:</B> PAUSE
+ <BR><B>Type:</B> int
+ <BR><B>Description:</B> Set to 1 to pause the simulation (currently not
working)
+ -# <B>Object name:</B> __GAZEBO__
+ <BR><B>Property:</B> RESET
+ <BR><B>Type:</B> int
+ <BR><B>Description:</B> Set to 1 to reset the simulation
+ -# <B>Object name:</B> __GAZEBO__
+ <BR><B>Property:</B> SAVE
+ <BR><B>Type:</B> int
+ <BR><B>Description:</B> Set to 1 to save the current status of the simulation
in the world file
+ */
! #include "../libgazebo/gazebo.h"
#include "GazeboDriver.hh"
#include "SimulationInterface.hh"
***************
*** 34,37 ****
--- 55,59 ----
: GazeboInterface(addr, driver, cf, section)
{
+
// ID of the server
int serverId = atoi((char*)cf->ReadString(section,"server_id","default"));
***************
*** 39,42 ****
--- 61,67 ----
// Initialize the Client. Creates the SHM connection
GazeboClient::Init(serverId, "");
+
+ // The driver should always be available to accept requests
+ driver->alwayson = TRUE;
}
***************
*** 52,61 ****
player_msghdr_t *hdr, void *data)
{
! if(Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
! PLAYER_SIMULATION_REQ_SET_POSE2D, this->device_addr))
{
! printf("Simulation setting pose2d\n");
}
return 0;
}
--- 77,271 ----
player_msghdr_t *hdr, void *data)
{
! #ifdef PLAYER_SIMULATION_REQ_GET_POSE3D
! // A variable to store new request info to send to the simulator
! gz_sim_req_item req_item;
! // 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))
{
! player_simulation_pose2d_req_t* req =
(player_simulation_pose2d_req_t*)data;
! 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;
! strncpy(req_item.modelid, req->name, req->name_count);
! gz_sim_lock(GazeboClient::sim,1);
!
memcpy(&GazeboClient::sim->data->request,&req_item,sizeof(gz_sim_req_item));
! gz_sim_unlock(GazeboClient::sim);
!
! // Wait for world to service and unlock
! sem_wait(&GazeboClient::sim->data->pending_request);
!
! if
(GazeboClient::sim->data->request.status==GAZEBO_SIM_REQ_STATUS_FAILED)
! {
! this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE2D, NULL, 0, NULL);
! }
! else
! {
! gz_sim_lock(GazeboClient::sim,1);
!
memcpy(&req_item,&GazeboClient::sim->data->request,sizeof(gz_sim_req_item));
! gz_sim_unlock(GazeboClient::sim);
! reply.pose.px = req_item.pos[0];
! reply.pose.py = req_item.pos[1];
! 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);
! }
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_POSE2D, this->device_addr))
! {
! player_simulation_pose2d_req_t* req =
(player_simulation_pose2d_req_t*)data;
!
! req_item.type = GAZEBO_SIM_REQ_SETPOSE2D; // Wait for world to unlock
!
! req_item.status = GAZEBO_SIM_REQ_STATUS_PENDING;
! strncpy(req_item.modelid, req->name, req->name_count);
! req_item.pos[0] = req->pose.px;
! req_item.pos[1] = req->pose.py;
! req_item.rot[2] = req->pose.pa;
! gz_sim_lock(GazeboClient::sim,1);
!
memcpy(&GazeboClient::sim->data->request,&req_item,sizeof(gz_sim_req_item));
! gz_sim_unlock(GazeboClient::sim);
!
! // Wait for world to service and unlock
! sem_wait(&GazeboClient::sim->data->pending_request);
!
! if
(GazeboClient::sim->data->request.status==GAZEBO_SIM_REQ_STATUS_FAILED)
! {
! 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);
! }
! }
! else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE3D, this->device_addr))
! {
! player_simulation_pose3d_req_t* req =
(player_simulation_pose3d_req_t*)data;
! player_simulation_pose3d_req_t reply;
! memcpy(&reply,req,sizeof(player_simulation_pose3d_req_t));
!
! req_item.type = GAZEBO_SIM_REQ_GETPOSE3D;
! req_item.status = GAZEBO_SIM_REQ_STATUS_PENDING;
! strncpy(req_item.modelid, req->name, req->name_count);
! gz_sim_lock(GazeboClient::sim,1);
!
memcpy(&GazeboClient::sim->data->request,&req_item,sizeof(gz_sim_req_item));
! gz_sim_unlock(GazeboClient::sim);
!
! // Wait for world to service and unlock
! sem_wait(&GazeboClient::sim->data->pending_request);
!
! if
(GazeboClient::sim->data->request.status==GAZEBO_SIM_REQ_STATUS_FAILED)
! {
! this->driver->Publish(this->device_addr, respQueue,
PLAYER_MSGTYPE_RESP_NACK,
! PLAYER_SIMULATION_REQ_GET_POSE3D, NULL, 0, NULL);
! }
! else
! {
! gz_sim_lock(GazeboClient::sim,1);
!
memcpy(&req_item,&GazeboClient::sim->data->request,sizeof(gz_sim_req_item));
! gz_sim_unlock(GazeboClient::sim);
! reply.pose.px = req_item.pos[0];
! reply.pose.py = req_item.pos[1];
! reply.pose.pz = req_item.pos[2];
! reply.pose.proll = req_item.rot[0];
! reply.pose.ppitch = req_item.rot[1];
! reply.pose.pyaw = req_item.rot[2];
! 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);
! }
}
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_POSE3D, this->device_addr))
+ {
+ player_simulation_pose3d_req_t* req =
(player_simulation_pose3d_req_t*)data;
+ req_item.type = GAZEBO_SIM_REQ_SETPOSE3D;
+ req_item.status = GAZEBO_SIM_REQ_STATUS_PENDING;
+ strncpy(req_item.modelid, req->name, req->name_count);
+ req_item.pos[0] = req->pose.px;
+ req_item.pos[1] = req->pose.py;
+ req_item.pos[2] = req->pose.pz;
+ req_item.rot[0] = req->pose.proll;
+ req_item.rot[1] = req->pose.ppitch;
+ req_item.rot[2] = req->pose.pyaw;
+ gz_sim_lock(GazeboClient::sim,1);
+
memcpy(&GazeboClient::sim->data->request,&req_item,sizeof(gz_sim_req_item));
+ gz_sim_unlock(GazeboClient::sim);
+
+ // Wait for world to service and unlock
+ sem_wait(&GazeboClient::sim->data->pending_request);
+
+ if
(GazeboClient::sim->data->request.status==GAZEBO_SIM_REQ_STATUS_FAILED)
+ {
+ 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);
+ }
+ }
+ else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_PROPERTY, this->device_addr))
+ {
+ player_simulation_property_req_t* req =
(player_simulation_property_req_t*)data;
+ int value;
+
+ if (!strncmp(req->name,"__GAZEBO__",req->name_count))
+ {
+ if (!strncmp(req->prop,"PAUSE",req->prop_count))
+ {
+ PLAYER_WARN("Pausing simulation");
+ memcpy(&value,req->value,sizeof(int));
+ gz_sim_lock(GazeboClient::sim,1);
+ GazeboClient::sim->data->pause = value;
+ 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))
+ {
+ PLAYER_WARN("Reseting simulation");
+ memcpy(&value,req->value,sizeof(int));
+ gz_sim_lock(GazeboClient::sim,1);
+ GazeboClient::sim->data->reset = value;
+ 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))
+ {
+ PLAYER_WARN("Saving simulation data to world file");
+ memcpy(&value,req->value,sizeof(int));
+ gz_sim_lock(GazeboClient::sim,1);
+ GazeboClient::sim->data->save = value;
+ 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
+ {
+ 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);
+ }
+ }
+ else
+ {
+ 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);
+ }
+ }
+
+ // Unlocking
+ sem_post(&GazeboClient::sim->data->pending_request);
+ #endif
return 0;
}
Index: GazeboDriver.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/player/GazeboDriver.cc,v
retrieving revision 1.8
retrieving revision 1.9
diff -C2 -d -r1.8 -r1.9
*** GazeboDriver.cc 2 Nov 2006 17:06:31 -0000 1.8
--- GazeboDriver.cc 7 Jan 2007 00:26:05 -0000 1.9
***************
*** 201,205 ****
{
iface = this->devices[i];
! iface->Update();
}
--- 201,206 ----
{
iface = this->devices[i];
! if (iface->device_addr.interf != PLAYER_SIMULATION_CODE)
! iface->Update();
}
-------------------------------------------------------------------------
Take Surveys. Earn Cash. Influence the Future of IT
Join SourceForge.net's Techsay panel and you'll get the chance to share your
opinions on IT & business topics through brief surveys - and earn cash
http://www.techsay.com/default.php?page=join.php&p=sourceforge&CID=DEVDEV
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
