Update of /cvsroot/playerstage/code/gazebo/server
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv18792/server
Modified Files:
World.cc World.hh
Log Message:
Player simulation interface support
Index: World.hh
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/server/World.hh,v
retrieving revision 1.48
retrieving revision 1.49
diff -C2 -d -r1.48 -r1.49
*** World.hh 14 Feb 2006 03:00:25 -0000 1.48
--- World.hh 7 Jan 2007 00:26:05 -0000 1.49
***************
*** 123,126 ****
--- 123,129 ----
public: dSpaceID GetSpaceId() {return this->spaceId;}
+ /// @brief Get a reference to a model from its id
+ private: Model *GetModelFromId(char *name);
+
// World file
private: WorldFile *worldFile;
Index: World.cc
===================================================================
RCS file: /cvsroot/playerstage/code/gazebo/server/World.cc,v
retrieving revision 1.94
retrieving revision 1.95
diff -C2 -d -r1.94 -r1.95
*** World.cc 14 Feb 2006 03:00:24 -0000 1.94
--- World.cc 7 Jan 2007 00:26:05 -0000 1.95
***************
*** 213,216 ****
--- 213,217 ----
gz_sim_lock(this->gz_sim, 1);
this->gz_sim->data->pause = this->pause;
+ sem_init(&this->gz_sim->data->pending_request,0,1);
gz_sim_unlock(this->gz_sim);
***************
*** 625,628 ****
--- 626,630 ----
Model *model;
GzPose pose;
+ GzVector rot;
gz_sim_data_t *data;
bool reset, save, pause;
***************
*** 637,640 ****
--- 639,702 ----
save = data->save;
data->save = 0;
+ if (data->request.status == GAZEBO_SIM_REQ_STATUS_PENDING)
+ {
+ switch (data->request.type)
+ {
+ case GAZEBO_SIM_REQ_GETPOSE2D:
+ case GAZEBO_SIM_REQ_GETPOSE3D:
+ if ((model=this->GetModelFromId(data->request.modelid))!=NULL)
+ {
+ pose = model->GetPose();
+ rot = GzQuaternToEuler(pose.rot);
+ data->request.pos[0] = pose.pos.x;
+ data->request.pos[1] = pose.pos.y;
+ data->request.pos[2] = pose.pos.z;
+ data->request.rot[0] = rot.x;
+ data->request.rot[1] = rot.y;
+ data->request.rot[2] = rot.z;
+ data->request.simtime = data->sim_time;
+ data->request.status = GAZEBO_SIM_REQ_STATUS_SERVICED;
+ }
+ else
+ {
+ PRINT_WARN1("Received request for unknown object
\"%s\".",data->request.modelid);
+ data->request.status = GAZEBO_SIM_REQ_STATUS_FAILED;
+ }
+ break;
+ case GAZEBO_SIM_REQ_SETPOSE2D:
+ case GAZEBO_SIM_REQ_SETPOSE3D:
+ if ((model=this->GetModelFromId(data->request.modelid))!=NULL)
+ {
+ pose = model->GetPose();
+ rot = GzQuaternToEuler(pose.rot);
+
+ pose.pos.x = data->request.pos[0];
+ pose.pos.y = data->request.pos[1];
+ if (data->request.type==GAZEBO_SIM_REQ_SETPOSE3D)
+ pose.pos.z = data->request.pos[2];
+
+ if (data->request.type==GAZEBO_SIM_REQ_SETPOSE3D)
+ {
+ rot.x = data->request.rot[0];
+ rot.y = data->request.rot[1];
+ }
+ rot.z = data->request.rot[2];
+ pose.rot = GzQuaternFromEuler(rot.x, rot.y, rot.z);
+ model->SetPose(pose);
+ data->request.status = GAZEBO_SIM_REQ_STATUS_SERVICED;
+ }
+ else
+ {
+ PRINT_WARN1("Received request for unknown object
\"%s\".",data->request.modelid);
+ data->request.status = GAZEBO_SIM_REQ_STATUS_FAILED;
+ }
+ break;
+ default:
+ PRINT_WARN("Unknown request type to simulator interface");
+ data->request.status = GAZEBO_SIM_REQ_STATUS_FAILED;
+ }
+ // Unlocking so that the interface can make new requests
+ sem_post(&data->pending_request);
+ }
gz_sim_unlock(this->gz_sim);
***************
*** 899,901 ****
--- 961,972 ----
+ //////////////////////////////////////////////////////////////////////////////
+ // Finds the model based on its name
+ Model *World::GetModelFromId(char *name)
+ {
+ for (int i=0; i<this->modelCount; i++)
+ if (!strcmp(this->models[i]->GetId(),name))
+ return this->models[i];
+ return NULL;
+ }
-------------------------------------------------------------------------
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
