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

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

Revision: 1396
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1396&view=rev
Author:   hsujohnhsu
Date:     2008-07-09 19:03:40 -0700 (Wed, 09 Jul 2008)

Log Message:
-----------
clean up some printf statements.

Modified Paths:
--------------
    pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp
    pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
    pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
    pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc

Modified: pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp	2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/libpr2GZ/src/pr2GZ.cpp	2008-07-10 02:03:40 UTC (rev 1396)
@@ -360,12 +360,6 @@
 
 PR2_ERROR_CODE PR2GZ::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
 {
-	printf("Called SetJointControlMode\n");
-	printf("id: %d, mode: %d\n", id, mode);
-	printf("IsHead: %d\n", IsHead(id));
-	printf("IsGripperLeft: %d\n", IsGripperLeft(id));
-	printf("IsGripperRight: %d\n", IsGripperRight(id));
-
    if(IsHead(id))
    {
      pr2HeadIface->Lock(1);
@@ -387,7 +381,6 @@
    else
    {
      pr2Iface->Lock(1);
-		 printf("id: %d, mode: %d\n", id, mode);
      pr2Iface->data->actuators[id].controlMode = mode;
      pr2Iface->Unlock();
    }

Modified: pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp	2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/libpr2HW/src/pr2HW.cpp	2008-07-10 02:03:40 UTC (rev 1396)
@@ -360,12 +360,6 @@
 
 PR2_ERROR_CODE PR2HW::SetJointControlMode(PR2_JOINT_ID id, PR2_JOINT_CONTROL_MODE mode)
 {
-	printf("Called SetJointControlMode\n");
-	printf("id: %d, mode: %d\n", id, mode);
-	printf("IsHead: %d\n", IsHead(id));
-	printf("IsGripperLeft: %d\n", IsGripperLeft(id));
-	printf("IsGripperRight: %d\n", IsGripperRight(id));
-
    if(IsHead(id))
    {
      pr2HeadIface->Lock(1);
@@ -387,7 +381,6 @@
    else
    {
      pr2Iface->Lock(1);
-		 printf("id: %d, mode: %d\n", id, mode);
      pr2Iface->data->actuators[id].controlMode = mode;
      pr2Iface->Unlock();
    }

Modified: pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h	2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/drivers/robot/pr2/pr2Core/include/pr2Core/pr2Misc.h	2008-07-10 02:03:40 UTC (rev 1396)
@@ -55,7 +55,6 @@
 
     inline bool IsGripperLeft(PR2_JOINT_ID id)
     {
-			printf("in IsGripperLeft:\nid: %d, Start: %d, end:%d\n",id,JointStart[PR2_LEFT_GRIPPER],JointEnd[PR2_LEFT_GRIPPER]);
        if (id >= JointStart[PR2_LEFT_GRIPPER] && id <= JointEnd[PR2_LEFT_GRIPPER])
           return true;
        return false;

Modified: pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc
===================================================================
--- pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc	2008-07-10 02:02:17 UTC (rev 1395)
+++ pkg/trunk/simulators/pr2MechCon/pr2MechCon.cc	2008-07-10 02:03:40 UTC (rev 1396)
@@ -28,7 +28,6 @@
 #include <gazebo/GazeboError.hh>
 #include <libpr2API/pr2API.h>
 
-#include <genericControllers/Controller.h>
 #include <pr2Controllers/ArmController.h>
 #include <pr2Controllers/HeadController.h>
 #include <pr2Controllers/SpineController.h>
@@ -146,9 +145,6 @@
 
     // keep count for full cloud
     int max_cloud_pts;
-
-    // clean up on interrupt
-    static void finalize(int);
 };
 
 void
@@ -310,7 +306,7 @@
 
 }
 
-void RosGazeboNode::finalize(int)
+void finalize(int)
 {
   fprintf(stderr,"Caught sig, clean-up and exit\n");
   sleep(1);
@@ -1108,6 +1104,7 @@
 
 void *nonRealtimeLoop(void *rgn)
 {
+  std::cout << "Started nonRT loop" << std::endl;
   while (1)
   {
     ((RosGazeboNode*)rgn)->Update();
@@ -1192,9 +1189,9 @@
   /*                                on termination...                                    */
   /*                                                                                     */
   /***************************************************************************************/
-  signal(SIGINT,  (&rgn.finalize));
-  signal(SIGQUIT, (&rgn.finalize));
-  signal(SIGTERM, (&rgn.finalize));
+  signal(SIGINT,  (&finalize));
+  signal(SIGQUIT, (&finalize));
+  signal(SIGTERM, (&finalize));
 
   // see if we can subscribe models needed
   if (rgn.AdvertiseSubscribeMessages() != 0)
@@ -1209,7 +1206,7 @@
   int rgnt = pthread_create(&threads[0],NULL, nonRealtimeLoop, (void *) (&rgn));
   if (rgnt)
   {
-    printf("Could not start ROSGazeboNode (code=%d)\n",rgnt);
+    printf("Could not start a separate thread for ROS Gazebo Node (code=%d)\n",rgnt);
     exit(-1);
   }
 
@@ -1223,6 +1220,8 @@
   {
 
     // Update Controllers
+    //   each controller will try to read new commands from shared memory with nonRT hooks,
+    //   and skip update if locked by nonRT loop.
     myArm.Update();
     myHead.Update();
     mySpine.Update();
@@ -1230,6 +1229,8 @@
     myLaserScanner.Update();
     myGripper.Update();
 
+    // TODO: Safety codes should go here...
+
     // Send updated controller commands to hardware
     // myPR2->hw.UpdateHW();
 


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