Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[16747] pkg/trunk/pr2/life_test
|
From: <wa...@us...> - 2009-06-04 23:51:19
|
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.
|
