Here is a very simple patch to enable SetOdometry support in the differential
position 2d controller. I needed to change the UpdateChild to use one large
lock instead of two smaller locks because it may be possible for a client to
set the odometry in between the locks and the change would then be lost. It
does not handle current velocity (prevUpdateTime) since I do not know what is
the expected behavior. This may need to be changed. The issue is that a
client could set the odometry just before gazebo is about to update the
model/controller. In this case, the odometry may receive too great of a change
(especially if the robot/model is maintaining a large velocity). Since I do
not have the ability to test this on a real pioneer, I am unsure what the
correct behavior should be. An equivalent fix may be needed in other
controllers but I have not looked.
Index: server/controllers/position2d/differential/Differential_Position2d.cc
===================================================================
--- server/controllers/position2d/differential/Differential_Position2d.cc
(revision 7002)
+++ server/controllers/position2d/differential/Differential_Position2d.cc
(working copy)
@@ -158,6 +158,7 @@
double dr, da;
double stepTime;
+ this->myIface->Lock(1);
this->GetPositionCmd();
wd = **(this->wheelDiamP);
@@ -206,6 +207,7 @@
}*/
this->PutPositionData();
+ this->myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
@@ -221,9 +223,13 @@
{
double vr, va;
- if (this->myIface->Lock(1))
- {
+ //if (this->myIface->Lock(1))
+ //{
+ this->odomPose[0] = this->myIface->data->pose.pos.x;
+ this->odomPose[1] = this->myIface->data->pose.pos.y;
+ this->odomPose[2] = this->myIface->data->pose.yaw;
+
vr = this->myIface->data->cmdVelocity.pos.x;
va = this->myIface->data->cmdVelocity.yaw;
@@ -232,16 +238,16 @@
this->wheelSpeed[LEFT] = vr + va * **(this->wheelSepP) / 2;
this->wheelSpeed[RIGHT] = vr - va * **(this->wheelSepP) / 2;
- this->myIface->Unlock();
- }
+ //this->myIface->Unlock();
+ //}
}
//////////////////////////////////////////////////////////////////////////////
// Update the data in the interface
void Differential_Position2d::PutPositionData()
{
- if (this->myIface->Lock(1))
- {
+ //if (this->myIface->Lock(1))
+ //{
// TODO: Data timestamp
this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
@@ -255,6 +261,6 @@
// TODO
this->myIface->data->stall = 0;
- this->myIface->Unlock();
- }
+ //this->myIface->Unlock();
+ //}
}
Sandy
--
[EMAIL PROTECTED]
www.sanfordfreedman.com
"I have yet to see any problem, however complicated, which, when you looked at
it in the right way, did not become still more complicated."
- Poul Anderson
-------------------------------------------------------------------------
This SF.Net email is sponsored by the Moblin Your Move Developer's challenge
Build the coolest Linux based applications with Moblin SDK & win great prizes
Grand prize is a trip for two to an Open Source event anywhere in the world
http://moblin-contest.org/redirect.php?banner_id=100&url=/
_______________________________________________
Playerstage-gazebo mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-gazebo
