[Playerstage-commit] SF.net SVN: playerstage:[7017] code/gazebo/trunk/server/controllers/ position2d/differential/Differential_Position2d.cc

[natepak]

Revision: 7017
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7017&view=rev
Author:   natepak
Date:     2008-09-09 23:48:33 +0000 (Tue, 09 Sep 2008)

Log Message:
-----------
Applied patch from Sandy to put the Differential_Position2d update cycle inside 
on Lock

Modified Paths:
--------------
    
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc

Modified: 
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
===================================================================
--- 
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
     2008-09-09 22:04:34 UTC (rev 7016)
+++ 
code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc
     2008-09-09 23:48:33 UTC (rev 7017)
@@ -158,6 +158,8 @@
   double dr, da;
   double stepTime;
 
+  this->myIface->Lock(1);
+
   this->GetPositionCmd();
 
   wd = **(this->wheelDiamP);
@@ -206,6 +208,8 @@
   }*/
 
   this->PutPositionData();
+
+  this->myIface->Unlock();
 }
 
 
////////////////////////////////////////////////////////////////////////////////
@@ -221,40 +225,30 @@
 {
   double vr, va;
 
-  if (this->myIface->Lock(1))
-  {
+  vr = this->myIface->data->cmdVelocity.pos.x;
+  va = this->myIface->data->cmdVelocity.yaw;
 
-    vr = this->myIface->data->cmdVelocity.pos.x;
-    va = this->myIface->data->cmdVelocity.yaw;
+  this->enableMotors = this->myIface->data->cmdEnableMotors > 0;
 
-    this->enableMotors = this->myIface->data->cmdEnableMotors > 0;
+  this->wheelSpeed[LEFT] = vr + va * **(this->wheelSepP) / 2;
+  this->wheelSpeed[RIGHT] = vr - va * **(this->wheelSepP) / 2;
 
-    this->wheelSpeed[LEFT] = vr + va * **(this->wheelSepP) / 2;
-    this->wheelSpeed[RIGHT] = vr - va * **(this->wheelSepP) / 2;
-
-    this->myIface->Unlock();
-  }
 }
 
 //////////////////////////////////////////////////////////////////////////////
 // Update the data in the interface
 void Differential_Position2d::PutPositionData()
 {
-  if (this->myIface->Lock(1))
-  {
-    // TODO: Data timestamp
-    this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
+  // TODO: Data timestamp
+  this->myIface->data->head.time = Simulator::Instance()->GetSimTime();
 
-    this->myIface->data->pose.pos.x = this->odomPose[0];
-    this->myIface->data->pose.pos.y = this->odomPose[1];
-    this->myIface->data->pose.yaw = NORMALIZE(this->odomPose[2]);
+  this->myIface->data->pose.pos.x = this->odomPose[0];
+  this->myIface->data->pose.pos.y = this->odomPose[1];
+  this->myIface->data->pose.yaw = NORMALIZE(this->odomPose[2]);
 
-    this->myIface->data->velocity.pos.x = this->odomVel[0];
-    this->myIface->data->velocity.yaw = this->odomVel[2];
+  this->myIface->data->velocity.pos.x = this->odomVel[0];
+  this->myIface->data->velocity.yaw = this->odomVel[2];
 
-    // TODO
-    this->myIface->data->stall = 0;
-
-    this->myIface->Unlock();
-  }
+  // TODO
+  this->myIface->data->stall = 0;
 }


This was sent by the SourceForge.net collaborative development platform, the 
world's largest Open Source development site.

-------------------------------------------------------------------------
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-commit mailing list
Playerstage-commit@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/playerstage-commit