Revision: 7947
http://playerstage.svn.sourceforge.net/playerstage/?rev=7947&view=rev
Author: robotos
Date: 2009-07-03 10:43:03 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
Lots of properties can be required from player. Added new property to ask for
Fiducial information
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/FiducialInterface.cc
code/gazebo/trunk/player/SimulationInterface.cc
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/worlds/pioneer2dx.world
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-07-03 10:34:38 UTC (rev
7946)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-07-03 10:43:03 UTC (rev
7947)
@@ -433,6 +433,7 @@
GET_NUM_CHILDREN,
GET_CHILD_NAME,
GET_MODEL_NAME,
+ GET_MODEL_FIDUCIAL_ID,
GET_MODEL_EXTENT,
GET_MODEL_INTERFACES, // for getting interfaces as well
as the models which are ancestors of interfaces
GET_INTERFACE_TYPE, // if the model is not an
interface 'unknown' is returned
Modified: code/gazebo/trunk/player/FiducialInterface.cc
===================================================================
--- code/gazebo/trunk/player/FiducialInterface.cc 2009-07-03 10:34:38 UTC
(rev 7946)
+++ code/gazebo/trunk/player/FiducialInterface.cc 2009-07-03 10:43:03 UTC
(rev 7947)
@@ -192,6 +192,8 @@
this->data.fiducials = new
player_fiducial_item_t[this->data.fiducials_count];
}
+ std::cout << "fiducial count " << this->iface->data->count << std::endl;
+
for (i = 0; i < this->iface->data->count; i++)
{
fid = this->iface->data->fids + i;
Modified: code/gazebo/trunk/player/SimulationInterface.cc
===================================================================
--- code/gazebo/trunk/player/SimulationInterface.cc 2009-07-03 10:34:38 UTC
(rev 7946)
+++ code/gazebo/trunk/player/SimulationInterface.cc 2009-07-03 10:43:03 UTC
(rev 7947)
@@ -197,9 +197,9 @@
std::string name = req->name;
std::string prop = req->prop;
- this->iface->Lock(1);
if (name == "world")
{
+ this->iface->Lock(1);
req->value = new char[ sizeof(double) ];
req->value_count = sizeof(double);
@@ -219,31 +219,71 @@
{
memcpy(req->value, &this->iface->data->state, sizeof(int));
}
+ this->iface->Unlock();
+ this->driver->Publish(this->device_addr, respQueue,
+ PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_PROPERTY, req, sizeof(*req), NULL);
+
+ if (req->value)
+ {
+ delete [] req->value;
+ req->value = NULL;
+ }
}
else
{
- std::cerr << "Invalid Name[" << name << "]. Must be \"world\".\n";
+ this->iface->Lock(1);
+ gazebo::SimulationRequestData *gzReq = NULL;
+ gzReq =
&(this->iface->data->requests[this->iface->data->requestCount++]);
+
+ if (prop == "num_children")
+ {
+ gzReq->type = gazebo::SimulationRequestData::GET_NUM_CHILDREN;
+ strcpy((char*)gzReq->modelName, req->name);
+ }
+ else if (prop == "model_name")
+ {
+ gzReq->type = gazebo::SimulationRequestData::GET_MODEL_NAME;
+ gzReq->uintValue = req->index;
+ }
+ else if (prop == "child_name")
+ {
+ gzReq->type = gazebo::SimulationRequestData::GET_CHILD_NAME;
+ gzReq->uintValue = req->index;
+ strcpy((char*)gzReq->modelName, req->name);
+ }
+ else if (prop == "fiducial_id")
+ {
+ gzReq->type = gazebo::SimulationRequestData::GET_MODEL_FIDUCIAL_ID;
+ strcpy((char*)gzReq->modelName, req->name);
+ }
+ else if (prop == "model_type")
+ {
+ gzReq->type = gazebo::SimulationRequestData::GET_MODEL_TYPE;
+ strcpy((char*)gzReq->modelName, req->name);
+ }
+ else if ((prop == "num_models") && (name == "world"))
+ {
+ gazebo::SimulationRequestData *gzReq = NULL;
+ gzReq =
&(this->iface->data->requests[this->iface->data->requestCount++]);
+ gzReq->type = gazebo::SimulationRequestData::GET_NUM_MODELS;
+ }
+ else
+ {
+ this->iface->data->requestCount--; //we did ++ but didnt introduced
real request
+ std::cerr << "The object [" << name << "] does not have the property
[" << prop << "].\n";
+ }
+ this->iface->Unlock();
}
- this->iface->Unlock();
-
- this->driver->Publish(this->device_addr, respQueue,
- PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_PROPERTY, req, sizeof(*req), NULL);
-
- if (req->value)
- {
- delete [] req->value;
- req->value = NULL;
- }
}
-/* This depends on Player SVN, wait to the release of Player 3.0 to uncomment?
else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD,
PLAYER_SIMULATION_CMD_PAUSE,
this->device_addr))
{
this->iface->Lock(1);
- SimulationRequestData *gzReq =
&&(this->iface->data->requests[this->iface->data->requestCount++]);
+ gazebo::SimulationRequestData *gzReq = NULL;
+ gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]);
gzReq->type = SimulationRequestData::PAUSE;
this->iface->Unlock();
}
@@ -252,7 +292,8 @@
this->device_addr))
{
this->iface->Lock(1);
- SimulationRequestData *gzReq =
&&(this->iface->data->requests[this->iface->data->requestCount++]);
+ gazebo::SimulationRequestData *gzReq = NULL;
+ gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]);
gzReq->type = SimulationRequestData::RESET;
this->iface->Unlock();
}
@@ -261,17 +302,18 @@
this->device_addr))
{
this->iface->Lock(1);
- SimulationRequestData *gzReq =
&&(this->iface->data->requests[this->iface->data->requestCount++]);
+ gazebo::SimulationRequestData *gzReq = NULL;
+ gzReq = &(this->iface->data->requests[this->iface->data->requestCount++]);
gzReq->type = SimulationRequestData::SAVE;
this->iface->Unlock();
}
-*/
else
printf("Unhandled Process message[%d][%d]\n",0,0);
return 0;
}
+
///////////////////////////////////////////////////////////////////////////////
// Update this interface, publish new info. This is
// called from GazeboDriver::Update
@@ -346,6 +388,22 @@
break;
}
+
+ case gazebo::SimulationRequestData::GET_MODEL_FIDUCIAL_ID:
+ {
+ player_simulation_property_req_t *req ;
+ memset (req, 0, sizeof(player_simulation_property_req_t));
+
+ memcpy(req->value, (const void *) response->uintValue,
sizeof(response->uintValue));
+ req->value_count = sizeof(response->uintValue);
+ this->driver->Publish(this->device_addr, *(this->responseQueue),
+ PLAYER_MSGTYPE_RESP_ACK,
PLAYER_SIMULATION_REQ_GET_PROPERTY, req, sizeof(*req), NULL);
+ break;
+ }
+
+
+
+
}
}
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-07-03 10:34:38 UTC (rev 7946)
+++ code/gazebo/trunk/server/Model.cc 2009-07-03 10:43:03 UTC (rev 7947)
@@ -1053,7 +1053,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Set the collide mode of the model
+/// Set the Fiducial Id of the model
void Model::SetLaserFiducialId( const int &id )
{
Body *body;
@@ -1068,7 +1068,16 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Set the collide mode of the model
+/// Get the Fiducial Id of the Model
+int Model::GetLaserFiducialId( )
+{
+ //this is not correct if geoms set their own Fiducial.
+ //you can not expect it to be correct in that case anyway ...
+ return **this->laserFiducialP;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the Laser retro property of the model
void Model::SetLaserRetro( const float &retro )
{
Body *body;
Modified: code/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-07-03 10:34:38 UTC (rev 7946)
+++ code/gazebo/trunk/server/Model.hh 2009-07-03 10:43:03 UTC (rev 7947)
@@ -200,6 +200,9 @@
/// \brief Set the laser fiducial integer Id of the model
public: void SetLaserFiducialId( const int &id );
+ /// \brief Get the laser fiducial integer Id of the model
+ public: int GetLaserFiducialId();
+
/// \brief Set the laser retro reflectiveness of the model
public: void SetLaserRetro( const float &retro );
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-07-03 10:34:38 UTC (rev 7946)
+++ code/gazebo/trunk/server/World.cc 2009-07-03 10:43:03 UTC (rev 7947)
@@ -859,7 +859,18 @@
break;
}
-
+ case SimulationRequestData::GET_MODEL_FIDUCIAL_ID:
+ {
+ Model *model = this->GetModelByName((char*)req->modelName);
+ if (model)
+ {
+ response->type = req->type;
+ response->uintValue = model->GetLaserFiducialId();
+ response++;
+ this->simIface->data->responseCount += 1;
+ break;
+ }
+ }
case SimulationRequestData::GET_MODEL_TYPE:
{
Model *model = this->GetModelByName((char*)req->modelName);
Modified: code/gazebo/trunk/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/trunk/worlds/pioneer2dx.world 2009-07-03 10:34:38 UTC (rev
7946)
+++ code/gazebo/trunk/worlds/pioneer2dx.world 2009-07-03 10:43:03 UTC (rev
7947)
@@ -75,6 +75,8 @@
<xyz>2.15 -1.68 .3</xyz>
<rpy>0.0 0.0 0.0</rpy>
<static>false</static>
+ <laserFiducialId>1</laserFiducialId>
+ <laserRetro>0.5</laserRetro>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
@@ -150,6 +152,8 @@
<xyz>1 1.5 0.5</xyz>
<canonicalBody>box1_body</canonicalBody>
<static>true</static>
+ <laserFiducialId>2</laserFiducialId>
+ <laserRetro>0.5</laserRetro>
<body:box name="box1_body">
<geom:box name="box1_geom">
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
