Revision: 6683
http://playerstage.svn.sourceforge.net/playerstage/?rev=6683&view=rev
Author: natepak
Date: 2008-06-25 12:12:30 -0700 (Wed, 25 Jun 2008)
Log Message:
-----------
Incorporated Differentail steering patch from Jerome Berger
Modified Paths:
--------------
code/gazebo/trunk/examples/player/position2d/position2d.cc
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/player/LaserInterface.cc
code/gazebo/trunk/player/gazebo.cfg
code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh
code/gazebo/trunk/worlds/pioneer2dx.world
Modified: code/gazebo/trunk/examples/player/position2d/position2d.cc
===================================================================
--- code/gazebo/trunk/examples/player/position2d/position2d.cc 2008-06-25
17:00:34 UTC (rev 6682)
+++ code/gazebo/trunk/examples/player/position2d/position2d.cc 2008-06-25
19:12:30 UTC (rev 6683)
@@ -14,7 +14,7 @@
PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM);
// Subscribe to the simulation proxy
- Position2dProxy pp(&robot, 0);
+ Position2dProxy pp(&robot, 1);
// Print out some stuff
std::cout << robot << std::endl;
@@ -26,7 +26,7 @@
// This blocks until new data comes
robot.Read();
- pp.SetSpeed(0.2, 0.05);
+ pp.SetSpeed(0.0, 0.05);
}
}
catch (PlayerCc::PlayerError e)
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2008-06-25 17:00:34 UTC (rev
6682)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2008-06-25 19:12:30 UTC (rev
6683)
@@ -793,6 +793,9 @@
/// Angular resolution
public: double res_angle;
+ /// Range resolution
+ public: double res_range;
+
/// Max range value
public: double max_range;
Modified: code/gazebo/trunk/player/LaserInterface.cc
===================================================================
--- code/gazebo/trunk/player/LaserInterface.cc 2008-06-25 17:00:34 UTC (rev
6682)
+++ code/gazebo/trunk/player/LaserInterface.cc 2008-06-25 19:12:30 UTC (rev
6683)
@@ -115,6 +115,7 @@
plc.max_angle = this->iface->data->max_angle;
plc.max_range = this->iface->data->max_range;
plc.resolution = this->iface->data->res_angle;
+ plc.range_res = this->iface->data->res_range;
plc.intensity = intensity;
this->driver->Publish(this->device_addr, respQueue,
@@ -192,12 +193,12 @@
//printf("range res = %f %f\n", rangeRes, this->iface->data->max_range);
+ double oldCount = this->data.ranges_count;
+
this->data.min_angle = this->iface->data->min_angle;
this->data.max_angle = this->iface->data->max_angle;
+ this->data.resolution = angleRes;
this->data.max_range = this->iface->data->max_range;
- this->data.resolution = angleRes;
-
- double oldCount = this->data.ranges_count;
this->data.ranges_count = this->data.intensity_count =
this->iface->data->range_count;
this->data.id = this->scanId++;
@@ -216,10 +217,13 @@
this->data.intensity[i] = (uint8_t) (int)
this->iface->data->intensity[i];
}
- this->driver->Publish( this->device_addr,
- PLAYER_MSGTYPE_DATA,
- PLAYER_LASER_DATA_SCAN,
- (void*)&this->data, sizeof(this->data),
&this->datatime );
+ if (this->data.ranges_count > 0)
+ {
+ this->driver->Publish( this->device_addr,
+ PLAYER_MSGTYPE_DATA,
+ PLAYER_LASER_DATA_SCAN,
+ (void*)&this->data, sizeof(this->data), &this->datatime );
+ }
}
this->iface->Unlock();
Modified: code/gazebo/trunk/player/gazebo.cfg
===================================================================
--- code/gazebo/trunk/player/gazebo.cfg 2008-06-25 17:00:34 UTC (rev 6682)
+++ code/gazebo/trunk/player/gazebo.cfg 2008-06-25 19:12:30 UTC (rev 6683)
@@ -27,6 +27,23 @@
driver
(
+ name "vfh"
+ provides ["position2d:1"]
+ requires ["position2d:0" "laser:0"]
+ cell_size 0.1
+ window_diameter 61
+ sector_angle 1
+ safety_dist_0ms 0.2
+ safety_dist_1ms 0.4
+ max_speed 0.3
+ max_turnrate_0ms 75
+ max_turnrate_1ms 50
+ weight_desired_dir 5.0
+ weight_current_dir 3.0
+)
+
+driver
+(
name "gazebo"
provides ["fiducial:0"]
gz_id "fiducial_iface_0"
Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
===================================================================
--- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
2008-06-25 17:00:34 UTC (rev 6682)
+++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc
2008-06-25 19:12:30 UTC (rev 6683)
@@ -119,8 +119,6 @@
// Put stereo data to the interface
void Stereo_Camera::PutStereoData()
{
-
- printf("put stereo data\n");
StereoCameraData *stereo_data = this->stereoIface->data;
//const unsigned char *rgb_src = NULL;
//unsigned char *rgb_dst = NULL;
Modified:
code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
===================================================================
--- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
2008-06-25 17:00:34 UTC (rev 6682)
+++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc
2008-06-25 19:12:30 UTC (rev 6683)
@@ -147,7 +147,6 @@
int rayCount = this->myParent->GetRayCount();
int rangeCount = this->myParent->GetRangeCount();
-
if (this->laserIface->Lock(1))
{
// Data timestamp
@@ -157,6 +156,7 @@
this->laserIface->data->min_angle = minAngle;
this->laserIface->data->max_angle = maxAngle;
this->laserIface->data->res_angle = (maxAngle - minAngle) / (rangeCount -
1);
+ this->laserIface->data->res_range = 0.1;
this->laserIface->data->max_range = maxRange;
this->laserIface->data->range_count = rangeCount;
Modified:
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
===================================================================
---
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
2008-06-25 17:00:34 UTC (rev 6682)
+++
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
2008-06-25 19:12:30 UTC (rev 6683)
@@ -57,6 +57,8 @@
this->wheelSpeed[RIGHT] = 0;
this->wheelSpeed[LEFT] = 0;
+
+ this->prevUpdateTime = Simulator::Instance()->GetSimTime();
}
////////////////////////////////////////////////////////////////////////////////
@@ -90,7 +92,6 @@
if (!this->joints[RIGHT])
gzthrow("The controller couldn't get right hinge joint");
-
}
////////////////////////////////////////////////////////////////////////////////
@@ -148,14 +149,16 @@
ws = this->wheelSep;
- stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
+ //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
+ stepTime = Simulator::Instance()->GetSimTime() - this->prevUpdateTime;
+ this->prevUpdateTime = Simulator::Instance()->GetSimTime();
// Distance travelled by front wheels
d1 = stepTime * wd / 2 * this->joints[LEFT]->GetAngleRate();
d2 = stepTime * wd / 2 * this->joints[RIGHT]->GetAngleRate();
dr = (d1 + d2) / 2;
- da = (d2 - d1) / ws;
+ da = (d1 - d2) / ws;
// Compute odometric pose
this->odomPose[0] += dr * cos( this->odomPose[2] );
Modified:
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh
===================================================================
---
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh
2008-06-25 17:00:34 UTC (rev 6682)
+++
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh
2008-06-25 19:12:30 UTC (rev 6683)
@@ -113,6 +113,9 @@
/// Speeds of the wheels
private: float wheelSpeed[2];
+ // Simulation time of the last update
+ private: double prevUpdateTime;
+
/// True = enable motors
private: bool enableMotors;
Modified: code/gazebo/trunk/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-25 17:00:34 UTC (rev
6682)
+++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-25 19:12:30 UTC (rev
6683)
@@ -62,20 +62,22 @@
</body:plane>
</model:physical>
- <model:physical name="cylinder1_model">
- <xyz>1 -1.5 0.5</xyz>
+ <model:physical name="sphere1_model">
+ <xyz>2.15 -1.68 .7</xyz>
<rpy>0.0 0.0 0.0</rpy>
+ <static>true</static>
- <body:cylinder name="cylinder1_body">
- <geom:cylinder name="cylinder1_geom">
- <size>0.5 1</size>
- <mass>1.0</mass>
+ <body:sphere name="sphere1_body">
+ <geom:sphere name="sphere1_geom">
+ <size>0.1</size>
+
<visual>
- <mesh>unit_cylinder</mesh>
- <material>Gazebo/RustyBarrel</material>
+ <scale>0.1 0.1 0.1</scale>
+ <mesh>unit_sphere</mesh>
+ <material>Gazebo/Rocky</material>
</visual>
- </geom:cylinder>
- </body:cylinder>
+ </geom:sphere>
+ </body:sphere>
</model:physical>
@@ -95,7 +97,7 @@
<controller:differential_position2d name="controller1">
<leftJoint>left_wheel_hinge</leftJoint>
<rightJoint>right_wheel_hinge</rightJoint>
- <wheelSeparation>0.34</wheelSeparation>
+ <wheelSeparation>0.39</wheelSeparation>
<wheelDiameter>0.15</wheelDiameter>
<torque>5</torque>
<interface:position name="position_iface_0"/>
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://sourceforge.net/services/buy/index.php
_______________________________________________
Playerstage-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
