|
From: <wa...@us...> - 2009-07-21 21:50:55
|
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.
|