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

SourceForge Headquarters 1320 Columbia Street Suite 310 San Diego, CA 92101 +1 (858) 422-6466

Revision: 16747
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=16747&view=rev
Author:   wattsk
Date:     2009-06-04 23:51:12 +0000 (Thu, 04 Jun 2009)

Log Message:
-----------
Life test fixes and upgrades for wrist, arm and elbow

Modified Paths:
--------------
    pkg/trunk/pr2/life_test/arm_life_test/arm_defs.xml
    pkg/trunk/pr2/life_test/arm_life_test/full_arm.xml
    pkg/trunk/pr2/life_test/arm_life_test/random_poses_right_cb.py
    pkg/trunk/pr2/life_test/scripts/elbow_flex_life_test.py
    pkg/trunk/pr2/life_test/scripts/wrist_test.py
    pkg/trunk/pr2/life_test/simple_test/fake_test.py
    pkg/trunk/pr2/life_test/src/life_test/life_test.py
    pkg/trunk/pr2/life_test/src/life_test/ui.py
    pkg/trunk/pr2/life_test/tests.xml

Added Paths:
-----------
    pkg/trunk/pr2/life_test/wrist_test/test_gui_bar.launch

Modified: pkg/trunk/pr2/life_test/arm_life_test/arm_defs.xml
===================================================================

--- pkg/trunk/pr2/life_test/arm_life_test/arm_defs.xml	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/arm_life_test/arm_defs.xml	2009-06-04 23:51:12 UTC (rev 16747)
@@ -300,7 +300,7 @@
       <anchor xyz="0 0 0" />
 
       <limit min="-2.3" max="0.1" effort="30" velocity="${VELOCITY_LIMIT_SCALE*5.5}"
-             k_position="100" k_velocity="3"
+             k_position="60" k_velocity="8"
              safety_length_min="0.25" safety_length_max="0.1" />
       <calibration reference_position="${-1.1606 + cal_r_elbow_flex_flag}" values="1.5 -1" />
 

Modified: pkg/trunk/pr2/life_test/arm_life_test/full_arm.xml
===================================================================
--- pkg/trunk/pr2/life_test/arm_life_test/full_arm.xml	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/arm_life_test/full_arm.xml	2009-06-04 23:51:12 UTC (rev 16747)
@@ -1,10 +1,12 @@
 <?xml version="1.0"?>
 <robot name="test_arm">
 
+  <include filename="$(find pr2_defs)/calibration/default_cal.xml" />
   <include filename="$(find pr2_defs)/defs/arm_defs.xml" />
   <include filename="$(find pr2_defs)/defs/gripper_defs.xml" />
   <include filename="$(find pr2_defs)/defs/body_defs.xml" />
-  <include filename="$(find arm_life_test)/groups_arm.xml" />
+  <include filename="$(find life_test)/arm_life_test/groups_arm.xml" />
+  
 
 <pr2_torso name="base" parent="world"> 
     <origin xyz="0 0 0" rpy="0 0 0" />

Modified: pkg/trunk/pr2/life_test/arm_life_test/random_poses_right_cb.py
===================================================================
--- pkg/trunk/pr2/life_test/arm_life_test/random_poses_right_cb.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/arm_life_test/random_poses_right_cb.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -34,8 +34,7 @@
 from std_msgs.msg import *
 from robot_msgs.msg import *
 
-pub = rospy.Publisher('/r_arm_cartesian_pose_controller/command', PoseStamped)
-#pub = rospy.Publisher('/cartesian_trajectory_right/command', PoseStamped)
+pub = rospy.Publisher('r_arm_cartesian_pose_controller/command', PoseStamped)
 
 
 def p(x, y, z, rx, ry, rz, w):

Modified: pkg/trunk/pr2/life_test/scripts/elbow_flex_life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/elbow_flex_life_test.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/scripts/elbow_flex_life_test.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -88,9 +88,9 @@
         while not rospy.is_shutdown():
             if roll:
                 fore_roll.publish(Float64(1.0))
-            arm_pos.publish(Float64(0.0))
+            arm_pos.publish(Float64(-0.05))
             sleep(0.5)
-            arm_pos.publish(Float64(-2.05))
+            arm_pos.publish(Float64(-2.00))
             sleep(0.5)
     except Exception, e:
         print "Caught exception!"

Modified: pkg/trunk/pr2/life_test/scripts/wrist_test.py
===================================================================
--- pkg/trunk/pr2/life_test/scripts/wrist_test.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/scripts/wrist_test.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -114,9 +114,16 @@
     else:
         print "Spawned grip controller successfully"
 
+    if fixed_to_bar:
+        effort_flex = 1000
+        effort_roll = 1000
+    else:
+        effort_flex = 6
+        effort_roll = 3 # Turn down effort roll to make wrist flex up and down
+
+
+
     effort_grip = -100 
-    effort_flex = 4
-    effort_roll = 4
         
     try:
         while not rospy.is_shutdown():
@@ -128,8 +135,13 @@
                 
             pub_grip.publish(Float64(effort_grip))
             pub_flex.publish(Float64(effort_flex))
-            pub_roll.publish(Float64(effort_roll))
 
+            if fixed_to_bar and random.randint(0, 5) == 1:
+                pub_roll.publish(Float64(0))
+            else:
+                pub_roll.publish(Float64(effort_roll))
+
+
             sleep(0.3)
 
     finally:

Modified: pkg/trunk/pr2/life_test/simple_test/fake_test.py
===================================================================
--- pkg/trunk/pr2/life_test/simple_test/fake_test.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/simple_test/fake_test.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -57,7 +57,7 @@
         self._diag_timer = wx.Timer(self, 1)
         self.Bind(wx.EVT_TIMER, self.on_timer, self._diag_timer)
         self._last_publish = rospy.get_time()
-        self._diag_timer.Start(500, True)
+        self._diag_timer.Start(250, True)
         
         # Load XRC
         xrc_path = os.path.join(roslib.packages.get_pkg_dir('life_test'), 'xrc/gui.xrc')

Modified: pkg/trunk/pr2/life_test/src/life_test/life_test.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/life_test.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/src/life_test/life_test.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -86,6 +86,7 @@
         self._test_name = test._name
 
         self._log = {}
+        self._last_log_time = self._last_update_time
 
         csv_name = strftime("%m%d%Y_%H%M%S", localtime(self._start_time)) + '_' + str(self._serial) + '_' + self._test_name + '.csv'
         csv_name = csv_name.replace(' ', '_').replace('/', '__')
@@ -169,9 +170,10 @@
         self._was_launched = launched
         self._last_update_time = rospy.get_time()
 
-        if alert or note != '':
+        if alert or note != '' or  (running and self._last_log_time - rospy.get_time() > 7200):
             self.write_csv_row(self._last_update_time, state, msg, note)
             self._log[rospy.get_time()] = msg + ' ' + note
+            self._last_log_time = self._last_update_time
 
         return alert, msg
 
@@ -284,17 +286,18 @@
         self._test_complete = False
 
         # Timeout for etherCAT diagnostics
-        self.timer = wx.Timer(self, 1)
+        self.timer = wx.Timer(self)
         self.Bind(wx.EVT_TIMER, self.on_timer, self.timer)
         self.last_message_time = rospy.get_time()
-        self.timeout_interval = 2.0
+        self.timeout_interval = 5.0
         self._is_stale = True
 
         # Timer for invent logging
-        self.invent_timer = wx.Timer(self, 2)
+        self.invent_timer = wx.Timer(self)
         self.Bind(wx.EVT_TIMER, self.on_invent_timer, self.invent_timer)
         self._last_invent_time = rospy.get_time()
         self.invent_timeout = 600
+        self.invent_timer.Start(self.invent_timeout * 500)
         self._is_invent_stale = True
         self._invent_note_id = None
  
@@ -311,6 +314,10 @@
         # Somehow calling log function in destructor
         print 'Stopping launches'
         self.stop_test()
+
+        if self._diag_sub:
+            print 'Unregistering from diagnostics'
+            self._diag_sub.unregister()
         
     def is_launched(self):
         return self._test_launcher is not None
@@ -382,7 +389,8 @@
             self._current_log[rospy.get_time()] = msg
             self._manager.log_test_entry(self._test._name, self._machine, msg)
             
-            wx.CallAfter(self.display_logs)
+            #self._log_ctrl.AppendText(
+            self.display_logs()
             self.notify_operator(alert, msg)
 
 
@@ -414,14 +422,14 @@
         if rospy.get_time() - self._last_invent_time > self.invent_timeout and self.is_launched():
             self.update_invent()
         
-        self.invent_timer.Start(self.invent_timeout * 500, True)
+        #self.invent_timer.Start(self.invent_timeout * 500, True)
 
 
     def update_invent(self):
-        # rospy.logerr('Updating invent')
+        rospy.logerr('Updating invent')
 
         # Reset timer, last time
-        self.invent_timer.Start(self.invent_timeout * 500, True)
+        #self.invent_timer.Start(self.invent_timeout * 500, True)
         self._last_invent_time = rospy.get_time()
 
         # Don't log anything if we haven't launched
@@ -465,7 +473,7 @@
                                                 
         
     def start_timer(self):
-        self.timer.Start(1000, True)
+        self.timer.Start(1000 * self.timeout_interval, True)
         
     def on_timer(self, event):
         self._mutex.acquire()
@@ -481,11 +489,11 @@
             self.stop_if_done()
             self.update_controls(3)
         else:
-            self.start_timer()
+            #self.start_timer()
             self._is_stale = False
             
         self._mutex.release()
-        self.Refresh()
+        #self.Refresh()
 
     def update_controls(self, level = 3, msg = 'None'):
         # Assumes it has the lock
@@ -560,10 +568,12 @@
             self._test_launcher.stop()
             self._manager.test_stop(self._machine)
             self.log('Stopping test launch')
+            self._test_launcher = None            
 
+        if self._diag_sub:
+            self._diag_sub.unregister()
         self._diag_sub = None
 
-        self._test_launcher = None
         self._is_running = False
         
         if event is not None: # In GUI thread
@@ -575,25 +585,23 @@
         self._current_log[rospy.get_time()] = msg
         self._manager.log_test_entry(self._test._name, self._machine, msg)
 
-        wx.CallAfter(self.display_logs)
+        #wx.CallAfter(self.display_logs)
 
     def display_logs(self):
-        # Make them bottom of log visible...
-        try:
-            self._mutex.acquire()
-        except:
-            return
+        #try:
+        #    self._mutex.acquire()
+        #except:
+        #    rospy.logerr('Error attempting to acquire mutex!')
+        #    return
       
         self._log_ctrl.AppendText(self.print_cur_log())
-        self._log_ctrl.Refresh()
-        self._log_ctrl.Update()
+        #self._log_ctrl.Refresh()
+        #self._log_ctrl.Update()
         self._current_log = {}
-        self._mutex.release()
+        #self._mutex.release()
 
     def diag_callback(self, message):
-        #print 'Acquiring mutex for diagnostic callback'
         self._mutex.acquire()
-        #print 'Diagnostic mutex acquired'
         self._diags.append(message)
         self._mutex.release()
         wx.CallAfter(self.new_msg)
@@ -623,12 +631,14 @@
             rospy.logerr('Caught exception processing diagnostic msg.\nEx: %s' % traceback.format_exc())
           
         self._mutex.release()
-        self.Refresh()
+        #self.Refresh()
     
     
     def make_launch_script(self):
         launch = '<launch>\n'
         launch += '<group ns="%s">' % self._machine
+
+        # Remap
         launch += '<remap from="/diagnostics" to="/%s/diagnostics" />' % self._machine
         # Init machine
         launch += '<machine name="test_host_root" user="root" address="%s" ' % self._machine

Modified: pkg/trunk/pr2/life_test/src/life_test/ui.py
===================================================================
--- pkg/trunk/pr2/life_test/src/life_test/ui.py	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/src/life_test/ui.py	2009-06-04 23:51:12 UTC (rev 16747)
@@ -48,6 +48,8 @@
 import glob
 import wx
 import time
+import traceback
+import threading
 from wx import xrc
 from wx import html
 
@@ -58,14 +60,10 @@
 from cStringIO import StringIO
 import struct
 
-import traceback
-
 from invent_client.invent_client import Invent
 
 from life_test import *
 
-import threading
-
 class TestManagerFrame(wx.Frame):
     def __init__(self, parent):
         wx.Frame.__init__(self, parent, wx.ID_ANY, "Test Manager")
@@ -139,6 +137,7 @@
         if self._tab_ctrl.DeletePage(idx + 1):
             del self._test_monitors[serial]
             del self._active_serials[idx]
+            #print 'Active serials after deletion', self._active_serials
         
     def test_start_check(self, machine):
         if machine in self._active_machines:
@@ -151,6 +150,7 @@
         if machine in self._active_machines:
             idx = self._active_machines.index(machine)
             del self._active_machines[idx]
+            #print 'Active machines after delete:', self._active_machines
 
     def load_invent_client(self):
         # Loads invent client. Singleton, doesn't construct unless valid login

Modified: pkg/trunk/pr2/life_test/tests.xml
===================================================================
--- pkg/trunk/pr2/life_test/tests.xml	2009-06-04 23:29:28 UTC (rev 16746)
+++ pkg/trunk/pr2/life_test/tests.xml	2009-06-04 23:51:12 UTC (rev 16747)
@@ -9,6 +9,13 @@
 	rate="3" script="wrist_test/test_gui.launch" 
 	type="Life Test" />
 
+  <test serial="6804213" name="Fixed Wrist Test" 
+	desc="Test of wrist attached to a bar, jerking around."
+	rate="3" script="wrist_test/test_gui_bar.launch" 
+	type="Life Test" />
+
+
+
   <test serial="6804040" name="Head Life Test"
 	desc="Tests head on test cart, moving between [-0.4, 1.4] tilt, [-2.7, 2.7] pan."
 	rate="2.0" script="head_test/life_test/test_gui.launch"

Added: pkg/trunk/pr2/life_test/wrist_test/test_gui_bar.launch
===================================================================
--- pkg/trunk/pr2/life_test/wrist_test/test_gui_bar.launch	                        (rev 0)
+++ pkg/trunk/pr2/life_test/wrist_test/test_gui_bar.launch	2009-06-04 23:51:12 UTC (rev 16747)
@@ -0,0 +1,10 @@
+<launch>
+  <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find life_test)/wrist_test/wrist.xml'" />
+
+  <node machine="test_host_root" pkg="pr2_etherCAT" type="pr2_etherCAT" args="-i eth0 -x robotdesc/pr2"/>
+
+  <!-- Don't calibrate wrist attached to bar. -->
+
+  <include file="$(find life_test)/wrist_test/life_test.launch" args="--bar" />
+
+</launch>


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




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

personalrobots-commit