Revision: 6896
http://playerstage.svn.sourceforge.net/playerstage/?rev=6896&view=rev
Author: natepak
Date: 2008-07-21 18:16:50 +0000 (Mon, 21 Jul 2008)
Log Message:
-----------
Add simulation interface request/response queue
Modified Paths:
--------------
code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/SimulationInterface.cc
code/gazebo/trunk/player/SimulationInterface.hh
code/gazebo/trunk/server/World.cc
Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-21
16:12:15 UTC (rev 6895)
+++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-21
18:16:50 UTC (rev 6896)
@@ -34,19 +34,19 @@
// Example of how to move a model (box1_model)
uint8_t name[512] = "pioneer2dx_model1";
- uint8_t cmd[32] = "set_pose3d";
for (int i=0; i< 10; i++)
{
simIface->Lock(1);
- memcpy(simIface->data->model_name, name, 512);
- memcpy(simIface->data->model_req,cmd, 32);
+ gazebo::SimulationRequestData *request =
&(simIface->data->requests[simIface->data->requestCount++]);
- simIface->data->model_pose.pos.x = i+.1;
- simIface->data->model_pose.pos.y = 0;
- simIface->data->model_pose.pos.z = 0.2;
+ request->type = gazebo::SimulationRequestData::SET_POSE2D;
+ memcpy(request->modelName, name, 512);
+ request->modelPose.pos.x = i+.1;
+ request->modelPose.pos.y = 0;
+
simIface->Unlock();
usleep(100000);
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 16:12:15 UTC (rev
6895)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2008-07-21 18:16:50 UTC (rev
6896)
@@ -366,7 +366,25 @@
\{
*/
+#define GAZEBO_SIMULATION_MAX_REQUESTS 128
+class SimulationRequestData
+{
+ public: enum Type { PAUSE,
+ RESET,
+ SAVE,
+ GET_POSE3D,
+ GET_POSE2D,
+ SET_POSE3D,
+ SET_POSE2D,
+ };
+
+ public: Type type;
+ public: char modelName[512];
+ public: Pose modelPose;
+
+};
+
/// \brief Simulation interface data
class SimulationData
{
@@ -383,29 +401,14 @@
/// state of the simulation : 0 paused, 1 running -1 not_started/exiting
public: int state;
-
- /// Pause simulation (set by client) should check the state
- /// Changes the state of the simulation from pause to play and viceversa.
- public: bool pause;
- /// Reset simulation (set by client)
- public: int reset;
+ /// Array of requests to the simulator
+ public: SimulationRequestData requests[GAZEBO_SIMULATION_MAX_REQUESTS];
+ public: unsigned int requestCount;
- /// Save current poses to world file (set by client)
- public: int save;
-
- /// Name of the model to get/set data
- public: char model_name[512];
-
- /// Type of request
- /// - "get_pose" Sets model_pose to the pose of model_name
- /// - "set_pose3d" Set the model_name to model_pose
- /// - "set_pose2d" Set the model_name to model_pose
- public: char model_req[32];
-
- /// Pose of the model.
- /// \sa model_req
- public: Pose model_pose;
+ /// Array of request results from the simulator
+ public: SimulationRequestData results[GAZEBO_SIMULATION_MAX_REQUESTS];
+ public: unsigned int resultCount;
};
/// \brief Common simulation interface
Modified: code/gazebo/trunk/player/SimulationInterface.cc
===================================================================
--- code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 16:12:15 UTC
(rev 6895)
+++ code/gazebo/trunk/player/SimulationInterface.cc 2008-07-21 18:16:50 UTC
(rev 6896)
@@ -65,130 +65,112 @@
int SimulationInterface::ProcessMessage(QueuePointer &respQueue,
player_msghdr_t *hdr, void *data)
{
+ if (this->responseQueue)
+ delete this->responseQueue;
- timespec sleepTime = {0, 10000000};
+ this->responseQueue = new QueuePointer(respQueue);
+
/// Set a 3D pose
if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_SET_POSE3D,
this->device_addr))
{
+ gazebo::SimulationRequestData *gzReq = NULL;
player_simulation_pose3d_req_t *req =
(player_simulation_pose3d_req_t*)(data);
this->iface->Lock(1);
- strcpy((char*)this->iface->data->model_name,req->name);
- strcpy((char*)this->iface->data->model_req,"set_pose3d");
+ gzReq = &(this->iface->data->requests[ this->iface->data->requestCount++
]);
- this->iface->data->model_pose.pos.x = req->pose.px;
- this->iface->data->model_pose.pos.y = req->pose.py;
- this->iface->data->model_pose.pos.z = req->pose.pz;
+ gzReq->type = gazebo::SimulationRequestData::SET_POSE3D;
+ strcpy((char*)gzReq->modelName, req->name);
- this->iface->data->model_pose.roll = req->pose.proll;
- this->iface->data->model_pose.pitch = req->pose.ppitch;
- this->iface->data->model_pose.yaw = req->pose.pyaw;
+ gzReq->modelPose.pos.x = req->pose.px;
+ gzReq->modelPose.pos.y = req->pose.py;
+ gzReq->modelPose.pos.z = req->pose.pz;
+
+ gzReq->modelPose.roll = req->pose.proll;
+ gzReq->modelPose.pitch = req->pose.ppitch;
+ gzReq->modelPose.yaw = req->pose.pyaw;
+
this->iface->Unlock();
this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_SET_POSE3D);
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_SIMULATION_REQ_SET_POSE3D);
}
/// Set a 2D pose
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_SIMULATION_REQ_SET_POSE2D,
this->device_addr))
+ PLAYER_SIMULATION_REQ_SET_POSE2D,
+ this->device_addr))
{
+ gazebo::SimulationRequestData *gzReq = NULL;
+
player_simulation_pose2d_req_t *req =
(player_simulation_pose2d_req_t*)(data);
this->iface->Lock(1);
- strcpy((char*)this->iface->data->model_name,req->name);
- strcpy((char*)this->iface->data->model_req,"set_pose2d");
+ gzReq = &(this->iface->data->requests[ this->iface->data->requestCount++]);
- this->iface->data->model_pose.pos.x = req->pose.px;
- this->iface->data->model_pose.pos.y = req->pose.py;
- this->iface->data->model_pose.yaw = req->pose.pa;
+ gzReq->type = gazebo::SimulationRequestData::SET_POSE2D;
+
+ strcpy((char*)gzReq->modelName, req->name);
+
+ gzReq->modelPose.pos.x = req->pose.px;
+ gzReq->modelPose.pos.y = req->pose.py;
+ gzReq->modelPose.yaw = req->pose.pa;
+
this->iface->Unlock();
this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_SET_POSE2D);
+ PLAYER_MSGTYPE_RESP_ACK,
+ PLAYER_SIMULATION_REQ_SET_POSE2D);
}
/// Get a 3d pose
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_SIMULATION_REQ_GET_POSE3D,
this->device_addr))
+ PLAYER_SIMULATION_REQ_GET_POSE3D,
+ this->device_addr))
{
- bool response = false;
+ gazebo::SimulationRequestData *gzReq = NULL;
player_simulation_pose3d_req_t *req =
(player_simulation_pose3d_req_t*)(data);
this->iface->Lock(1);
- strcpy((char*)this->iface->data->model_name, req->name);
- strcpy((char*)this->iface->data->model_req,"get_pose");
+ gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]);
- this->iface->Unlock();
+ gzReq->type = gazebo::SimulationRequestData::GET_POSE3D;
- // Wait for response from gazebo
- while (!response)
- {
- this->iface->Lock(1);
- response = strcmp((char*)this->iface->data->model_name,"") == 0;
- this->iface->Unlock();
- nanosleep(&sleepTime, NULL);
- }
+ strcpy((char*)gzReq->modelName, req->name);
- this->iface->Lock(1);
- req->pose.px = this->iface->data->model_pose.pos.x;
- req->pose.py = this->iface->data->model_pose.pos.y;
- req->pose.pz = this->iface->data->model_pose.pos.z;
-
- req->pose.proll = this->iface->data->model_pose.roll;
- req->pose.ppitch = this->iface->data->model_pose.pitch;
- req->pose.pyaw = this->iface->data->model_pose.yaw;
this->iface->Unlock();
-
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_POSE3D,
- req, sizeof(*req), NULL);
}
/// Get a 2D pose
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
PLAYER_SIMULATION_REQ_GET_POSE2D,
this->device_addr))
{
- bool response = false;
+ gazebo::SimulationRequestData *gzReq = NULL;
player_simulation_pose2d_req_t *req =
(player_simulation_pose2d_req_t*)(data);
this->iface->Lock(1);
- strcpy((char*)this->iface->data->model_name, req->name);
- strcpy((char*)this->iface->data->model_req,"get_pose");
+ gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]);
- this->iface->Unlock();
+ gzReq->type = gazebo::SimulationRequestData::GET_POSE2D;
- // Wait for response from gazebo
- while (!response)
- {
- this->iface->Lock(1);
- response = strcmp((char*)this->iface->data->model_name,"") == 0;
- this->iface->Unlock();
- nanosleep(&sleepTime, NULL);
- }
+ strcpy((char*)gzReq->modelName, req->name);
- this->iface->Lock(1);
- req->pose.px = this->iface->data->model_pose.pos.x;
- req->pose.py = this->iface->data->model_pose.pos.y;
- req->pose.pa = this->iface->data->model_pose.yaw;
this->iface->Unlock();
-
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_POSE2D,
- req, sizeof(*req), NULL);
}
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ,
- PLAYER_SIMULATION_REQ_GET_PROPERTY,
this->device_addr))
+ PLAYER_SIMULATION_REQ_GET_PROPERTY,
+ this->device_addr))
{
player_simulation_property_req_t *req =
(player_simulation_property_req_t*)(data);
@@ -241,6 +223,65 @@
// called from GazeboDriver::Update
void SimulationInterface::Update()
{
+ gazebo::SimulationRequestData *result = NULL;
+ this->iface->Lock(1);
+
+ for (unsigned int i=0; i < this->iface->data->resultCount; i++)
+ {
+ result = &(this->iface->data->results[i]);
+
+ switch (result->type)
+ {
+ case gazebo::SimulationRequestData::PAUSE:
+ case gazebo::SimulationRequestData::RESET:
+ case gazebo::SimulationRequestData::SAVE:
+ case gazebo::SimulationRequestData::SET_POSE2D:
+ case gazebo::SimulationRequestData::SET_POSE3D:
+ break;
+
+ case gazebo::SimulationRequestData::GET_POSE3D:
+ {
+ player_simulation_pose3d_req_t req;
+
+ strcpy(req.name, result->modelName);
+ req.name_count = strlen(req.name);
+
+ req.pose.px = result->modelPose.pos.x;
+ req.pose.py = result->modelPose.pos.y;
+ req.pose.pz = result->modelPose.pos.z;
+
+ req.pose.proll = result->modelPose.roll;
+ req.pose.ppitch = result->modelPose.pitch;
+ req.pose.pyaw = result->modelPose.yaw;
+
+ this->driver->Publish(this->device_addr, *(this->responseQueue),
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE3D,
+ &req, sizeof(req), NULL);
+
+ break;
+ }
+ case gazebo::SimulationRequestData::GET_POSE2D:
+ {
+ player_simulation_pose2d_req_t req;
+
+ strcpy(req.name, result->modelName);
+ req.name_count = strlen(req.name);
+
+ req.pose.px = result->modelPose.pos.x;
+ req.pose.py = result->modelPose.pos.y;
+ req.pose.pa = result->modelPose.yaw;
+
+ this->driver->Publish(this->device_addr, *(this->responseQueue),
+ PLAYER_MSGTYPE_RESP_ACK, PLAYER_SIMULATION_REQ_GET_POSE2D,
+ &req, sizeof(req), NULL);
+
+ break;
+ }
+ }
+ }
+
+ this->iface->Unlock();
+
return;
}
Modified: code/gazebo/trunk/player/SimulationInterface.hh
===================================================================
--- code/gazebo/trunk/player/SimulationInterface.hh 2008-07-21 16:12:15 UTC
(rev 6895)
+++ code/gazebo/trunk/player/SimulationInterface.hh 2008-07-21 18:16:50 UTC
(rev 6896)
@@ -81,6 +81,8 @@
/// \brief Pointer to the Simulation Interface
public: SimulationIface *iface;
+
+ private: QueuePointer *responseQueue;
};
/// \}
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2008-07-21 16:12:15 UTC (rev 6895)
+++ code/gazebo/trunk/server/World.cc 2008-07-21 18:16:50 UTC (rev 6896)
@@ -150,11 +150,6 @@
(*miter)->Init();
}
- // Set initial simulator state
- this->simIface->Lock(1);
- this->simIface->data->pause = Simulator::Instance()->IsPaused();
- this->simIface->Unlock();
-
this->physicsEngine->Init();
this->toAddModels.clear();
@@ -495,91 +490,139 @@
// Update the simulation interface
void World::UpdateSimulationIface()
{
+ SimulationRequestData *results = NULL;
+
//TODO: Move this method to simulator? Hard because of the models
this->simIface->Lock(1);
- if (this->simIface->GetOpenCount() > 0)
+ if (this->simIface->GetOpenCount() <= 0)
{
- this->simIface->data->simTime = Simulator::Instance()->GetSimTime();
- this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime();
- this->simIface->data->realTime = Simulator::Instance()->GetRealTime();
- this->simIface->data->state = !Simulator::Instance()->GetUserPause();
+ this->simIface->Unlock();
+ return;
+ }
- if (this->simIface->data->reset)
- {
- this->Reset();
- this->simIface->data->reset = 0;
- }
+ results = this->simIface->data->results;
+ this->simIface->data->resultCount = 0;
- if (this->simIface->data->pause)
- {
-
Simulator::Instance()->SetUserPause(!Simulator::Instance()->GetUserPause());
- this->simIface->data->pause = 0;
- }
+ this->simIface->data->simTime = Simulator::Instance()->GetSimTime();
+ this->simIface->data->pauseTime = Simulator::Instance()->GetPauseTime();
+ this->simIface->data->realTime = Simulator::Instance()->GetRealTime();
+ this->simIface->data->state = !Simulator::Instance()->GetUserPause();
- //TODO: save as , load
- if (this->simIface->data->save)
- {
- Simulator::Instance()->Save();
- this->simIface->data->save=0;
- }
+ unsigned int requestCount = this->simIface->data->requestCount;
- // If the model_name is set, then a request has been received
- if (strcmp((char*)this->simIface->data->model_name,"")!=0)
+ // Max sure the request count is valid
+ if (this->simIface->data->requestCount > GAZEBO_SIMULATION_MAX_REQUESTS)
+ {
+ gzerr(0) << "Request count[" << this->simIface->data->requestCount << "]
greater than max allowable[" << GAZEBO_SIMULATION_MAX_REQUESTS << "]\n";
+
+ requestCount = GAZEBO_SIMULATION_MAX_REQUESTS;
+ }
+
+ // Process all the requests
+ for (unsigned int i=0; i < requestCount; i++)
+ {
+ SimulationRequestData *req = &(this->simIface->data->requests[i]);
+
+ switch (req->type)
{
- /// Get the model requested
- Model *model =
this->GetModelByName((char*)this->simIface->data->model_name);
- if (model)
- {
- std::string req = (char*)this->simIface->data->model_req;
- if (req == "get_pose")
- {
- Pose3d pose = model->GetPose();
- Vector3 rot = pose.rot.GetAsEuler();
+ case SimulationRequestData::PAUSE:
+ Simulator::Instance()->SetUserPause(
+ !Simulator::Instance()->GetUserPause());
+ break;
- this->simIface->data->model_pose.pos.x = pose.pos.x;
- this->simIface->data->model_pose.pos.y = pose.pos.y;
- this->simIface->data->model_pose.pos.z = pose.pos.z;
+ case SimulationRequestData::RESET:
+ this->Reset();
+ break;
+ case SimulationRequestData::SAVE:
+ Simulator::Instance()->Save();
+ break;
- this->simIface->data->model_pose.roll = rot.x;
- this->simIface->data->model_pose.pitch = rot.y;
- this->simIface->data->model_pose.yaw = rot.z;
- }
- else if (req == "set_pose3d")
+ case SimulationRequestData::SET_POSE3D:
{
Pose3d pose;
+ Model *model = this->GetModelByName((char*)req->modelName);
+ if (model)
+ {
+ pose.pos.x = req->modelPose.pos.x;
+ pose.pos.y = req->modelPose.pos.y;
+ pose.pos.z = req->modelPose.pos.z;
- pose.pos.x = this->simIface->data->model_pose.pos.x;
- pose.pos.y = this->simIface->data->model_pose.pos.y;
- pose.pos.z = this->simIface->data->model_pose.pos.z;
- pose.rot.SetFromEuler(Vector3(this->simIface->data->model_pose.roll,
- this->simIface->data->model_pose.pitch,
- this->simIface->data->model_pose.yaw));
- model->SetPose(pose);
+ pose.rot.SetFromEuler(
+ Vector3(req->modelPose.roll,
+ req->modelPose.pitch,
+ req->modelPose.yaw));
+ model->SetPose(pose);
+ }
+ else
+ {
+ gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Set Pose 3d Request.\n";
+ }
+
+ break;
}
- else if (req == "set_pose2d")
+
+ case SimulationRequestData::GET_POSE3D:
{
- Pose3d pose = model->GetPose();
- Vector3 rot = pose.rot.GetAsEuler();
+ Model *model = this->GetModelByName((char*)req->modelName);
+ if (model)
+ {
+ Pose3d pose = model->GetPose();
+ Vector3 rot = pose.rot.GetAsEuler();
- pose.pos.x = this->simIface->data->model_pose.pos.x;
- pose.pos.y = this->simIface->data->model_pose.pos.y;
+ results->type = req->type;
- pose.rot.SetFromEuler(Vector3(rot.x, rot.y,
- this->simIface->data->model_pose.yaw));
- model->SetPose(pose);
+ strcpy( results->modelName, req->modelName);
+ results->modelPose.pos.x = pose.pos.x;
+ results->modelPose.pos.y = pose.pos.y;
+ results->modelPose.pos.z = pose.pos.z;
+
+ results->modelPose.roll = rot.x;
+ results->modelPose.pitch = rot.y;
+ results->modelPose.yaw = rot.z;
+
+ results++;
+ this->simIface->data->resultCount++;
+ }
+ else
+ {
+ gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Get Pose 3d Request.\n";
+ }
+
+ break;
}
- }
- else
- {
- gzmsg(-1) << "Simulation Iface: Model[" <<
this->simIface->data->model_name << "] does not exist\n";
- }
+ case SimulationRequestData::SET_POSE2D:
+ {
+ Model *model = this->GetModelByName((char*)req->modelName);
+ if (model)
+ {
+ Pose3d pose = model->GetPose();
+ Vector3 rot = pose.rot.GetAsEuler();
- strcpy((char*)this->simIface->data->model_name, "");
+ pose.pos.x = req->modelPose.pos.x;
+ pose.pos.y = req->modelPose.pos.y;
+
+ pose.rot.SetFromEuler(Vector3(rot.x, rot.y,
+ req->modelPose.yaw));
+ model->SetPose(pose);
+ }
+ else
+ {
+ gzerr(0) << "Invalid model name[" << req->modelName << "] in
simulation interface Set Pose 2d Request.\n";
+ }
+ break;
+ }
+
+ default:
+ gzerr(0) << "Unknown simulation iface request[" << req->type << "]\n";
+ break;
}
+
+ this->simIface->data->requestCount = 0;
}
+
this->simIface->Unlock();
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
