[Personalrobots-commit] SF.net SVN: personalrobots:[19331] pkg/trunk/pr2/life_test

[<wa...@us...>]

Revision: 19331
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19331&view=rev
Author:   wattsk
Date:     2009-07-21 21:50:50 +0000 (Tue, 21 Jul 2009)

Log Message:
-----------
Fixes to wrist test, test manager

Modified Paths:
--------------
    pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch
    pkg/trunk/pr2/life_test/arm_life_test/full_arm.launch
    pkg/trunk/pr2/life_test/arm_life_test/test_gui_cb_forearm.launch
    pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py
    pkg/trunk/pr2/life_test/scripts/impact_head_test.py
    pkg/trunk/pr2/life_test/scripts/wrist_test.py
    pkg/trunk/pr2/life_test/src/life_test/life_test.py
    pkg/trunk/pr2/life_test/wrist_test/test_gui.launch

Modified: pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch
===================================================================
--- pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/arm_life_test/controller_cartesian_pose_right_forearm.launch	2009-07-21 21:50:50 UTC (rev 19331)
@@ -2,12 +2,12 @@
 
 <!-- Cartesian wrench controller -->
   <param name="r_arm_cartesian_wrench_controller/root_name" type="string" value="torso_lift_link" />
-  <param name="r_arm_cartesian_wrench_controller/tip_name" type="string" value="r_forearm_roll_link" />
-<!-- Change above to forearm roll when fixed -->
+  <param name="r_arm_cartesian_wrench_controller/tip_name" type="string" value="r_elbow_flex_link" />
 
+
 <!-- Cartesian twist controller -->
   <param name="r_arm_cartesian_twist_controller/root_name" type="string" value="torso_lift_link" />
-  <param name="r_arm_cartesian_twist_controller/tip_name" type="string" value="r_forearm_roll_link" />
+  <param name="r_arm_cartesian_twist_controller/tip_name" type="string" value="r_elbow_flex_link" />
   <param name="r_arm_cartesian_twist_controller/output" type="string" value="r_arm_cartesian_wrench_controller" />
 
   <param name="r_arm_cartesian_twist_controller/ff_trans" value="0.0" />
@@ -26,7 +26,7 @@
 
 <!-- Cartesian pose controller -->
   <param name="r_arm_cartesian_pose_controller/root_name" type="string" value="torso_lift_link" />
-  <param name="r_arm_cartesian_pose_controller/tip_name" type="string" value="r_forearm_roll_link" />
+  <param name="r_arm_cartesian_pose_controller/tip_name" type="string" value="r_elbow_flex_link" />
   <param name="r_arm_cartesian_pose_controller/output" type="string" value="r_arm_cartesian_twist_controller" />
 
   <param name="r_arm_cartesian_pose_controller/p" value="20.0" />

Modified: pkg/trunk/pr2/life_test/arm_life_test/full_arm.launch
===================================================================
--- pkg/trunk/pr2/life_test/arm_life_test/full_arm.launch	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/arm_life_test/full_arm.launch	2009-07-21 21:50:50 UTC (rev 19331)
@@ -1,20 +1,7 @@
 <launch>
   <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find pr2_defs)/robots/r_arm.xacro.xml'" />
-  <!--param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find life_test)/arm_life_test/full_arm.xml'" /-->
 
   <include file="$(find life_test)/init.machine" />
   <include file="$(find life_test)/pr2_etherCAT.launch" />
 
-<!-- Arm Calibration -->
-  <node pkg="mechanism_bringup" type="calibrate.py"
-  	args="$(find life_test)/arm_life_test/full_arm_cal.xml" />
-
-<!-- spacenav -->
-  <!--node pkg="spacenav_node" type="spacenav_node" /-->
-
-<!-- Runtime Diagnostics Logging -->
-  <node pkg="rosrecord" type="rosrecord" args="-f /hwlog/arm_life_test_runtime_automatic /diagnostics" />
-
-<!-- Tests launched separately -->
-
 </launch>

Modified: pkg/trunk/pr2/life_test/arm_life_test/test_gui_cb_forearm.launch
===================================================================
--- pkg/trunk/pr2/life_test/arm_life_test/test_gui_cb_forearm.launch	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/arm_life_test/test_gui_cb_forearm.launch	2009-07-21 21:50:50 UTC (rev 19331)
@@ -12,4 +12,7 @@
   
   <include file="$(find life_test)/arm_life_test/run_random_poses_cb_forearm.launch" />
 
+  <node pkg="life_test" type="forearm_effort_controller.py"
+  	   args="r_forearm_roll_joint" />
+
 </launch>

Modified: pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/scripts/ethercat_test_monitor.py	2009-07-21 21:50:50 UTC (rev 19331)
@@ -60,8 +60,10 @@
     def __init__(self):
         rospy.init_node('test_monitor', anonymous = True)
 
-        csv_filename = rospy.myargv()[1] # CSV of device data
-        self.create_trans_monitors(csv_filename)
+        self._trans_monitors = []
+        if len(rospy.myargv()) > 1:
+            csv_filename = rospy.myargv()[1] # CSV of device data
+            self.create_trans_monitors(csv_filename)
 
         self._mutex = threading.Lock()
 
@@ -100,7 +102,7 @@
         
 
     def create_trans_monitors(self, csv_filename):
-        self._trans_monitors = []
+
         trans_csv = csv.reader(open(csv_filename, 'rb'))
         for row in trans_csv:
             actuator = row[0].lstrip().rstrip()
@@ -120,9 +122,12 @@
 
         status = TestStatus()
       
-        if rospy.get_time() - self._ethercat_update_time > 3 or rospy.get_time() - self._mech_update_time > 1:        
+        if rospy.get_time() - self._ethercat_update_time > 3:
             status.test_ok = TestStatus.TEST_STALE
             status.message = 'EtherCAT Master Stale'
+        elif rospy.get_time() - self._mech_update_time > 1:
+            status.message = 'Mechanism state stale'
+            status.test_ok = TestStatus.TEST_STALE
         elif self._ethercat_ok and self._transmissions_ok:
             status.test_ok = TestStatus.TEST_RUNNING
             status.message = 'OK'

Modified: pkg/trunk/pr2/life_test/scripts/impact_head_test.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/impact_head_test.py	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/scripts/impact_head_test.py	2009-07-21 21:50:50 UTC (rev 19331)
@@ -56,10 +56,9 @@
 import rospy
 from std_msgs.msg import *
 from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
 from time import sleep
 
-## Create XML code for controller on the fly
 def xml_for(controller, joint):
     return  '''\
 <controller name=\"%s\" type=\"JointEffortControllerNode\">\
@@ -77,13 +76,14 @@
         spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
         kill_controller = rospy.ServiceProxy('kill_controller', KillController)
     
-        resp = spawn_controller(xml_for(controller, joint))
-        if len(resp.ok) < 1 or not ord(resp.ok[0]):
+        resp = spawn_controller(xml_for(controller, joint),1)
+        if len(resp.ok) < 1 or not resp.ok[0] == 1:
             print "Failed to spawn effort controller"
+            sys.exit(100)
         else:
             print "Spawned controller %s successfully" % controller
 
-            pub = rospy.Publisher("/%s/command" % controller, Float64)
+            pub = rospy.Publisher("%s/command" % controller, Float64)
 
             try:
                 for i in range(0, 500):
@@ -99,7 +99,7 @@
             finally:
                 kill_controller(controller)
                 sleep(5)
-                sys.exit(0)
+                #sys.exit(0)
     
 if __name__ == '__main__':
     main()

Modified: pkg/trunk/pr2/life_test/scripts/wrist_test.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/wrist_test.py	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/scripts/wrist_test.py	2009-07-21 21:50:50 UTC (rev 19331)
@@ -52,16 +52,13 @@
 import rospy
 from std_msgs.msg import *
 from mechanism_control import mechanism
-from robot_srvs.srv import SpawnController, KillController
+from mechanism_msgs.srv import SpawnController, KillController
 from time import sleep
 
-spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
-kill_controller = rospy.ServiceProxy('kill_controller', KillController)
 
-pub_flex = rospy.Publisher("wrist_flex_effort/command", Float64)
-pub_grip = rospy.Publisher("grip_effort/command", Float64)
-pub_roll = rospy.Publisher("wrist_roll_effort/command", Float64)
 
+
+
 ## Create XML code for controller on the fly
 def xml_for_flex():
     return  "\
@@ -81,25 +78,29 @@
   <joint name=\"r_gripper_joint\" />\
 </controller>" 
 
-
-
-
 def main():
     rospy.init_node('wrist_test', anonymous=True)
     
     rospy.wait_for_service('spawn_controller')
+
+    spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+    kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+    pub_grip = rospy.Publisher("grip_effort/command", Float64)
+    pub_flex = rospy.Publisher("wrist_flex_effort/command", Float64)
+    pub_roll = rospy.Publisher("wrist_roll_effort/command", Float64)
     
-    resp = spawn_controller(xml_for_flex())
-    if len(resp.ok) < 1 or not ord(resp.ok[0]):
-        rospy.logerr("Failed to spawn effort controller")
+    resp = spawn_controller(xml_for_flex(), 1)
+    if len(resp.ok) < 1 or not resp.ok[0] == 1:
+        rospy.logerr("Failed to spawn effort controller, resp: %d" % resp.ok[0])
         print xml_for_flex()
         sys.exit(100)
     else:
         print "Spawned flex controller successfully"
 
 
-    resp = spawn_controller(xml_for_roll())
-    if len(resp.ok) < 1 or not ord(resp.ok[0]):
+    resp = spawn_controller(xml_for_roll(), 1)
+    if len(resp.ok) < 1 or not resp.ok[0] == 1:
         rospy.logerr("Failed to spawn effort controller roll")
         print xml_for_roll()
         sys.exit(101)
@@ -107,23 +108,24 @@
         print "Spawned flex controller successfully"
 
 
-    resp = spawn_controller(xml_for_grip())
-    if len(resp.ok) < 1 or not ord(resp.ok[0]):
+    resp = spawn_controller(xml_for_grip(), 1)
+    if len(resp.ok) < 1 or not resp.ok[0] == 1:
         rospy.logerr("Failed to spawn effort controller roll")
         print xml_for_grip()
         sys.exit(102)
     else:
         print "Spawned grip controller successfully"
 
-    effort_flex = rospy.get_param('flex_effort')
-    effort_roll = rospy.get_param('roll_effort')
-    
+    print 'Getting parameters'
+    effort_flex = float(rospy.get_param('flex_effort'))
+    effort_roll = float(rospy.get_param('roll_effort'))
     effort_grip = -100 
     
-    freq = rospy.get_param('cycle_rate')
+    freq = float(rospy.get_param('cycle_rate'))
     
     try:
         while not rospy.is_shutdown():
+            print 'Publishing commands'
             if random.randint(0, 1) == 1:
                 effort_flex = effort_flex * -1
 
@@ -134,7 +136,6 @@
             pub_flex.publish(Float64(effort_flex))
             pub_roll.publish(Float64(effort_roll))
 
-
             sleep(1.0 / freq)
 
     finally:

Modified: pkg/trunk/pr2/life_test/src/life_test/life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/life_test.py	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/src/life_test/life_test.py	2009-07-21 21:50:50 UTC (rev 19331)
@@ -411,6 +411,7 @@
         
     def stop_test(self, event = None):
         if self.is_launched():
+            print 'Shutting down test'
             self.on_halt_motors(None)
             self._test_launcher.shutdown()
             self._manager.test_stop(self._machine)
@@ -454,6 +455,7 @@
 
         # Remap
         launch += '<remap from="/diagnostics" to="%s" />' % local_diag_topic
+         
         # Init machine
         launch += '<machine name="test_host_root" user="root" address="%s" ' % self._machine
         launch += 'ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>'

Modified: pkg/trunk/pr2/life_test/wrist_test/test_gui.launch
===================================================================
--- pkg/trunk/pr2/life_test/wrist_test/test_gui.launch	2009-07-21 21:41:32 UTC (rev 19330)
+++ pkg/trunk/pr2/life_test/wrist_test/test_gui.launch	2009-07-21 21:50:50 UTC (rev 19331)
@@ -5,10 +5,11 @@
 
 
   <!-- Calibration -->
+  <!--
   <node pkg="mechanism_bringup" type="calibrate.py"
         args="$(find life_test)/wrist_test/wrist_cal.xml"
         output="screen" />
-
+-->	
   <node pkg="life_test" type="wrist_test.py" />
 
   <node pkg="life_test" type="ethercat_test_monitor.py" />


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