Revision: 7902
http://playerstage.svn.sourceforge.net/playerstage/?rev=7902&view=rev
Author: natepak
Date: 2009-06-25 15:43:44 +0000 (Thu, 25 Jun 2009)
Log Message:
-----------
Added fixes for model name scoping
Modified Paths:
--------------
code/gazebo/trunk/cmake/SearchForStuff.cmake
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Entity.hh
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/Joint.cc
code/gazebo/trunk/webgazebo/WebGazebo.hh
code/gazebo/trunk/worlds/lights.world
code/gazebo/trunk/worlds/models/pioneer2dx.model
code/gazebo/trunk/worlds/pioneer2dx.world
Modified: code/gazebo/trunk/cmake/SearchForStuff.cmake
===================================================================
--- code/gazebo/trunk/cmake/SearchForStuff.cmake 2009-06-25 14:04:08 UTC
(rev 7901)
+++ code/gazebo/trunk/cmake/SearchForStuff.cmake 2009-06-25 15:43:44 UTC
(rev 7902)
@@ -20,7 +20,7 @@
pkg_check_modules(OGRE OGRE>=${OGRE_VERSION})
IF (NOT OGRE_FOUND)
- MESSAGE (SEND_ERROR "\nError: Ogre3d version ${OGRE_VERSION} and
development files not found. See the following website: http://www.orge3d.org")
+ MESSAGE (SEND_ERROR "\nError: Ogre3d version >=${OGRE_VERSION} and
development files not found. See the following website: http://www.orge3d.org")
ELSE (NOT OGRE_FOUND)
SET (OGRE_LIBRARY_PATH ${OGRE_LIBRARY_DIRS} CACHE INTERNAL "Ogre library
path")
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2009-06-25 14:04:08 UTC (rev 7901)
+++ code/gazebo/trunk/server/Entity.cc 2009-06-25 15:43:44 UTC (rev 7902)
@@ -241,6 +241,26 @@
}
////////////////////////////////////////////////////////////////////////////////
+/// Return the name of this entity with the model scope
+/// model1::...::modelN::entityName
+std::string Entity::GetScopedName()
+{
+ Entity *p = this->parent;
+ std::string scopedName = this->GetName();
+
+ while (p)
+ {
+ Model *m = dynamic_cast<Model*>(p);
+ if (m)
+ scopedName.insert(0, m->GetName()+"::");
+ p = p->GetParent();
+ }
+
+ return scopedName;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
// Get the pose of the entity relative to its parent
Pose3d Entity::GetPoseRelative() const
{
Modified: code/gazebo/trunk/server/Entity.hh
===================================================================
--- code/gazebo/trunk/server/Entity.hh 2009-06-25 14:04:08 UTC (rev 7901)
+++ code/gazebo/trunk/server/Entity.hh 2009-06-25 15:43:44 UTC (rev 7902)
@@ -127,6 +127,10 @@
/// \brief Return true if the entity is a model
public: bool IsModel();
+ /// \brief Return the name of this entity with the model scope
+ /// model1::...::modelN::entityName
+ public: std::string GetScopedName();
+
/// \brief Parent of this entity
protected: Entity *parent;
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-06-25 14:04:08 UTC (rev 7901)
+++ code/gazebo/trunk/server/Model.cc 2009-06-25 15:43:44 UTC (rev 7902)
@@ -160,23 +160,27 @@
void Model::Load(XMLConfigNode *node, bool removeDuplicate)
{
XMLConfigNode *childNode;
+ std::string scopedName;
Pose3d pose;
Model* dup;
this->nameP->Load(node);
+
+ scopedName = this->GetScopedName();
+
// Look for existing models by the same name
- if((dup = World::Instance()->GetModelByName(this->nameP->GetValue())) !=
NULL)
+ if((dup = World::Instance()->GetModelByName(scopedName)) != NULL)
{
if(!removeDuplicate)
{
- gzthrow("Duplicate model name" + this->nameP->GetValue() + "\n");
+ gzthrow("Duplicate model name" + scopedName + "\n");
}
else
{
// Delete the existing one (this should only be reached when called
// via the factory interface).
- printf("Queuing duplicate model %s (%p) for deletion\n",
this->nameP->GetValue().c_str(), dup);
- World::Instance()->DeleteEntity(this->nameP->GetValue().c_str());
+ printf("Queuing duplicate model %s (%p) for deletion\n",
scopedName.c_str(), dup);
+ World::Instance()->DeleteEntity(scopedName.c_str());
}
}
@@ -256,7 +260,7 @@
// Create the graphics iface handler
this->graphicsHandler = new GraphicsIfaceHandler();
- this->graphicsHandler->Load(this->GetName(), this);
+ this->graphicsHandler->Load(this->GetScopedName(), this);
// Get the name of the python module
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-06-25 14:04:08 UTC (rev
7901)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-06-25 15:43:44 UTC (rev
7902)
@@ -153,7 +153,7 @@
while (childNode)
{
std::ostringstream visname;
- visname << this->GetName() << "_VISUAL_" << this->visuals.size();
+ visname << this->GetScopedName() << "_VISUAL_" << this->visuals.size();
OgreVisual *visual = OgreCreator::Instance()->CreateVisual(
visname.str(), this->visualNode, this);
@@ -179,7 +179,7 @@
Vector3 max(aabb[1], aabb[3], aabb[5]);
std::ostringstream visname;
- visname << this->GetName() << "_BBVISUAL" ;
+ visname << this->GetScopedName() << "_BBVISUAL" ;
this->bbVisual = OgreCreator::Instance()->CreateVisual(
visname.str(), this->visualNode);
Modified: code/gazebo/trunk/server/physics/Joint.cc
===================================================================
--- code/gazebo/trunk/server/physics/Joint.cc 2009-06-25 14:04:08 UTC (rev
7901)
+++ code/gazebo/trunk/server/physics/Joint.cc 2009-06-25 15:43:44 UTC (rev
7902)
@@ -135,7 +135,7 @@
this->Attach(body1,body2);
std::ostringstream visname;
- visname << this->GetName() << "_VISUAL";
+ visname << this->model->GetScopedName() << "::" << this->GetName() <<
"_VISUAL";
/// Add a renderable for the joint
this->visual = OgreCreator::Instance()->CreateVisual(
Modified: code/gazebo/trunk/webgazebo/WebGazebo.hh
===================================================================
--- code/gazebo/trunk/webgazebo/WebGazebo.hh 2009-06-25 14:04:08 UTC (rev
7901)
+++ code/gazebo/trunk/webgazebo/WebGazebo.hh 2009-06-25 15:43:44 UTC (rev
7902)
@@ -95,6 +95,9 @@
/** Get the current simulation time */
virtual websim::Time GetTime();
+ virtual bool ClockStart() {return true;}
+
+ virtual bool ClockStop() {return true;}
// end WebSim Interface
============================================================
Modified: code/gazebo/trunk/worlds/lights.world
===================================================================
--- code/gazebo/trunk/worlds/lights.world 2009-06-25 14:04:08 UTC (rev
7901)
+++ code/gazebo/trunk/worlds/lights.world 2009-06-25 15:43:44 UTC (rev
7902)
@@ -20,6 +20,7 @@
<gravity>0 0 -9.8</gravity>
<cfm>10e-5</cfm>
<erp>0.8</erp>
+ <updateRate>-1</updateRate>
</physics:ode>
<rendering:gui>
@@ -29,10 +30,11 @@
</rendering:gui>
<rendering:ogre>
- <ambient>0.8 0.8 0.8 0.0</ambient>
+ <ambient>0 0 0 0</ambient>
<sky>
<material>Gazebo/CloudySky</material>
</sky>
+ <grid>false</grid>
</rendering:ogre>
<!-- Ground Plane -->
@@ -44,111 +46,115 @@
<body:plane name="plane1_body">
<geom:plane name="plane1_geom">
<normal>0 0 1</normal>
- <size>2000 2000</size>
- <segments>1 1</segments>
- <uvTile>100 100</uvTile>
- <material>Gazebo/Grey</material>
+ <size>1000 1000</size>
+ <segments>200 200</segments>
+ <uvTile>1000 1000</uvTile>
+ <material>Gazebo/GrayGrid</material>
</geom:plane>
</body:plane>
</model:physical>
<!-- Red Spot light -->
- <model:renderable name="spot_red">
+ <!--<model:renderable name="spot_red">
<xyz>0.0 0.0 2.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<enableGravity>false</enableGravity>
<light>
<type>spot</type>
- <direction>0 -1 0</direction>
- <diffuseColor>0.1 0.0 0.0</diffuseColor>
+ <direction>0 0 -1</direction>
+ <diffuseColor>1.0 0.0 0.0</diffuseColor>
<specularColor>0.2 0.0 0.0</specularColor>
- <range>5</range>
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
- <attenuation>0.2 0.2 0.00</attenuation>
+ <range>20</range>
+ <attenuation>0.1 0.02 0.00</attenuation>
</light>
</model:renderable>
+ -->
<!-- Green Spot light -->
- <model:renderable name="spot_green">
- <xyz>0.0 1.0 2.0</xyz>
+ <!--<model:renderable name="spot_green">
+ <xyz>0.0 0.0 5.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<enableGravity>false</enableGravity>
<light>
<type>spot</type>
- <direction>0 -1 0</direction>
- <diffuseColor>0.0 0.1 0.0</diffuseColor>
+ <direction>0 0 -1</direction>
+ <diffuseColor>0.0 1.0 0.0</diffuseColor>
<specularColor>0.0 0.2 0.0</specularColor>
- <range>10</range>
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
- <attenuation>0.2 0.1 0.00</attenuation>
+ <range>40</range>
+ <attenuation>0.1 0. 0.00</attenuation>
+ <spotCone>2 20 2</spotCone>
</light>
</model:renderable>
+ -->
+ <!--
<model:physical name="sphere1_model">
- <xyz>0 2 5.5</xyz>
+ <xyz>0 0 5.5</xyz>
<rpy>0.0 0.0 0.0</rpy>
<static>false</static>
<body:sphere name="sphere1_body">
<geom:sphere name="sphere1_geom">
- <mesh>default</mesh>
- <size>0.5</size>
- <mass>1.0</mass>
- <material>Gazebo/Rocky</material>
+ <size>0.2</size>
+ <mass>0.1</mass>
+
+ <visual>
+ <size>0.4 0.4 0.4</size>
+ <mesh>unit_sphere</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
</geom:sphere>
</body:sphere>
- <!-- Blue Spot light -->
<model:renderable name="spot_blue">
<xyz>0.0 0 0</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <enableGravity>false</enableGravity>
+ <enableGravity>true</enableGravity>
<light>
<type>spot</type>
- <direction>0 -1 0</direction>
- <diffuseColor>0.0 0.0 0.1</diffuseColor>
- <specularColor>0.0 0.0 0.2</specularColor>
- <attenuation>100 0.2 0.1 0.00</attenuation>
+ <direction>0 0 -1</direction>
+ <diffuseColor>0.0 0.0 1.0</diffuseColor>
+ <specularColor>0.0 0.0 0.1</specularColor>
+ <attenuation>.1 0.0 0.0</attenuation>
+ <range>5</range>
- <!-- InnerAngle OutterAngle Falloff -->
<spotCone>8.0 10.0 2.0</spotCone>
</light>
</model:renderable>
</model:physical>
+ -->
-
<!-- Yellow Point light -->
- <model:renderable name="point_yellow">
- <xyz>5 5 5</xyz>
+ <!--<model:renderable name="point_yellow">
+ <xyz>0 0 .5</xyz>
<enableGravity>false</enableGravity>
<light>
<type>point</type>
- <diffuseColor>0.1 0.1 0.0</diffuseColor>
- <specularColor>0.3 0.3 0.3</specularColor>
- <range>10</range>
+ <diffuseColor>1.0 1.0 0.0</diffuseColor>
+ <specularColor>1.0 1.0 0.0</specularColor>
+ <range>20</range>
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
- <attenuation>0.4 0.1 0.0</attenuation>
+ <attenuation>0.4 0.1 0.001</attenuation>
</light>
</model:renderable>
+ -->
+
<!-- White Directional light -->
<model:renderable name="directional_white">
<enableGravity>false</enableGravity>
<light>
<type>directional</type>
- <direction>0 -1 0</direction>
+ <direction>0 0 -1</direction>
<diffuseColor>0.4 0.4 0.4</diffuseColor>
- <specularColor>0.0 0.0 0.0</specularColor>
- <range>100</range>
-
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
- <attenuation>0.0 1.0 0.4</attenuation>
+ <specularColor>0.2 0.2 0.2</specularColor>
+ <range>1000</range>
+ <attenuation>0.2 0.0 0.0</attenuation>
</light>
</model:renderable>
Modified: code/gazebo/trunk/worlds/models/pioneer2dx.model
===================================================================
--- code/gazebo/trunk/worlds/models/pioneer2dx.model 2009-06-25 14:04:08 UTC
(rev 7901)
+++ code/gazebo/trunk/worlds/models/pioneer2dx.model 2009-06-25 15:43:44 UTC
(rev 7902)
@@ -7,21 +7,13 @@
xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body"
xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom"
xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
+ xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint"
>
<xyz>0 0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
-<controller:differential_position2d name="controller1">
- <leftJoint>left_wheel_hinge</leftJoint>
- <rightJoint>right_wheel_hinge</rightJoint>
- <wheelSeparation>0.39</wheelSeparation>
- <wheelDiameter>0.15</wheelDiameter>
- <torque>5</torque>
- <interface:position name="position_iface_0"/>
-</controller:differential_position2d>
-
-
<canonicalBody>chassis_body</canonicalBody>
<body:box name="chassis_body">
@@ -181,5 +173,14 @@
<cfm>10e-5</cfm>
</joint:ball>
+<controller:differential_position2d name="controller1">
+ <leftJoint>left_wheel_hinge</leftJoint>
+ <rightJoint>right_wheel_hinge</rightJoint>
+ <wheelSeparation>0.39</wheelSeparation>
+ <wheelDiameter>0.15</wheelDiameter>
+ <torque>5</torque>
+ <interface:position name="position_iface_0"/>
+</controller:differential_position2d>
+
</model:physical>
Modified: code/gazebo/trunk/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/trunk/worlds/pioneer2dx.world 2009-06-25 14:04:08 UTC (rev
7901)
+++ code/gazebo/trunk/worlds/pioneer2dx.world 2009-06-25 15:43:44 UTC (rev
7902)
@@ -122,6 +122,30 @@
</include>
</model:physical>
+ <model:physical name="pioneer2dx_model2">
+ <xyz>0 -1 .145</xyz>
+ <rpy>0.0 0.0 -90.0</rpy>
+ <collide>all</collide>
+
+ <model:physical name="laser">
+ <xyz>0.15 0 0.18</xyz>
+
+ <attach>
+ <parentBody>chassis_body</parentBody>
+ <myBody>laser_body</myBody>
+ </attach>
+
+ <include embedded="true">
+ <xi:include href="models/sicklms200.model" />
+ </include>
+ </model:physical>
+
+ <include embedded="true">
+ <xi:include href="models/pioneer2dx.model" />
+ </include>
+ </model:physical>
+
+
<model:physical name="box1_model">
<xyz>1 1.5 0.5</xyz>
<canonicalBody>box1_body</canonicalBody>
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