Revision: 4466
http://playerstage.svn.sourceforge.net/playerstage/?rev=4466&view=rev
Author: natepak
Date: 2008-04-01 15:22:40 -0700 (Tue, 01 Apr 2008)
Log Message:
-----------
Fixed the terrain geometry
Modified Paths:
--------------
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/HeightmapGeom.cc
code/gazebo/trunk/worlds/terrain.world
code/gazebo/trunk/worlds/test.world
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2008-03-31 20:32:03 UTC (rev
4465)
+++ code/gazebo/trunk/server/physics/Geom.cc 2008-04-01 22:22:40 UTC (rev
4466)
@@ -113,7 +113,6 @@
pose.pos = node->GetVector3("xyz",Vector3(0,0,0));
pose.rot = node->GetRotation("rpy",Quatern());
-
// TODO: This should probably be true....but "true" breaks trimesh postions.
this->SetPose(pose, true);
@@ -279,7 +278,6 @@
// Set the pose relative to the body
void Geom::SetPose(const Pose3d &pose, bool updateCoM)
{
-
if (this->placeable && this->geomId)
{
Pose3d localPose;
Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-03-31 20:32:03 UTC
(rev 4465)
+++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-04-01 22:22:40 UTC
(rev 4466)
@@ -28,6 +28,7 @@
#include <Ogre.h>
#include <iostream>
#include <string.h>
+#include <math.h>
#include "GazeboError.hh"
#include "OgreAdaptor.hh"
@@ -147,16 +148,19 @@
this->terrainVertSize = tmpImage.getWidth();
+ float nf = log(this->terrainVertSize-1)/log(2);
+ int ni = log(this->terrainVertSize-1)/log(2);
// Make sure the heightmap image size is (2^n)+1 in size
- if ( (this->terrainVertSize-1) & (this->terrainVertSize-2) != 0)
+ if ( nf - ni != 0)
{
gzthrow("Heightmap image size must be (2^n)+1\n");
}
// Calculate a good tile size
- tileSize = (this->terrainVertSize-1)/8;
+ tileSize = pow( 2, ni/2 );
+
if (tileSize <= 2)
{
tileSize = 4;
@@ -168,13 +172,18 @@
this->terrainScale = this->terrainSize / this->terrainVertSize;
- this->odeVertSize = terrainVertSize * 1;
+ this->odeVertSize = this->terrainVertSize * 4;
this->odeScale = this->terrainSize / this->odeVertSize;
std::ostringstream stream;
-
+
+ /*std::cout << "Terrain Scale[" << this->terrainScale << "]\n";
+ std::cout << "ODE Scale[" << this->odeScale << "]\n";
std::cout << "Terrain Image[" << this->terrainImage << "] Size[" <<
this->terrainSize << "]\n";
+ printf("Terrain Size[%f %f %f]\n", this->terrainSize.x, this->terrainSize.y,
this->terrainSize.z);
+ printf("VertSize[%d] Tile Size[%d]\n", this->terrainVertSize, tileSize);
+ */
stream << "WorldTexture=" << worldTexture << "\n";
//The detail texture
@@ -249,12 +258,16 @@
this->SetGeom(this->geomId, false);
+ this->SetStatic(true);
+
//Rotate so Z is up, not Y (which is the default orientation)
- dMatrix3 R;
- dRSetIdentity(R);
- dRFromAxisAndAngle(R, 1, 0, 0, DTOR(90));
+ Quatern quat;
+ Pose3d pose = this->GetPose();
- dGeomSetRotation(this->geomId, R);
+ quat.SetFromEuler(Vector3(DTOR(90),0,0));
+ pose.rot = pose.rot * quat;
+ this->body->SetPose(pose);
+
delete [] mstr;
}
Modified: code/gazebo/trunk/worlds/terrain.world
===================================================================
--- code/gazebo/trunk/worlds/terrain.world 2008-03-31 20:32:03 UTC (rev
4465)
+++ code/gazebo/trunk/worlds/terrain.world 2008-04-01 22:22:40 UTC (rev
4466)
@@ -81,8 +81,8 @@
</model:physical>
<model:physical name="cam2_model">
- <xyz>210 -75 1.39</xyz>
- <rpy>0 2.5 -87</rpy>
+ <xyz>211.42 -78.48 2.57</xyz>
+ <rpy>0 13 0</rpy>
<static>true</static>
<body:empty name="cam2_body">
@@ -99,8 +99,8 @@
</model:physical>
<model:physical name="pioneer2dx_model2">
- <xyz>210 -78.18 1.15</xyz>
- <rpy>0 0 -130</rpy>
+ <xyz>214.76 -79.34 1.29</xyz>
+ <rpy>0 0 20</rpy>
<controller:differential_position2d name="controller1">
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
Modified: code/gazebo/trunk/worlds/test.world
===================================================================
--- code/gazebo/trunk/worlds/test.world 2008-03-31 20:32:03 UTC (rev 4465)
+++ code/gazebo/trunk/worlds/test.world 2008-04-01 22:22:40 UTC (rev 4466)
@@ -5,24 +5,22 @@
xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz";
xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model";
xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor";
- xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window";
- xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param";
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:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface";
- xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui";
xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering";
+
xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable";
xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller";
xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics";
>
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.03</stepTime>
- <gravity>0 0 -9.8</gravity>
+ <stepTime>0.02</stepTime>
+ <gravity>0 0 -9.80665</gravity>
<cfm>10e-5</cfm>
- <erp>0.8</erp>
+ <erp>0.3</erp>
</physics:ode>
<rendering:gui>
@@ -32,16 +30,43 @@
</rendering:gui>
<rendering:ogre>
- <ambient>0.5 0.5 0.5 1.0</ambient>
+ <ambient>1.0 1.0 1.0 1.0</ambient>
<sky>
<material>Gazebo/CloudySky</material>
</sky>
+
</rendering:ogre>
- <!-- Ground Plane -->
+ <model:physical name="sphere1_model">
+ <xyz>0 0 9.0</xyz>
+ <static>false</static>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <size>0.2</size>
+ <mass>1.0</mass>
+
+ <visual>
+ <scale>0.2 0.2 0.2</scale>
+ <material>Gazebo/Rocky</material>
+ <mesh>unit_sphere</mesh>
+ </visual>
+ </geom:sphere>
+ </body:sphere>
+ </model:physical>
+
+ <model:physical name="terrain_model">
+ <body:heightmap name ="terrain_body">
+ <geom:heightmap name="terrain_geom">
+ <image>test.jpg</image>
+ <worldTexture>test.jpg</worldTexture>
+ <detailTexture>test.jpg</detailTexture>
+ <size>17 17 1.0</size>
+ </geom:heightmap>
+ </body:heightmap>
+ </model:physical>
+
<model:physical name="plane1_model">
<xyz>0 0 0</xyz>
- <rpy>0 0 0</rpy>
<static>true</static>
<body:plane name="plane1_body">
@@ -55,66 +80,138 @@
</body:plane>
</model:physical>
- <!-- The camera -->
- <model:physical name="cam1_model">
- <xyz>0.07 -1.10 0.5</xyz>
- <rpy>0 12 52.5</rpy>
+ <model:physical name="cam2_model">
+ <xyz>-1.62 10.92 0.77</xyz>
+ <rpy>0 16 -79</rpy>
<static>true</static>
- <body:empty name="cam1_body">
- <sensor:camera name="cam1_sensor">
- <imageSize>640 480</imageSize>
+ <body:empty name="cam2_body">
+ <sensor:camera name="cam2_sensor">
+ <imageSize>800 600</imageSize>
<hfov>60</hfov>
<nearClip>0.1</nearClip>
<farClip>100</farClip>
- <!--<saveFrames>true</saveFrames>
+ <saveFrames>false</saveFrames>
<saveFramePath>frames</saveFramePath>
- -->
</sensor:camera>
</body:empty>
</model:physical>
- <model:physical name="cylinder1_model">
- <xyz>1 0 0.8</xyz>
+ <model:physical name="pioneer2dx_model1">
+ <xyz>-12 0 0.145</xyz>
<rpy>0.0 0.0 0.0</rpy>
- <body:box name="cylinder1_body">
- <geom:box name="cylinder1_geom">
- <size>0.8 0.8 0.8</size>
- <mass>0.1</mass>
+ <controller:differential_position2d name="controller1">
+ <leftJoint>left_wheel_hinge</leftJoint>
+ <rightJoint>right_wheel_hinge</rightJoint>
+ <wheelSeparation>0.34</wheelSeparation>
+ <wheelDiameter>0.15</wheelDiameter>
+ <torque>5</torque>
+ <interface:position name="position_iface_0"/>
+ </controller:differential_position2d>
- <mu1>0.1</mu1>
+ <model:physical name="laser">
+ <xyz>0.15 0 0.18</xyz>
- <visual>
- <mesh>unit_box</mesh>
- <size>0.8 0.8 0.8</size>
- <material>Gazebo/RustyBarrel</material>
- </visual>
- </geom:box>
- </body:box>
+ <attach>
+ <parentBody>chassis_body</parentBody>
+ <myBody>laser_body</myBody>
+ </attach>
+
+ <include embedded="true">
+ <xi:include href="models/sicklms200.model" />
+ </include>
+ </model:physical>
+
+ <!--
+ The include should be last within a model. All previous statements
+ will override those in the included file
+ -->
+ <include embedded="true">
+ <xi:include href="models/pioneer2dx.model" />
+ </include>
</model:physical>
+ <model:physical name="pioneer2dx_model2">
+ <xyz>12 0 0.145</xyz>
+ <rpy>0.0 0.0 180.0</rpy>
- <!--
- Include the complete model described in the .model file
- This assumes the root node is a <model:...>
- -->
- <!-- <include embedded="false">
- <xi:include href="pioneer2dx.model" />
- </include>
- -->
+ <model:physical name="laser2">
+ <xyz>0.15 0 0.18</xyz>
- <!-- White Directional light -->
- <model:renderable name="directional_white">
- <light>
- <type>directional</type>
- <direction>0 -0.6 -0.4</direction>
- <diffuseColor>1.0 1.0 1.0</diffuseColor>
- <specularColor>0.2 0.2 0.2</specularColor>
- <attenuation>1000 1.0 0.0 0</attenuation>
- </light>
- </model:renderable>
+ <attach>
+ <parentBody>chassis_body</parentBody>
+ <myBody>laser_body</myBody>
+ </attach>
+ <include embedded="true">
+ <xi:include href="models/sicklms200.model" />
+ </include>
+ </model:physical>
+ <!--
+ The include should be last within a model. All previous statements
+ will override those in the included file
+ -->
+ <include embedded="true">
+ <xi:include href="models/pioneer2dx.model" />
+ </include>
+ </model:physical>
+
+
+ <model:physical name="pioneer2dx_model3">
+ <xyz>0 5 0.145</xyz>
+ <rpy>0.0 0.0 -90.0</rpy>
+
+ <model:physical name="laser3">
+ <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>
+
+ <!--
+ The include should be last within a model. All previous statements
+ will override those in the included file
+ -->
+ <include embedded="true">
+ <xi:include href="models/pioneer2dx.model" />
+ </include>
+ </model:physical>
+
+ <model:physical name="pioneer2dx_model4">
+ <xyz>0 -5 0.145</xyz>
+ <rpy>0.0 0.0 90.0</rpy>
+
+ <model:physical name="laser4">
+ <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>
+
+ <!--
+ The include should be last within a model. All previous statements
+ will override those in the included file
+ -->
+ <include embedded="true">
+ <xi:include href="models/pioneer2dx.model" />
+ </include>
+ </model:physical>
+
+
+
</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://ad.doubleclick.net/clk;164216239;13503038;w?http://sf.net/marketplace
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
