[Personalrobots-commit] SF.net SVN: personalrobots: [978] pkg/trunk

[<hsu...@us...>]

Revision: 978
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=978&view=rev
Author:   hsujohnhsu
Date:     2008-06-25 12:03:01 -0700 (Wed, 25 Jun 2008)

Log Message:
-----------
added some high level passthrough api's for the gripper and time.

Modified Paths:
--------------
    pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
    pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
    pkg/trunk/simulators/rosgazebo/rosgazebo.cc

Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h	2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h	2008-06-25 19:03:01 UTC (rev 978)
@@ -614,6 +614,21 @@
          */
       public: PR2_ERROR_CODE StopRobot();
 
+
+      public: PR2_ERROR_CODE GetLeftGripperCmd(double *gap,double *force);
+
+      public: PR2_ERROR_CODE GetLeftGripperActual(double *gap,double *force);
+
+      public: PR2_ERROR_CODE GetRightGripperCmd(double *gap,double *force);
+
+      public: PR2_ERROR_CODE GetRightGripperActual(double *gap,double *force);
+
+         /*! \fn
+           \brief - returns current time from simulator
+         */
+      public: PR2_ERROR_CODE GetTime(double *time);
+
+
       protected: PR2_CONTROL_MODE armControlMode[2];
       protected: PR2_CONTROL_MODE baseControlMode;
       protected: PR2Arm myArm; 

Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp	2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp	2008-06-25 19:03:01 UTC (rev 978)
@@ -804,5 +804,33 @@
 };
 
 
+PR2_ERROR_CODE PR2Robot::GetLeftGripperCmd(double *gap,double *force)
+{
+   hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+   return PR2_ALL_OK;
+}
 
+PR2_ERROR_CODE PR2Robot::GetLeftGripperActual(double *gap,double *force)
+{
+   hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_LEFT_GRIPPER,gap,force);
+   return PR2_ALL_OK;
+}
 
+PR2_ERROR_CODE PR2Robot::GetRightGripperCmd(double *gap,double *force)
+{
+   hw.GetGripperCmd((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+   return PR2_ALL_OK;
+}
+
+PR2_ERROR_CODE PR2Robot::GetRightGripperActual(double *gap,double *force)
+{
+   hw.GetGripperActual((PR2_MODEL_ID)PR2::PR2_RIGHT_GRIPPER,gap,force);
+   return PR2_ALL_OK;
+}
+
+
+PR2_ERROR_CODE PR2Robot::GetTime(double *time)
+{
+   return hw.GetSimTime(time);
+}
+

Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc	2008-06-25 19:01:17 UTC (rev 977)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc	2008-06-25 19:03:01 UTC (rev 978)
@@ -206,7 +206,7 @@
 
   // smooth out the commands by time decay
   // with w1,w2=1, this means equal weighting for new command every second
-  this->myPR2->hw.GetSimTime(&(this->simTime));
+  this->myPR2->GetTime(&(this->simTime));
   dt = simTime - lastTime;
 
   // smooth if dt is larger than zero
@@ -289,8 +289,8 @@
   this->myPR2->EnableGripperLeft();
   this->myPR2->EnableGripperRight();
 
-  this->myPR2->hw.GetSimTime(&(this->lastTime));
-  this->myPR2->hw.GetSimTime(&(this->simTime));
+  this->myPR2->GetTime(&(this->lastTime));
+  this->myPR2->GetTime(&(this->simTime));
 
   // set torques for driving the robot wheels
   // this->myPR2->hw.SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
@@ -441,7 +441,7 @@
   }
 
 
-  this->myPR2->hw.GetSimTime(&(this->simTime));
+  this->myPR2->GetTime(&(this->simTime));
 
   /***************************************************************/
   /*                                                             */
@@ -569,7 +569,7 @@
 	simPitchOffset = -M_PI / 8.0;
 	simPitchAngle = simPitchOffset + simPitchAmp * sin(this->simTime * simPitchTimeScale);
 	simPitchRate  =  simPitchAmp * simPitchTimeScale * cos(this->simTime * simPitchTimeScale); // TODO: check rate correctness
-  this->myPR2->hw.GetSimTime(&this->simTime);
+  this->myPR2->GetTime(&this->simTime);
 	//std::cout << "sim time: " << this->simTime << std::endl;
 	//std::cout << "ang: " << simPitchAngle*180.0/M_PI << "rate: " << simPitchRate*180.0/M_PI << std::endl;
 	this->myPR2->hw.SetJointTorque(PR2::HEAD_LASER_PITCH , 1000.0);
@@ -610,7 +610,7 @@
   larm.wristPitchAngle   = position;
   this->myPR2->hw.GetJointPositionActual(PR2::ARM_L_WRIST_ROLL,     &position, &velocity);
   larm.wristRollAngle    = position;
-  this->myPR2->hw.GetGripperActual  (PR2::PR2_LEFT_GRIPPER,      &position, &velocity);
+  this->myPR2->GetLeftGripperActual  (                              &position, &velocity);
   larm.gripperForceCmd   = velocity;
   larm.gripperGapCmd     = position;
   publish("left_pr2arm_pos", larm);
@@ -630,7 +630,7 @@
   rarm.wristPitchAngle   = position;
   this->myPR2->hw.GetJointPositionActual(PR2::ARM_R_WRIST_ROLL,     &position, &velocity);
   rarm.wristRollAngle    = position;
-  this->myPR2->hw.GetGripperActual  (PR2::PR2_RIGHT_GRIPPER,     &position, &velocity);
+  this->myPR2->GetRightGripperActual  (                             &position, &velocity);
   rarm.gripperForceCmd   = velocity;
   rarm.gripperGapCmd     = position;
   publish("right_pr2arm_pos", rarm);


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