Menu
▾
▴
personalrobots-commit
|
From: <tf...@us...> - 2009-06-23 21:02:22
|
Revision: 17545
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17545&view=rev
Author: tfoote
Date: 2009-06-23 21:02:21 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
creating geometery stack
Added Paths:
-----------
pkg/trunk/stacks/geometery/
pkg/trunk/stacks/geometery/stack.xml
Added: pkg/trunk/stacks/geometery/stack.xml
===================================================================
--- pkg/trunk/stacks/geometery/stack.xml (rev 0)
+++ pkg/trunk/stacks/geometery/stack.xml 2009-06-23 21:02:21 UTC (rev 17545)
@@ -0,0 +1,13 @@
+<stack name="geometery" version="0.1">
+ <description brief="geometery and math libraries">
+ The basis geometric and math libraries used in ros.
+ </description>
+ <author>Maintained by Tully Foote tf...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/ROS</url>
+ <depend stack="ros" version="0.5"/>
+ <depend stack="common_msgs" version="0.5"/>
+ <depend stack="common" version="0.5"/>
+</stack>
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-23 21:17:30
|
Revision: 17547
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17547&view=rev
Author: tfoote
Date: 2009-06-23 21:17:29 +0000 (Tue, 23 Jun 2009)
Log Message:
-----------
creating common_msgs stack
Added Paths:
-----------
pkg/trunk/stacks/common_msgs/
pkg/trunk/stacks/common_msgs/stack.xml
Copied: pkg/trunk/stacks/common_msgs/stack.xml (from rev 17545, pkg/trunk/common/stack.xml)
===================================================================
--- pkg/trunk/stacks/common_msgs/stack.xml (rev 0)
+++ pkg/trunk/stacks/common_msgs/stack.xml 2009-06-23 21:17:29 UTC (rev 17547)
@@ -0,0 +1,14 @@
+<stack name="common_msgs" version="0.1">
+ <description brief="common messages personal robots">
+ A set of code and messages that are widely useful to all robots. Things
+ like generic robot messages (i.e., kinematics, transforms), a generic
+ transform library (tf), laser-scan utilities, etc.
+ </description>
+ <author>Maintained by Tully Foote tf...@wi...</author>
+ <license>BSD</license>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/ROS</url>
+ <depend stack="ros" version="0.5"/
+ <depend stack="common" version="0.1"/>
+</stack>
+
Property changes on: pkg/trunk/stacks/common_msgs/stack.xml
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/common/stack.xml:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-06-24 01:22:13
|
Revision: 17586
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17586&view=rev
Author: tfoote
Date: 2009-06-24 00:49:03 +0000 (Wed, 24 Jun 2009)
Log Message:
-----------
geometery to geometry
Modified Paths:
--------------
pkg/trunk/stacks/geometry/stack.xml
Added Paths:
-----------
pkg/trunk/stacks/geometry/
Removed Paths:
-------------
pkg/trunk/stacks/geometery/
Property changes on: pkg/trunk/stacks/geometry
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/stacks/geometery:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
Modified: pkg/trunk/stacks/geometry/stack.xml
===================================================================
--- pkg/trunk/stacks/geometery/stack.xml 2009-06-23 23:43:34 UTC (rev 17583)
+++ pkg/trunk/stacks/geometry/stack.xml 2009-06-24 00:49:03 UTC (rev 17586)
@@ -1,5 +1,5 @@
-<stack name="geometery" version="0.1">
- <description brief="geometery and math libraries">
+<stack name="geometry" version="0.1">
+ <description brief="geometry and math libraries">
The basis geometric and math libraries used in ros.
</description>
<author>Maintained by Tully Foote tf...@wi...</author>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2009-06-25 01:15:53
|
Revision: 17646
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17646&view=rev
Author: stuglaser
Date: 2009-06-25 01:15:51 +0000 (Thu, 25 Jun 2009)
Log Message:
-----------
calibrate_arms.py had bad indentation.
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py
pkg/trunk/stacks/pr2/pr2_alpha/manifest.xml
Modified: pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py 2009-06-25 01:01:49 UTC (rev 17645)
+++ pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py 2009-06-25 01:15:51 UTC (rev 17646)
@@ -242,8 +242,8 @@
xml = ''
if len(sys.argv) > 1:
- xacro_cmd = roslib.packages.get_pkg_dir('xacro', True) + '/xacro.py'
- xmls = [os.popen2(xacro_cmd + " %s" % f)[1].read() for f in rospy.myargv()[1:]]
+ xacro_cmd = roslib.packages.get_pkg_dir('xacro', True) + '/xacro.py'
+ xmls = [os.popen2(xacro_cmd + " %s" % f)[1].read() for f in rospy.myargv()[1:]]
# Poor man's xml splicer
for i in range(len(xmls) - 1):
Modified: pkg/trunk/stacks/pr2/pr2_alpha/manifest.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/manifest.xml 2009-06-25 01:01:49 UTC (rev 17645)
+++ pkg/trunk/stacks/pr2/pr2_alpha/manifest.xml 2009-06-25 01:15:51 UTC (rev 17646)
@@ -27,6 +27,7 @@
<depend package="forearm_cam" />
<depend package="robot_state_publisher" />
<depend package="prosilica_cam" />
+ <depend package="mechanism_bringup" />
<!-- For testing -->
<depend package="std_msgs"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-06-29 22:18:47
|
Revision: 17904
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17904&view=rev
Author: wattsk
Date: 2009-06-29 22:17:36 +0000 (Mon, 29 Jun 2009)
Log Message:
-----------
Changed name of calibrate_arms to calibrate_pr2 and changed sequence. Renamed calibration controllers in pr2_default_controllers for each robot
Modified Paths:
--------------
pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
pkg/trunk/stacks/pr2/pr2_alpha/pre_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prf_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/prg_ethercat_reset.launch
pkg/trunk/stacks/pr2/pr2_alpha/test/alpha_full_connection.xml
Added Paths:
-----------
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/pr2_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/pre_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/prf_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/prg_calibration_controller.xml
Removed Paths:
-------------
pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py
pkg/trunk/stacks/pr2/pr2_alpha/calibrate.launch
pkg/trunk/stacks/pr2/pr2_alpha/calibrate_pre.launch
pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prf.launch
pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prg.launch
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller_pre.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller_prf.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/calibration_controllers/full_calibration_controller_prg.xml
Deleted: pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,277 +0,0 @@
-#!/usr/bin/python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2008, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above
-# copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided
-# with the distribution.
-# * Neither the name of the Willow Garage nor the names of its
-# contributors may be used to endorse or promote products derived
-# from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-
-# Author: Kevin Watts
-
-# Calibrate the arm sequentially
-# Wrist -> Elbow roll -> Elbow flex -> Upperarm Roll ->
-# Shoulder Lift -> Shoulder Pan
-
-import roslib
-import copy
-import threading
-import sys, os
-from time import sleep
-
-# Loads interface with the robot.
-roslib.load_manifest('mechanism_bringup')
-import rospy
-from std_msgs.msg import *
-import std_srvs.srv
-from robot_srvs.srv import *
-
-from robot_mechanism_controllers.srv import *
-from robot_mechanism_controllers import controllers
-
-
-class SendMessageOnSubscribe(rospy.SubscribeListener):
- def __init__(self, msg):
- self.msg = msg
- print "Waiting for subscriber..."
-
- def peer_subscribe(self, topic_name, topic_publish, peer_publish):
- peer_publish(self.msg)
- sleep(0.1)
- # TODO: change this line when flushing messages is implemented
- # rospy.signal_shutdown("Done")
- # sys.exit(0)
-
-
-spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
-kill_controller = rospy.ServiceProxy('kill_controller', KillController)
-
-def slurp(filename):
- f = open(filename)
- stuff = f.read()
- f.close()
- return stuff
-
-def calibrate(config):
- #spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
- #kill_controller = rospy.ServiceProxy('kill_controller', KillController)
-
- # Spawns the controllers
- resp = spawn_controller(SpawnControllerRequest(config,1))
-
- # Accumulates the list of spawned controllers
- launched = []
- print "OKs: " + ','.join([str(ok) for ok in resp.ok])
- try:
- for i in range(len(resp.ok)):
- if resp.ok[i] == 0:
- print "Failed: %s" % resp.name[i]
- else:
- launched.append(resp.name[i])
- print "Launched: %s" % ', '.join(launched)
-
- # Sets up callbacks for calibration completion
- waiting_for = launched[:]
- def calibrated(msg, name): # Somewhat not thread-safe
- if name in waiting_for:
- waiting_for.remove(name)
- for name in waiting_for:
- rospy.Subscriber("/%s/calibrated" % name, Empty, calibrated, name)
-
- # Waits until all the controllers have calibrated
- while waiting_for and not rospy.is_shutdown():
- print "Waiting for: %s" % ', '.join(waiting_for)
- sleep(0.5)
- finally:
- # Try to kill controllers several times
- # Make sure they're dead.
- for name in launched:
- for i in range(1,6):
- try:
- kill_controller(name)
- break # Go to next controller if no exception
- except:
- print "Failed to kill controller %s on try %d" % (name, i)
-
-#
-# Functions make xml code for controllers
-#
-def xml_for_cal(name, velocity, p, i, d, iClamp):
- return """
-<controller name="cal_%s" topic="cal_%s" type="JointUDCalibrationControllerNode">
-<calibrate joint="%s_joint" actuator="%s_motor"
-transmission="%s_trans" velocity="%d" />
-<pid p="%d" i="%d" d="%d" iClamp="%d" />
-</controller>""" % (name, name, name, name, name, velocity, p, i, d, iClamp)
-
-def xml_for_hold(name, p, i, d, iClamp):
- return """
-<controller name="%s_controller" type="JointPositionControllerNode">
-<joint name="%s_joint">
-<pid p="%d" i="%d" d="%d" iClamp="%d" />
-</controller>""" % (name, name, p, i, d, iClamp)
-
-def xml_for_wrist(side):
- return """
-<controller name="cal_%s_wrist" type="WristCalibrationControllerNode">
-<calibrate transmission="%s_wrist_trans"
-actuator_l="%s_wrist_l_motor" actuator_r="%s_wrist_r_motor"
-flex_joint="%s_wrist_flex_joint" roll_joint="%s_wrist_roll_joint"
-velocity="1.5" />
-<pid p="4.0" i="0.2" d="0" iClamp="2.0" />
-</controller>""" % (side, side, side, side, side, side)
-
-def hold_joint(name, p, i, d, iClamp, holding):
- # Try to launch 3x
- # If launched, add to list of holding controllers and return true
- for i in range(1,4):
- try:
- resp = spawn_controller(xml_for_hold(name, p, i, d, iClamp))
- if resp.ok[0] != 0:
- holding.append(resp.name[0])
- return True
- except:
- print "Failed to spawn holding controller %s on try %d" % (name, i)
-
- return False
-
-def set_controller(controller, command):
- # try:
- #rospy.init_node('control', anonymous = True)
- #finally:
- pub = rospy.Publisher('/' + controller + '/set_command', Float64,
- SendMessageOnSubscribe(Float64(command)))
-
-def calibrate_imu():
- print "Calibrating IMU"
- try:
- rospy.wait_for_service('/imu/calibrate', 5)
- calibrate = rospy.ServiceProxy('/imu/calibrate', std_srvs.srv.Empty)
- calibrate(timeout=20) # This should take 10 seconds
- return True
- except:
- print "IMU calibration failed: %s"%sys.exc_info()[0]
- return False
-
-if __name__ == '__main__':
- rospy.wait_for_service('spawn_controller')
- # if rospy.is_shutdown():
- # return
-
- rospy.init_node('calibration', anonymous=True)
-
- holding = [] # Tracks which controllers are holding joints by name
-
- # Calibrate all joints sequentially
- # Hold joints after they're calibrated.
- # Can set desired joint position by using name + /set_command service
- # for joint position controllers
-
- # Wrist and forearm calibrated with forearm_calibrator in arm_defs.xml
-
- imustatus = calibrate_imu()
- if not imustatus:
- print "IMU Calibration failed."
-
- print "Calibrating shoulder pan"
- calibrate(xml_for_cal("r_shoulder_pan", 1.0, 7, 0.5, 0, 1.0))
- hold_joint("r_shoulder_pan", 60, 10, 5, 4, holding)
- set_controller("r_shoulder_pan_controller", float(-0.7))
-
- calibrate(xml_for_cal("l_shoulder_pan", 1.0, 7, 0.5, 0, 1.0))
- hold_joint("l_shoulder_pan", 60, 10, 5, 4, holding)
- set_controller("l_shoulder_pan_controller", float(0.7))
-
- print "Calibrating elbow flex"
- calibrate(xml_for_cal("r_elbow_flex", -1.0, 6, 0.2, 0, 1) + "\n" + xml_for_cal("l_elbow_flex", -1.0, 6, 0.2, 0, 1))
- hold_joint("r_elbow_flex", 100, 20, 10, 2, holding)
- hold_joint("l_elbow_flex", 100, 20, 10, 2, holding)
-
- set_controller("r_elbow_flex_controller", float(3.0))
- set_controller("l_elbow_flex_controller", float(3.0))
-
- print "Calibrating right upperarm roll"
- upperarm_roll_name = "r_upper_arm_roll"
- calibrate(xml_for_cal(upperarm_roll_name, 1.0, 6, 0.2, 0, 2))
- hold_joint(upperarm_roll_name, 25, 2, 1.0, 1.0, holding)
- set_controller("r_upper_arm_roll_controller", float(0.0))
-
- print "Calibrating left upperarm roll"
- upperarm_roll_name = "l_upper_arm_roll"
- calibrate(xml_for_cal(upperarm_roll_name, 1.0, 6, 0.2, 0, 2))
- hold_joint(upperarm_roll_name, 25, 2, 1.0, 1.0, holding)
- set_controller("l_upper_arm_roll_controller", float(0.0))
-
- print "Calibrating shoulder lift"
- shoulder_lift_name = "r_shoulder_lift"
- calibrate(xml_for_cal("r_shoulder_lift", -1.0, 9, 1.0, 0, 6) + "\n" + xml_for_cal("l_shoulder_lift", -1.0, 9, 1.0, 0, 6))
-
- hold_joint("r_shoulder_lift", 60, 10, 5, 4, holding)
- hold_joint("l_shoulder_lift", 60, 10, 5, 4, holding)
-
- set_controller("r_shoulder_lift_controller", float(3.0))
- set_controller("l_shoulder_lift_controller", float(3.0))
-
-
-
- sleep(0.5)
- print "Calibrating rest of robot"
-
- # Calibrate other controllers given
- xml = ''
-
- if len(sys.argv) > 1:
- xacro_cmd = roslib.packages.get_pkg_dir('xacro', True) + '/xacro.py'
- xmls = [os.popen2(xacro_cmd + " %s" % f)[1].read() for f in rospy.myargv()[1:]]
-
- # Poor man's xml splicer
- for i in range(len(xmls) - 1):
- xmls[i] = xmls[i].replace('</controllers>', '')
- xmls[i+1] = xmls[i+1].replace('<controllers>', '')
- xml = "\n".join(xmls)
- else:
- print "Reading from stdin..."
- xml = sys.stdin.read()
-
- try:
- calibrate(xml)
- finally:
- # Kill all holding controllers
- for name in holding:
- for i in range(1,6):
- try:
- kill_controller(name)
- break # Go to next controller if no exception
- except:
- print "Failed to kill controller %s on try %d" % (name, i)
-
- if not imustatus:
- print "Mechanism calibration complete, but IMU calibration failed."
- sys.exit(2)
-
- print "Calibration complete"
Copied: pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py (from rev 17893, pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_arms.py)
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py (rev 0)
+++ pkg/trunk/stacks/mechanism/mechanism_bringup/scripts/calibrate_pr2.py 2009-06-29 22:17:36 UTC (rev 17904)
@@ -0,0 +1,266 @@
+#!/usr/bin/python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+# Author: Kevin Watts
+
+# Calibrates the PR-2 in a safe sequence
+
+import roslib
+import copy
+import threading
+import sys, os
+from time import sleep
+
+# Loads interface with the robot.
+roslib.load_manifest('mechanism_bringup')
+import rospy
+from std_msgs.msg import *
+from robot_srvs.srv import *
+
+from robot_mechanism_controllers.srv import *
+from robot_mechanism_controllers import controllers
+
+
+class SendMessageOnSubscribe(rospy.SubscribeListener):
+ def __init__(self, msg):
+ self.msg = msg
+ print "Waiting for subscriber..."
+
+ def peer_subscribe(self, topic_name, topic_publish, peer_publish):
+ peer_publish(self.msg)
+ sleep(0.1)
+ # TODO: change this line when flushing messages is implemented
+ # rospy.signal_shutdown("Done")
+ # sys.exit(0)
+
+
+spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+def slurp(filename):
+ f = open(filename)
+ stuff = f.read()
+ f.close()
+ return stuff
+
+def calibrate(config):
+ # Spawns the controllers
+ resp = spawn_controller(SpawnControllerRequest(config,1))
+
+ # Accumulates the list of spawned controllers
+ launched = []
+ print "OKs: " + ','.join([str(ok) for ok in resp.ok])
+ try:
+ for i in range(len(resp.ok)):
+ if resp.ok[i] == 0:
+ print "Failed: %s" % resp.name[i]
+ else:
+ launched.append(resp.name[i])
+ print "Launched: %s" % ', '.join(launched)
+
+ # Sets up callbacks for calibration completion
+ waiting_for = launched[:]
+ def calibrated(msg, name): # Somewhat not thread-safe
+ if name in waiting_for:
+ waiting_for.remove(name)
+ for name in waiting_for:
+ rospy.Subscriber("%s/calibrated" % name, Empty, calibrated, name)
+
+ # Waits until all the controllers have calibrated
+ while waiting_for and not rospy.is_shutdown():
+ print "Waiting for: %s" % ', '.join(waiting_for)
+ sleep(0.5)
+ finally:
+ # Try to kill controllers several times
+ # Make sure they're dead.
+ for name in launched:
+ for i in range(1,6):
+ try:
+ kill_controller(name)
+ break # Go to next controller if no exception
+ except:
+ print "Failed to kill controller %s on try %d" % (name, i)
+
+#
+# Functions make xml code for controllers
+#
+def xml_for_cal(name, velocity, p, i, d, iClamp):
+ return """
+<controller name="cal_%s" topic="cal_%s" type="JointUDCalibrationControllerNode">
+<calibrate joint="%s_joint" actuator="%s_motor"
+transmission="%s_trans" velocity="%d" />
+<pid p="%d" i="%d" d="%d" iClamp="%d" />
+</controller>""" % (name, name, name, name, name, velocity, p, i, d, iClamp)
+
+def xml_for_hold(name, p, i, d, iClamp):
+ return """
+<controller name="%s_controller" type="JointPositionControllerNode">
+<joint name="%s_joint">
+<pid p="%d" i="%d" d="%d" iClamp="%d" />
+</controller>""" % (name, name, p, i, d, iClamp)
+
+def xml_for_wrist(side):
+ return """
+<controller name="cal_%s_wrist" type="WristCalibrationControllerNode">
+<calibrate transmission="%s_wrist_trans"
+actuator_l="%s_wrist_l_motor" actuator_r="%s_wrist_r_motor"
+flex_joint="%s_wrist_flex_joint" roll_joint="%s_wrist_roll_joint"
+velocity="1.5" />
+<pid p="4.0" i="0.2" d="0" iClamp="2.0" />
+</controller>""" % (side, side, side, side, side, side)
+
+def hold_joint(name, p, i, d, iClamp, holding):
+ # Try to launch 3x
+ # If launched, add to list of holding controllers and return true
+ for i in range(1,4):
+ try:
+ resp = spawn_controller(xml_for_hold(name, p, i, d, iClamp), 1)
+ if resp.ok[0] != 0:
+ holding.append(resp.name[0])
+ return True
+ except:
+ rospy.logerr("Failed to spawn holding controller %s on try %d" % (name, i))
+
+ return False
+
+def set_controller(controller, command):
+ pub = rospy.Publisher(controller + '/set_command', Float64,
+ SendMessageOnSubscribe(Float64(command)))
+
+def calibrate_imu():
+ rospy.logout("Calibrating IMU")
+ try:
+ rospy.wait_for_service('imu/calibrate', 5)
+ calibrate = rospy.ServiceProxy('imu/calibrate', std_srvs.srv.Empty)
+ calibrate(timeout=20) # This should take 10 seconds
+ return True
+ except:
+ rospy.logout("IMU calibration failed: %s"%sys.exc_info()[0])
+ return False
+
+if __name__ == '__main__':
+ rospy.wait_for_service('spawn_controller')
+
+ rospy.init_node('calibration', anonymous=True)
+
+ holding = [] # Tracks which controllers are holding joints by name
+
+ # Calibrate all joints sequentially
+ # Hold joints after they're calibrated.
+ # Can set desired joint position by using name + /set_command service
+ # for joint position controllers
+
+ imustatus = calibrate_imu()
+ if not imustatus:
+ rospy.logerr("IMU Calibration failed.")
+
+ # Check sign on torso lift controller
+ calibrate(xml_for_cal("torso_lift", -10, 2000000, 0, 10000, 12000))
+ hold_joint("torso_lift", 1000000, 0.0, 10000, 1000000, holding)
+ set_controller("torso_lift_controller", float(0.08))
+ sleep(0.5)
+
+ calibrate(xml_for_cal("r_shoulder_pan", 1.0, 7, 0.5, 0, 1.0))
+ hold_joint("r_shoulder_pan", 60, 10, 5, 4, holding)
+ set_controller("r_shoulder_pan_controller", float(-0.7))
+
+ calibrate(xml_for_cal("l_shoulder_pan", 1.0, 7, 0.5, 0, 1.0))
+ hold_joint("l_shoulder_pan", 60, 10, 5, 4, holding)
+ set_controller("l_shoulder_pan_controller", float(0.7))
+
+ calibrate(xml_for_cal("r_elbow_flex", -1.0, 6, 0.2, 0, 1) + "\n" + xml_for_cal("l_elbow_flex", -1.0, 6, 0.2, 0, 1))
+ hold_joint("r_elbow_flex", 100, 20, 10, 2, holding)
+ hold_joint("l_elbow_flex", 100, 20, 10, 2, holding)
+
+ set_controller("r_elbow_flex_controller", float(-2.0))
+ set_controller("l_elbow_flex_controller", float(-2.0))
+
+ upperarm_roll_name = "r_upper_arm_roll"
+ calibrate(xml_for_cal(upperarm_roll_name, 1.0, 6, 0.2, 0, 2))
+ hold_joint(upperarm_roll_name, 25, 2, 1.0, 1.0, holding)
+ set_controller("r_upper_arm_roll_controller", float(0.0))
+
+ upperarm_roll_name = "l_upper_arm_roll"
+ calibrate(xml_for_cal(upperarm_roll_name, 1.0, 6, 0.2, 0, 2))
+ hold_joint(upperarm_roll_name, 25, 2, 1.0, 1.0, holding)
+ set_controller("l_upper_arm_roll_controller", float(0.0))
+
+ shoulder_lift_name = "r_shoulder_lift"
+ calibrate(xml_for_cal("r_shoulder_lift", -1.0, 9, 1.0, 0, 6) + "\n" + xml_for_cal("l_shoulder_lift", -1.0, 9, 1.0, 0, 6))
+
+ hold_joint("r_shoulder_lift", 60, 10, 5, 4, holding)
+ hold_joint("l_shoulder_lift", 60, 10, 5, 4, holding)
+
+ set_controller("r_shoulder_lift_controller", float(1.0))
+ set_controller("l_shoulder_lift_controller", float(1.0))
+
+ rospy.logerr('Setting torso lift controller')
+ torso_pub = rospy.Publisher('torso_lift_controller/set_command', Float64)
+ torso_pub.publish(Float64(float(0.0)))
+ sleep(1.5)
+
+ # Calibrate other controllers given
+ xml = ''
+
+ if len(sys.argv) > 1:
+ xacro_cmd = roslib.packages.get_pkg_dir('xacro', True) + '/xacro.py'
+
+ xmls = [os.popen2(xacro_cmd + " %s" % f)[1].read() for f in rospy.myargv()[1:]]
+
+ # Poor man's xml splicer
+ for i in range(len(xmls) - 1):
+ xmls[i] = xmls[i].replace('</controllers>', '')
+ xmls[i+1] = xmls[i+1].replace('<controllers>', '')
+ xml = "\n".join(xmls)
+ else:
+ print "Reading from stdin..."
+ xml = sys.stdin.read()
+
+ try:
+ calibrate(xml)
+ finally:
+ # Kill all holding controllers
+ for name in holding:
+ for i in range(1,6):
+ try:
+ kill_controller(name)
+ break # Go to next controller if no exception
+ except:
+ rospy.logerr("Failed to kill controller %s on try %d" % (name, i))
+
+ if not imustatus:
+ print "Mechanism calibration complete, but IMU calibration failed."
+ sys.exit(2)
+
+ rospy.logout("Calibration complete")
Deleted: pkg/trunk/stacks/pr2/pr2_alpha/calibrate.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/calibrate.launch 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/calibrate.launch 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,5 +0,0 @@
-<launch>
- <node pkg="mechanism_bringup" type="calibrate.py"
- args="$(find pr2_default_controllers)/calibration_controllers/full_calibration_controller.xml"
- output="screen" />
-</launch>
Deleted: pkg/trunk/stacks/pr2/pr2_alpha/calibrate_pre.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/calibrate_pre.launch 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/calibrate_pre.launch 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,6 +0,0 @@
-<launch>
-<!-- Calibrates both arms -->
- <node pkg="mechanism_bringup" type="calibrate_arms.py"
- args="$(find pr2_default_controllers)/calibration_controllers/full_calibration_controller_pre.xml"
- output="screen" />
-</launch>
Deleted: pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prf.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prf.launch 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prf.launch 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,6 +0,0 @@
-<launch>
-<!-- Calibrates both arms -->
- <node pkg="mechanism_bringup" type="calibrate_arms.py"
- args="$(find pr2_default_controllers)/calibration_controllers/full_calibration_controller_prf.xml"
- output="screen" />
-</launch>
\ No newline at end of file
Deleted: pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prg.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prg.launch 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/calibrate_prg.launch 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,6 +0,0 @@
-<launch>
-<!-- Calibrates both arms -->
- <node pkg="mechanism_bringup" type="calibrate_arms.py"
- args="$(find pr2_default_controllers)/calibration_controllers/full_calibration_controller_prg.xml"
- output="screen" />
-</launch>
\ No newline at end of file
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.launch
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.launch 2009-06-29 22:17:36 UTC (rev 17904)
@@ -5,10 +5,10 @@
<!-- Joystick -->
<param name="joy/deadzone" value="5000"/>
- <node machine="four" pkg="joy" type="joy" respawn="true"/>
+ <node machine="two" pkg="joy" type="joy" respawn="true"/>
<!-- Power Board Control Node -->
- <node pkg="pr2_power_board" type="power_node" respawn="true"/>
+ <node machine="two" pkg="pr2_power_board" type="power_node" respawn="true"/>
<!-- Robot state publisher -->
<node machine="two" pkg="robot_state_publisher" type="state_publisher" name="robot_state_publisher">
@@ -18,6 +18,7 @@
<!-- Battery Monitor -->
<node machine="two" pkg="ocean_battery_driver" type="monitor" respawn="true"/>
+
<!-- Battery Assertions -->
<include file="$(find ocean_battery_driver)/expected_batteries.launch"/>
@@ -30,7 +31,7 @@
</node>
<!-- Tilt Laser -->
- <node machine="three" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
+ <node machine="realtime" pkg="hokuyo_node" type="hokuyo_node" name="tilt_hokuyo_node" args="scan:=tilt_scan">
<param name="port" type="string" value="/dev/ttyACM0" />
<param name="frameid" type="string" value="laser_tilt_link" />
<param name="min_ang_degrees" type="double" value="-80.0" />
@@ -45,28 +46,30 @@
<param name="imu/autostart" type="bool" value="true" />
<param name="imu/frameid" type="string" value="imu" />
<param name="imu/autocalibrate" type="bool" value="false" />
- <node machine="four" pkg="imu_node" type="imu_node" output="screen"/>
+ <node machine="realtime" pkg="imu_node" type="imu_node" output="screen"/>
-<!-- Videre Stereo cam -->
+<!-- Videre Stereo cam
<include file="$(find pr2_alpha)/stereo.launch" />
-
-<!-- Sound -->
+-->
+<!-- Sound
<node pkg="sound_play" type="soundplay_node.py" machine="three" />
+-->
<!-- Runtime Diagnostics Logging -->
<node pkg="rosrecord" type="rosrecord" args="-f /hwlog/pre_runtime_automatic /diagnostics" />
-<!-- NTP monitoring script Warns to console if sync error -->
+<!-- NTP monitoring script Warns to console if sync error
<node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="realtime"/>
<node pkg="runtime_monitor" type="ntp_monitor.py" args="fw1 7000" machine="two"/>
- <node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="three"/>
- <node pkg="runtime_monitor" type="ntp_monitor.py" args="pre2" machine="four"/>
+-->
-<!-- Disk usage monitoring script Warns to console if disk full -->
- <node pkg="pr2_computer_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="realtime"/>
- <node pkg="pr2_computer_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="two"/>
- <node pkg="pr2_computer_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="three"/>
- <node pkg="pr2_computer_monitor" type="disk_usage.py" args="$(optenv HOME /home)" machine="four"/>
+ <!-- Disk usage monitoring script Warns to console if disk full -->
+ <node pkg="pr2_computer_monitor" type="hd_monitor.py" args="$(optenv HOME /home) /dev/sda" machine="realtime_root"/>
+ <node pkg="pr2_computer_monitor" type="hd_monitor.py" args="$(optenv HOME /home) /dev/sda /dev/sdb" machine="two_root"/>
+ <!-- Monitor CPU temp, usage -->
+ <node pkg="pr2_computer_monitor" type="cpu_monitor.py" machine="realtime_root" />
+ <node pkg="pr2_computer_monitor" type="cpu_monitor.py" machine="two_root" />
+
</launch>
Modified: pkg/trunk/stacks/pr2/pr2_alpha/pre.machine
===================================================================
--- pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-06-29 22:07:15 UTC (rev 17903)
+++ pkg/trunk/stacks/pr2/pr2_alpha/pre.machine 2009-06-29 22:17:36 UTC (rev 17904)
@@ -1,9 +1,8 @@
<launch>
- <machine name="realtime_root" user="root" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
+ <machine name="realtime_root" user="root" address="c1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="never"/>
- <machine name="realtime" address="pre1" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" />
- <machine name="two" address="pre2" ros-root="$(env ROS_ROOT)" ros-package-path="$(env ROS_PACKAGE_PATH)" default="tru...
[truncated message content] |
|
From: <tf...@us...> - 2009-07-03 01:04:32
|
Revision: 18247
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18247&view=rev
Author: tfoote
Date: 2009-07-03 01:04:31 +0000 (Fri, 03 Jul 2009)
Log Message:
-----------
real fix for #950 including removing work around hacks from rviz, tested with move_base_stage and rviz together
Modified Paths:
--------------
pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
pkg/trunk/stacks/geometry/tf/include/tf/tf.h
pkg/trunk/stacks/geometry/tf/src/tf.cpp
pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp
pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp
Modified: pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/include/tf/message_notifier.h 2009-07-03 01:04:31 UTC (rev 18247)
@@ -222,7 +222,7 @@
std::stringstream ss;
for (std::vector<std::string>::const_iterator it = target_frames_.begin(); it != target_frames_.end(); ++it)
{
- ss << *it << " ";
+ ss << tf::remap(tf_.getTFPrefix(), *it) << " ";
}
target_frames_string_ = ss.str();
}
Modified: pkg/trunk/stacks/geometry/tf/include/tf/tf.h
===================================================================
--- pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/include/tf/tf.h 2009-07-03 01:04:31 UTC (rev 18247)
@@ -234,6 +234,11 @@
*/
boost::signals::connection addTransformChangedListener(boost::function<void(void)> callback);
+ /**
+ * \brief Get the tf_prefix this is running with
+ */
+ std::string getTFPrefix() const { return tf_prefix_;};
+
protected:
/** \brief The internal storage class for ReferenceTransform.
Modified: pkg/trunk/stacks/geometry/tf/src/tf.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/src/tf.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -45,15 +45,13 @@
if (frame_id.size() > 0)
if (frame_id[0] == '/')
{
- std::string stripped_frame_id = frame_id.substr(1,frame_id.length());
- return stripped_frame_id;
+ return frame_id;
}
if (prefix.size() > 0)
{
if (prefix[0] == '/')
{
- std::string stripped_prefix = prefix.substr(1,prefix.length());
- std::string composite = stripped_prefix;
+ std::string composite = prefix;
composite.append("/");
composite.append(frame_id);
return composite;
@@ -61,7 +59,8 @@
else
{
std::string composite;
- composite = prefix;
+ composite = "/";
+ composite.append(prefix);
composite.append("/");
composite.append(frame_id);
return composite;
@@ -70,7 +69,10 @@
}
else
{
- return frame_id;
+ std::string composite;
+ composite = "/";
+ composite.append(frame_id);
+ return composite;
}
};
@@ -115,46 +117,52 @@
bool Transformer::setTransform(const Stamped<btTransform>& transform, const std::string& authority)
{
+
+ Stamped<btTransform> mapped_transform = transform;
+ mapped_transform.frame_id_ = remap(tf_prefix_, transform.frame_id_);
+ mapped_transform.parent_id_ = remap(tf_prefix_, transform.parent_id_);
+
+
bool error_exists = false;
- if (transform.frame_id_ == transform.parent_id_)
+ if (mapped_transform.frame_id_ == mapped_transform.parent_id_)
{
- ROS_ERROR("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with parent_id and frame_id \"%s\" because they are the same", authority.c_str(), transform.frame_id_.c_str());
+ ROS_ERROR("TF_SELF_TRANSFORM: Ignoring transform from authority \"%s\" with parent_id and frame_id \"%s\" because they are the same", authority.c_str(), mapped_transform.frame_id_.c_str());
error_exists = true;
}
- if (transform.frame_id_ == "")
+ if (mapped_transform.frame_id_ == "/")//empty frame id will be mapped to "/"
{
ROS_ERROR("TF_NO_FRAME_ID: Ignoring transform from authority \"%s\" because frame_id not set ", authority.c_str());
error_exists = true;
}
- if (transform.parent_id_ == "")
+ if (mapped_transform.parent_id_ == "/")//empty parent id will be mapped to "/"
{
- ROS_ERROR("TF_NO_PARENT_ID: Ignoring transform with frame_id \"%s\" from authority \"%s\" because parent_id not set", transform.frame_id_.c_str(), authority.c_str());
+ ROS_ERROR("TF_NO_PARENT_ID: Ignoring transform with frame_id \"%s\" from authority \"%s\" because parent_id not set", mapped_transform.frame_id_.c_str(), authority.c_str());
error_exists = true;
}
- if (std::isnan(transform.getOrigin().x()) || std::isnan(transform.getOrigin().y()) || std::isnan(transform.getOrigin().z())||
- std::isnan(transform.getRotation().x()) || std::isnan(transform.getRotation().y()) || std::isnan(transform.getRotation().z()) || std::isnan(transform.getRotation().w()))
+ if (std::isnan(mapped_transform.getOrigin().x()) || std::isnan(mapped_transform.getOrigin().y()) || std::isnan(mapped_transform.getOrigin().z())||
+ std::isnan(mapped_transform.getRotation().x()) || std::isnan(mapped_transform.getRotation().y()) || std::isnan(mapped_transform.getRotation().z()) || std::isnan(mapped_transform.getRotation().w()))
{
ROS_ERROR("TF_NAN_INPUT: Ignoring transform for frame_id \"%s\" from authority \"%s\" because of a nan value in the transform (%f %f %f) (%f %f %f %f)",
- transform.frame_id_.c_str(), authority.c_str(),
- transform.getOrigin().x(), transform.getOrigin().y(), transform.getOrigin().z(),
- transform.getRotation().x(), transform.getRotation().y(), transform.getRotation().z(), transform.getRotation().w()
+ mapped_transform.frame_id_.c_str(), authority.c_str(),
+ mapped_transform.getOrigin().x(), mapped_transform.getOrigin().y(), mapped_transform.getOrigin().z(),
+ mapped_transform.getRotation().x(), mapped_transform.getRotation().y(), mapped_transform.getRotation().z(), mapped_transform.getRotation().w()
);
error_exists = true;
}
if (error_exists)
return false;
- unsigned int frame_number = lookupOrInsertFrameNumber(transform.frame_id_);
- if (getFrame(frame_number)->insertData(TransformStorage(transform, lookupOrInsertFrameNumber(transform.parent_id_))))
+ unsigned int frame_number = lookupOrInsertFrameNumber(mapped_transform.frame_id_);
+ if (getFrame(frame_number)->insertData(TransformStorage(mapped_transform, lookupOrInsertFrameNumber(mapped_transform.parent_id_))))
{
frame_authority_[frame_number] = authority;
}
else
{
- ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", transform.frame_id_.c_str(), transform.stamp_.toSec(), authority.c_str());
+ ROS_WARN("TF_OLD_DATA ignoring data from the past for frame %s at time %g according to authority %s\nPossible reasons are listed at ", mapped_transform.frame_id_.c_str(), mapped_transform.stamp_.toSec(), authority.c_str());
return false;
}
@@ -340,11 +348,11 @@
bool Transformer::getParent(const std::string& frame_id, ros::Time time, std::string& parent) const
{
-
+ std::string mapped_frame_id = tf::remap(tf_prefix_, frame_id);
tf::TimeCache* cache;
try
{
- cache = getFrame(lookupFrameNumber(frame_id));
+ cache = getFrame(lookupFrameNumber(mapped_frame_id));
}
catch (tf::LookupException &ex)
{
@@ -366,12 +374,15 @@
int Transformer::getLatestCommonTime(const std::string& source, const std::string& dest, ros::Time & time, std::string * error_string) const
{
+ std::string mapped_source = tf::remap(tf_prefix_, source);
+ std::string mapped_dest = tf::remap(tf_prefix_, dest);
+
time = ros::Time(UINT_MAX, 999999999);///\todo replace with ros::TIME_MAX when it is merged from stable
int retval;
TransformLists lists;
try
{
- retval = lookupLists(lookupFrameNumber(dest), ros::Time(), lookupFrameNumber(source), lists, error_string);
+ retval = lookupLists(lookupFrameNumber(mapped_dest), ros::Time(), lookupFrameNumber(mapped_source), lists, error_string);
}
catch (tf::LookupException &ex)
{
@@ -381,7 +392,7 @@
}
if (retval == NO_ERROR)
{
- //Set time to latest timestamp of frameid in case of target and source frame id are the same
+ //Set time to latest timestamp of frameid in case of target and mapped_source frame id are the same
if (lists.inverseTransforms.size() == 0 && lists.forwardTransforms.size() == 0)
{
time = ros::Time::now();
@@ -923,7 +934,7 @@
// for (std::vector< TimeCache*>::iterator it = frames_.begin(); it != frames_.end(); ++it)
for (unsigned int counter = 1; counter < frames_.size(); counter ++)
{
- vec.push_back(std::string("/") + frameIDs_reverse[counter]);
+ vec.push_back(frameIDs_reverse[counter]);
}
return;
}
Modified: pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/test/test_notifier.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -270,7 +270,7 @@
Counter<robot_msgs::PointStamped> c("test_message", 20);
- ros::Duration().fromSec(0.2).sleep();
+ ros::Duration().fromSec(0.5).sleep();
ros::Time stamp = ros::Time::now();
@@ -456,7 +456,7 @@
xt.sec += 10;
boost::timed_mutex::scoped_timed_lock lock(n2.mutex_, xt);
-
+ printf("HHHHHHHHHHHHHHHEEEEEEEEEEEEEEEEEEEERRRRRRRRRRRRRRRRREEEEEEEEEEEEEEEEEEE %s\n", notifier2->getTargetFramesString().c_str());
EXPECT_EQ(true, lock.owns_lock());
}
Modified: pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp
===================================================================
--- pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/geometry/tf/test/tf_unittest.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -819,7 +819,7 @@
for (uint64_t i = 0; i < children.size(); i++)
{
EXPECT_TRUE(mTR.getParent(children[i], ros::Time().fromNSec(10), output));
- EXPECT_STREQ(parents[i].c_str(), output.c_str());
+ EXPECT_STREQ(tf::remap("",parents[i]).c_str(), output.c_str());
}
EXPECT_FALSE(mTR.getParent("j", ros::Time().fromNSec(10), output));
@@ -1270,38 +1270,39 @@
{
Transformer mTR;
- EXPECT_FALSE(mTR.frameExists("b"));;
- EXPECT_FALSE(mTR.frameExists("parent"));
- EXPECT_FALSE(mTR.frameExists("other"));
- EXPECT_FALSE(mTR.frameExists("frame"));
- mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "b", "parent"));
+ EXPECT_FALSE(mTR.frameExists("/b"));;
+ EXPECT_FALSE(mTR.frameExists("/parent"));
+ EXPECT_FALSE(mTR.frameExists("/other"));
+ EXPECT_FALSE(mTR.frameExists("/frame"));
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/b", "/parent"));
- EXPECT_TRUE(mTR.frameExists("b"));
- EXPECT_TRUE(mTR.frameExists("parent"));
- EXPECT_FALSE(mTR.frameExists("other"));
- EXPECT_FALSE(mTR.frameExists("frame"));
+ EXPECT_TRUE(mTR.frameExists("/b"));
+ EXPECT_TRUE(mTR.frameExists("/parent"));
+ EXPECT_FALSE(mTR.frameExists("/other"));
+ EXPECT_FALSE(mTR.frameExists("/frame"));
- mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "other", "frame"));
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/other", "/frame"));
- EXPECT_TRUE(mTR.frameExists("b"));
- EXPECT_TRUE(mTR.frameExists("parent"));
- EXPECT_TRUE(mTR.frameExists("other"));
- EXPECT_TRUE(mTR.frameExists("frame"));
+ EXPECT_TRUE(mTR.frameExists("/b"));
+ EXPECT_TRUE(mTR.frameExists("/parent"));
+ EXPECT_TRUE(mTR.frameExists("/other"));
+ EXPECT_TRUE(mTR.frameExists("/frame"));
}
+
TEST(tf, remap)
{
//no prefix
- EXPECT_STREQ("id", tf::remap("","id").c_str());
+ EXPECT_STREQ("/id", tf::remap("","id").c_str());
//prefix w/o /
- EXPECT_STREQ("asdf/id", tf::remap("asdf","id").c_str());
+ EXPECT_STREQ("/asdf/id", tf::remap("asdf","id").c_str());
//prefix w /
- EXPECT_STREQ("asdf/id", tf::remap("/asdf","id").c_str());
+ EXPECT_STREQ("/asdf/id", tf::remap("/asdf","id").c_str());
// frame_id w / -> no prefix
- EXPECT_STREQ("id", tf::remap("asdf","/id").c_str());
+ EXPECT_STREQ("/id", tf::remap("asdf","/id").c_str());
// frame_id w / -> no prefix
- EXPECT_STREQ("id", tf::remap("/asdf","/id").c_str());
+ EXPECT_STREQ("/id", tf::remap("/asdf","/id").c_str());
}
@@ -1580,6 +1581,31 @@
}
+
+TEST(tf, getFrameStrings)
+{
+ Transformer mTR;
+
+
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,0,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/b", "/parent"));
+ std::vector <std::string> frames_string;
+ mTR.getFrameStrings(frames_string);
+ ASSERT_EQ(frames_string.size(), 2);
+ EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
+ EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
+
+
+ mTR.setTransform( Stamped<btTransform> (btTransform(btQuaternion(1,1,0), btVector3(0,0,0)), ros::Time().fromNSec(4000), "/other", "/frame"));
+
+ mTR.getFrameStrings(frames_string);
+ ASSERT_EQ(frames_string.size(), 4);
+ EXPECT_STREQ(frames_string[0].c_str(), std::string("/b").c_str());
+ EXPECT_STREQ(frames_string[1].c_str(), std::string("/parent").c_str());
+ EXPECT_STREQ(frames_string[2].c_str(), std::string("/other").c_str());
+ EXPECT_STREQ(frames_string[3].c_str(), std::string("/frame").c_str());
+
+}
+
int main(int argc, char **argv){
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
Modified: pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp
===================================================================
--- pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp 2009-07-03 01:01:45 UTC (rev 18246)
+++ pkg/trunk/stacks/visualization/rviz/src/rviz/robot/robot.cpp 2009-07-03 01:04:31 UTC (rev 18247)
@@ -231,10 +231,8 @@
M_NameToLink::iterator link_end = links_.end();
for ( ; link_it != link_end; ++link_it )
{
- ///@todo TODO: REMOVE THIS HACK
- const std::string& name = "/" + link_it->first;
- ///end HACK
-
+ const std::string& name = link_it->first;
+
RobotLink* info = link_it->second;
if ( std::find( frames.begin(), frames.end(), name ) == frames.end() )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-07-08 22:03:43
|
Revision: 18502
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18502&view=rev
Author: wattsk
Date: 2009-07-08 22:03:40 +0000 (Wed, 08 Jul 2009)
Log Message:
-----------
Changed runtime test to publish on diagnositics when batteries aren't there
Modified Paths:
--------------
pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-08 22:03:40 UTC (rev 18502)
@@ -47,6 +47,7 @@
latest_messages = {}
test_name = 'uninitialized'
+status_name = 'Runtime Test'
package = 'uninitialized'
last_runtime = rospy.get_time()
startup_delay = 5.0
@@ -60,31 +61,33 @@
str_map[val.label] = val.value;
str_map["name"]= status.name
str_map["message"] = status.message
+
+ # Store last time message was recorded
+ str_map["last_time"] = rospy.get_time()
return str_map
-def analyze(package, test_impl, params):
- # run the test
- return test_impl.test(latest_messages, params)
-def error_response(error):
- rospy.logerr( "Error: %s"% error)
-def warning_response(warning):
- rospy.logwarn("Warning: %s"% warning)
+def analyze(test_impl, params):
+ # run the test
+ return test_impl.test(latest_messages, params, status_name)
def callback(message, args):
for s in message.status:
latest_messages[s.name] = status_to_map(s)
- sys.stdout.flush()
+ #sys.stdout.flush()
execute_test(args)
def execute_test(args):
- global startup_delay
global last_runtime
+ global publisher
+
if rospy.get_time() < start_time + startup_delay:
rospy.logdebug("Waiting to for startup delay")
return
+
+ # Don't execute at greater than max frequency
time_step = rospy.get_time() - last_runtime
if 1.0 /time_step > options.max_freq:
return
@@ -92,17 +95,11 @@
last_runtime = rospy.get_time()
test_impl, params = args
- results = analyze(package, test_impl, params)
- if 'error' in results:
- for err in results['error']:
- error_response(err)
-
- if 'warn' in results:
- for warn in results['warn']:
- warning_response(warn)
+ msg = DiagnosticMessage()
+ msg.status = [analyze(test_impl, params)]
+ publisher.publish(msg)
-
def runtime_test(package, test_name):
# retrieve the test implementation
roslib.load_manifest(package)
@@ -118,12 +115,16 @@
# must be inited before reading parameters
rospy.init_node(NAME, anonymous=True)
-
# get it's parameters
params = rospy.get_param("~")
rospy.Subscriber("/diagnostics", DiagnosticMessage, callback, (test_impl, params))
+ # Publish in diagnostics the test results
+ global publisher
+ publisher = rospy.Publisher('/diagnostics', DiagnosticMessage)
+
+ # Always executes at greater than the min frequency
while not rospy.is_shutdown():
if rospy.get_time() - last_runtime > 1/options.min_freq:
execute_test((test_impl, params))
@@ -135,6 +136,9 @@
parser.add_option("--test", metavar="TEST_NAME",
dest="test_name", default='',
type="string", help="test name")
+ parser.add_option("--name", metavar="STATUS_NAME",
+ dest="status_name", default='Runtime Test',
+ type="string", help="Name of status message")
parser.add_option("--package", metavar="ROS_PACKAGE",
dest="package", default='runtime_monitor',
type="string", help="package test is in")
@@ -147,16 +151,17 @@
parser.add_option("--startup_delay", metavar="startup_delay",
dest="startup_delay", default='5.0',
type="float", help="Time to wait before Polling(Seconds)")
+
options, args = parser.parse_args()
-
# expected or default
package = options.package
- global startup_delay
startup_delay = options.startup_delay
+ status_name = options.status_name
+
if options.test_name:
test_name = options.test_name
else:
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -39,35 +39,43 @@
import rospy
from diagnostic_msgs.msg import *
+stat_dict = { 0: 'OK', 2: 'Error', 1: 'Warning' }
+def test(latest_msgs, parameters, test_name):
+ status = DiagnosticStatus()
+ status.name = 'Expected %s' % test_name
+ status.level = 0
+ status.message = 'OK'
+ status.strings = []
+ status.values = []
-
-def test(latest_status, parameters):
- #print latest_status
- results = {}
if "expected_present" in parameters:
for name in parameters["expected_present"]:
- if name in latest_status:
- #print "OK"
- pass
+ if name in latest_msgs and latest_msgs[name]["last_time"] - rospy.get_time() < 3:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Error'
+ status.level = max(status.level, 2)
else:
- #print name, "expected but not observed"
- if 'error' in results:
- results['error'].append("%s expected but not observed"%name)
- else:
- results['error'] = ["%s expected but not observed"%name]
+ msg = 'Missing - Error'
+ status.level = max(status.level, 2)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+
if "desired_present" in parameters:
for name in parameters["desired_present"]:
- if name in latest_status:
- #print "OK"
- pass
+ if name in latest_msgs and latest_msgs[name]["last_time"] - rospy.get_time() < 3:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Warning'
+ status.level = max(status.level, 1)
else:
- #print name, "expected but not observed"
- if 'warn' in results:
- results['warn'].append("%s desired but not observed"%name)
- else:
- results['warn'] = ["%s desired but not observed"%name]
- return results
+ msg = 'Missing - Warning'
+ status.level = max(status.level, 1)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+ status.message = stat_dict[status.level]
+
+ return status
+
Modified: pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -37,19 +37,27 @@
import sys
import rospy
-from robot_msgs.msg import *
+from diagnostic_msgs.msg import *
-def test(latest_status, parameters):
- #print latest_status
+def test(latest_status, parameters, test_name):
+ status = DiagnosticStatus()
+ status.name = test_name
+ status.level = 0
+ status.message = 'OK'
+ status.strings = []
+ status.values = []
for name in parameters["expected_present"]:
if name in latest_status:
- print "OK"
+ msg = 'OK'
else:
- print name, "expected but not observed"
+ msg = 'Error'
+ status.strings.append(DiagnosticString(label = name, value = msg))
+ return status
+
Modified: pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch
===================================================================
--- pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch 2009-07-08 22:03:40 UTC (rev 18502)
@@ -1,6 +1,6 @@
<launch>
-<group ns="/runtime">
- <node pkg="runtime_monitor" type="runtime_test" args="--test=expected --min_freq=1.0 --max_freq=2.0 --startup_delay=10.0" output="screen" name="battery_presence">
+<group ns="runtime">
+ <node pkg="runtime_monitor" type="runtime_test" args="--name=Batteries --test=expected --min_freq=1.0 --max_freq=2.0 --startup_delay=10.0" output="screen" name="battery_presence">
<rosparam command="load" file="$(find ocean_battery_driver)/battery_presence.yaml"/>
</node>
</group>
Modified: pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py
===================================================================
--- pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py 2009-07-08 21:58:51 UTC (rev 18501)
+++ pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/scripts/battery_notifier.py 2009-07-08 22:03:40 UTC (rev 18502)
@@ -52,7 +52,7 @@
self.mail_sent = False
def update(self, state):
- if((state.energy_remaining / state.energy_capacity) <= self.notify_limit):
+ if(state.energy_capacity == 0 or (state.energy_remaining / state.energy_capacity) <= self.notify_limit):
if not self.mail_sent and state.power_consumption < 0.0:
self.sendEmail()
self.mail_sent = True
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <j_b...@us...> - 2009-07-13 22:09:26
|
Revision: 18702
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18702&view=rev
Author: j_bohren
Date: 2009-07-13 22:09:16 +0000 (Mon, 13 Jul 2009)
Log Message:
-----------
Index: trex_pr2/nddl/nav/imports.nddl
Removed DetectPlugOnBase timeline from navigation nddl module
Index: trex_pr2/nddl/rcs/plugs/exports.nddl
Added DetectPlugOnBase to the exports of the plugs model
Index: trex_pr2/nddl/rcs/safety/exports.nddl
Removed DetectPlugOnBase from the exports of the safety model
Index: trex_pr2/src/pr2_adapters.cpp
Removed NoArgumentsAction, ShellCommand action, StopAction action from pr2_adapters
Index: trex_ros/include/trex_ros/components.h
Added inclusion guard
Index: trex_ros/CMakeLists.txt
Added trex_ros adapters executable
Updated all of the test launch files that launched the stopaction executable. This is now in trex_ros
Removed a duplicate NoArgumentsAction from pr2_robot_actions
>>>>>>> These actions moved into robot_actions might belong somewhere else, but they don't belong in pr2_robot_actions... they have nothing to do with the pr2.
Modified Paths:
--------------
pkg/trunk/stacks/trex/trex_pr2/CMakeLists.txt
pkg/trunk/stacks/trex/trex_pr2/include/trex_pr2/pr2_adapters.h
pkg/trunk/stacks/trex/trex_pr2/nddl/nav/imports.nddl
pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/plugs/exports.nddl
pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/safety/exports.nddl
pkg/trunk/stacks/trex/trex_pr2/src/pr2_adapters.cpp
pkg/trunk/stacks/trex/trex_pr2/src/stub_ros_container.cpp
pkg/trunk/stacks/trex/trex_pr2/test/baseline/real.launch
pkg/trunk/stacks/trex/trex_pr2/test/baseline/stubs.launch
pkg/trunk/stacks/trex/trex_pr2/test/doors/real.launch
pkg/trunk/stacks/trex/trex_pr2/test/master/domain_tests/master.1/real.launch
pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_arms/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_plugs/no_arms/run_real.launch
pkg/trunk/stacks/trex/trex_pr2/test/master/real.launch
pkg/trunk/stacks/trex/trex_pr2/test/stubs.launch
pkg/trunk/stacks/trex/trex_ros/CMakeLists.txt
pkg/trunk/stacks/trex/trex_ros/include/trex_ros/components.h
Added Paths:
-----------
pkg/trunk/stacks/common/robot_actions/msg/ShellCommandState.msg
pkg/trunk/stacks/common/robot_actions/msg/StopActionState.msg
Removed Paths:
-------------
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/NoArgumentsActionState.msg
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/ShellCommandState.msg
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/StopActionState.msg
Added: pkg/trunk/stacks/common/robot_actions/msg/ShellCommandState.msg
===================================================================
--- pkg/trunk/stacks/common/robot_actions/msg/ShellCommandState.msg (rev 0)
+++ pkg/trunk/stacks/common/robot_actions/msg/ShellCommandState.msg 2009-07-13 22:09:16 UTC (rev 18702)
@@ -0,0 +1,8 @@
+Header header
+robot_actions/ActionStatus status
+
+# Goal parameters
+std_msgs/String goal
+
+# Feedback parameters - same structure as goal
+std_msgs/String feedback
Added: pkg/trunk/stacks/common/robot_actions/msg/StopActionState.msg
===================================================================
--- pkg/trunk/stacks/common/robot_actions/msg/StopActionState.msg (rev 0)
+++ pkg/trunk/stacks/common/robot_actions/msg/StopActionState.msg 2009-07-13 22:09:16 UTC (rev 18702)
@@ -0,0 +1,8 @@
+Header header
+robot_actions/ActionStatus status
+
+# Goal parameter - the action name to stop
+std_msgs/String goal
+
+# Feedback parameters - none
+std_msgs/Empty feedback
Deleted: pkg/trunk/stacks/pr2/pr2_robot_actions/msg/NoArgumentsActionState.msg
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/msg/NoArgumentsActionState.msg 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/msg/NoArgumentsActionState.msg 2009-07-13 22:09:16 UTC (rev 18702)
@@ -1,13 +0,0 @@
-# Many actions take no goal, and give no feedback other than the success or failure. For these
-# we define a comon message format
-
-Header header
-
-# Status
-robot_actions/ActionStatus status
-
-# Goal
-std_msgs/Empty goal
-
-# Feedback
-std_msgs/Empty feedback
\ No newline at end of file
Deleted: pkg/trunk/stacks/pr2/pr2_robot_actions/msg/ShellCommandState.msg
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/msg/ShellCommandState.msg 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/msg/ShellCommandState.msg 2009-07-13 22:09:16 UTC (rev 18702)
@@ -1,8 +0,0 @@
-Header header
-robot_actions/ActionStatus status
-
-# Goal parameters
-std_msgs/String goal
-
-# Feedback parameters - same structure as goal
-std_msgs/String feedback
Deleted: pkg/trunk/stacks/pr2/pr2_robot_actions/msg/StopActionState.msg
===================================================================
--- pkg/trunk/stacks/pr2/pr2_robot_actions/msg/StopActionState.msg 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/pr2/pr2_robot_actions/msg/StopActionState.msg 2009-07-13 22:09:16 UTC (rev 18702)
@@ -1,8 +0,0 @@
-Header header
-robot_actions/ActionStatus status
-
-# Goal parameter - the action name to stop
-std_msgs/String goal
-
-# Feedback parameters - none
-std_msgs/Empty feedback
Modified: pkg/trunk/stacks/trex/trex_pr2/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/CMakeLists.txt 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/CMakeLists.txt 2009-07-13 22:09:16 UTC (rev 18702)
@@ -54,9 +54,6 @@
# stub_ros__container implements stub state publishers and actions to allow easy testing of the ROS adapters for the model
rospack_add_executable(bin/stub_ros_container src/stub_ros_container.cpp)
-# Implementation of actions that we actually use
-rospack_add_executable(bin/actions src/actions.cpp)
-
# Try the API for running the service calls to the executive
rospack_add_executable(test/ros_reactor_test test/ros_reactor_test.cpp)
Modified: pkg/trunk/stacks/trex/trex_pr2/include/trex_pr2/pr2_adapters.h
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/include/trex_pr2/pr2_adapters.h 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/include/trex_pr2/pr2_adapters.h 2009-07-13 22:09:16 UTC (rev 18702)
@@ -49,12 +49,12 @@
#include <plugs_msgs/PlugStow.h>
#include <robot_actions/action_runner.h>
#include <robot_actions/action.h>
+#include <robot_actions/StopActionState.h>
+#include <robot_actions/ShellCommandState.h>
#include <robot_actions/NoArgumentsActionState.h>
#include <pr2_robot_actions/DoorActionState.h>
#include <pr2_robot_actions/CheckPathState.h>
#include <pr2_robot_actions/NotifyDoorBlockedState.h>
-#include <pr2_robot_actions/ShellCommandState.h>
-#include <pr2_robot_actions/StopActionState.h>
#include <nav_robot_actions/MoveBaseState.h>
#include <pr2_robot_actions/RechargeState.h>
#include <pr2_robot_actions/DetectPlugOnBaseState.h>
Modified: pkg/trunk/stacks/trex/trex_pr2/nddl/nav/imports.nddl
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/nddl/nav/imports.nddl 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/nddl/nav/imports.nddl 2009-07-13 22:09:16 UTC (rev 18702)
@@ -13,11 +13,6 @@
// Mechanisms used for driving and tucking arms
HeadMechanism head_mechanism = new HeadMechanism(External);
-// Controller requirements for actions
-DetectPlugOnBase::Active{
- contained_by(laser_tilt_mechanism.use_laser_tilt_controller);
-}
-
// Controller and laser configuration requirements
MoveBase::Active{
contained_by(head_mechanism.use_head_controller);
Modified: pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/plugs/exports.nddl
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/plugs/exports.nddl 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/plugs/exports.nddl 2009-07-13 22:09:16 UTC (rev 18702)
@@ -1,7 +1,31 @@
/**
* This file specifies the interface for actions in the plugs domain
- * @note Detection of the plug on the base is specified in the safety area.
*/
+
+/**
+ * @brief This is a sensing action which will be used during navigation and recharging.
+ *
+ * It should return success if it found the plug, and the plug is on the base in the expected position.
+ * It will take as input an expected x, y, and th triple in the 'robot frame' and it will give feedback of the
+ * actual x and y values.
+ */
+class DetectPlugOnBase extends ROSAction {
+
+ predicate Active {}
+
+ predicate Inactive {
+ float time_stamp; // A double encoding is reliable
+ string frame_id;
+ bool stowed;
+ float x;
+ float y;
+ float z;
+ }
+
+ DetectPlugOnBase(Mode _mode){ super(_mode);}
+};
+
+
class PlugDomainAction extends ROSAction{
predicate Active{}
predicate Inactive{}
Modified: pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/safety/exports.nddl
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/safety/exports.nddl 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/nddl/rcs/safety/exports.nddl 2009-07-13 22:09:16 UTC (rev 18702)
@@ -4,29 +4,6 @@
*/
/**
- * @brief This is a sensing action which will be used during navigation and recharging.
- *
- * It should return success if it found the plug, and the plug is on the base in the expected position.
- * It will take as input an expected x, y, and th triple in the 'robot frame' and it will give feedback of the
- * actual x and y values.
- */
-class DetectPlugOnBase extends ROSAction {
-
- predicate Active {}
-
- predicate Inactive {
- float time_stamp; // A double encoding is reliable
- string frame_id;
- bool stowed;
- float x;
- float y;
- float z;
- }
-
- DetectPlugOnBase(Mode _mode){ super(_mode);}
-};
-
-/**
* @brief This is a general utility for tucking arms.
*/
class SafetyTuckArms extends ROSAction {
Modified: pkg/trunk/stacks/trex/trex_pr2/src/pr2_adapters.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/src/pr2_adapters.cpp 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/src/pr2_adapters.cpp 2009-07-13 22:09:16 UTC (rev 18702)
@@ -127,21 +127,7 @@
// Allocate Factory
TeleoReactor::ConcreteFactory<CheckPathAdapter> CheckPathAdapter_Factory("CheckPathAdapter");
- /***********************************************************************
- * @brief NoArgumentsAction action
- **********************************************************************/
- class NoArgumentsActionAdapter: public ROSActionAdapter<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty> {
- public:
- NoArgumentsActionAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSActionAdapter<std_msgs::Empty, robot_actions::NoArgumentsActionState, std_msgs::Empty>(agentName, configData){
- }
- };
-
- // Allocate Factory
- TeleoReactor::ConcreteFactory<NoArgumentsActionAdapter> NoArgumentsActionAdapter_Factory("NoArgumentsActionAdapter");
-
-
/***********************************************************************
* @@brief Recharge action
**********************************************************************/
@@ -188,58 +174,6 @@
/***********************************************************************
- * @brief ShellCommand action will take system commands in strings and
- * ship them for execution.
- **********************************************************************/
- class ShellCommandAdapter: public ROSActionAdapter<std_msgs::String, pr2_robot_actions::ShellCommandState, std_msgs::String> {
- public:
-
- ShellCommandAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSActionAdapter<std_msgs::String, pr2_robot_actions::ShellCommandState, std_msgs::String>(agentName, configData){
- }
-
- virtual void fillActiveObservationParameters(const std_msgs::String& msg, ObservationByValue* obs){
- obs->push_back("request", AdapterUtilities::toStringDomain(msg));
- }
-
- virtual void fillInactiveObservationParameters(const std_msgs::String& msg, ObservationByValue* obs){
- obs->push_back("response", AdapterUtilities::toStringDomain(msg));
- }
-
- void fillDispatchParameters(std_msgs::String& msg, const TokenId& goalToken){
- const StringDomain& dom = static_cast<const StringDomain&>(goalToken->getVariable("request")->lastDomain());
- AdapterUtilities::write(dom, msg);
- }
- };
-
- // Allocate Factory
- TeleoReactor::ConcreteFactory<ShellCommandAdapter> ShellCommandAdapter_Factory("ShellCommandAdapter");
-
- /***********************************************************************
- * @brief StopAction action
- **********************************************************************/
- class StopActionAdapter: public ROSActionAdapter<std_msgs::String, pr2_robot_actions::StopActionState, std_msgs::Empty> {
- public:
-
- StopActionAdapter(const LabelStr& agentName, const TiXmlElement& configData)
- : ROSActionAdapter<std_msgs::String, pr2_robot_actions::StopActionState, std_msgs::Empty>(agentName, configData){
- }
-
- virtual void fillActiveObservationParameters(const std_msgs::String& msg, ObservationByValue* obs){
- obs->push_back("action_name", AdapterUtilities::toStringDomain(msg));
- }
-
- void fillDispatchParameters(std_msgs::String& msg, const TokenId& goalToken){
- const StringDomain& dom = static_cast<const StringDomain&>(goalToken->getVariable("action_name")->lastDomain());
- AdapterUtilities::write(dom, msg);
- }
- };
-
- // Allocate Factory
- TeleoReactor::ConcreteFactory<StopActionAdapter> StopActionAdapter_Factory("StopActionAdapter");
-
-
- /***********************************************************************
* @brief
**********************************************************************/
class BaseStateAdapter: public ROSStateAdapter<robot_msgs::PoseStamped> {
Modified: pkg/trunk/stacks/trex/trex_pr2/src/stub_ros_container.cpp
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/src/stub_ros_container.cpp 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/src/stub_ros_container.cpp 2009-07-13 22:09:16 UTC (rev 18702)
@@ -354,7 +354,7 @@
trex_pr2::SimpleStubAction<std_msgs::String> shell_command("shell_command");
if (getComponentParam("/trex/enable_shell_command"))
- runner.connect<std_msgs::String, pr2_robot_actions::ShellCommandState, std_msgs::String>(shell_command);
+ runner.connect<std_msgs::String, robot_actions::ShellCommandState, std_msgs::String>(shell_command);
trex_pr2::SimpleStubAction<door_msgs::Door> notify_door_blocked("notify_door_blocked");
Modified: pkg/trunk/stacks/trex/trex_pr2/test/baseline/real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/baseline/real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/baseline/real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -42,5 +42,5 @@
<param name="/trex/enable_doors_grasp_handle" value="false"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="true" />
- <node pkg="trex_pr2" type="actions" respawn="true" />
+ <node pkg="trex_ros" type="actions" respawn="true" />
</launch>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/baseline/stubs.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/baseline/stubs.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/baseline/stubs.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -37,5 +37,5 @@
<param name="/trex/enable_doors_grasp_handle" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="false" />
- <node pkg="trex_pr2" type="actions" respawn="false" />
+ <node pkg="trex_ros" type="actions" respawn="false" />
</launch>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/doors/real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/doors/real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/doors/real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -10,7 +10,7 @@
<param name="/trex/enable_plugs_untuck_arms" value="true"/>
<param name="/trex/enable_set_laser_tilt" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="false" />
- <node pkg="trex_pr2" type="actions" respawn="false" />
+ <node pkg="trex_ros" type="actions" respawn="false" />
<param name="/trex/input_file" value="doorman.cfg"/>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/master/domain_tests/master.1/real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/master/domain_tests/master.1/real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/master/domain_tests/master.1/real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -40,7 +40,7 @@
<param name="/trex/enable_doors_touch_door" value="true"/>
<param name="/trex/enable_doors_grasp_handle" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="false" />
- <node pkg="trex_pr2" type="actions" respawn="false" />
+ <node pkg="trex_ros" type="actions" respawn="false" />
<param name="/trex/update_rate" value="1"/>
<param name="/trex/input_file" value="master.cfg"/>
@@ -48,4 +48,4 @@
<param name="/trex/time_limit" value="20000"/>
<param name="/trex/log_dir" value="$(find trex_pr2)/logs"/>
<param name="/trex/play_back" value="0"/>
-</launch>
\ No newline at end of file
+</launch>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_arms/run_real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_arms/run_real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_arms/run_real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -39,7 +39,7 @@
<param name="/trex/enable_doors_touch_door" value="true"/>
<param name="/trex/enable_doors_grasp_handle" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="true" />
- <node pkg="trex_pr2" type="actions" respawn="true" />
+ <node pkg="trex_ros" type="actions" respawn="true" />
<param name="/trex/update_rate" value="10"/>
<param name="/trex/input_file" value="master.cfg"/>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_plugs/no_arms/run_real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_plugs/no_arms/run_real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/master/m2/no_plugs/no_arms/run_real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -39,7 +39,7 @@
<param name="/trex/enable_doors_touch_door" value="true"/>
<param name="/trex/enable_doors_grasp_handle" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="true" />
- <node pkg="trex_pr2" type="actions" respawn="true" />
+ <node pkg="trex_ros" type="actions" respawn="true" />
<param name="/trex/update_rate" value="10"/>
<param name="/trex/input_file" value="master.cfg"/>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/master/real.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/master/real.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/master/real.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -42,7 +42,7 @@
<param name="/trex/enable_doors_touch_door" value="false"/>
<param name="/trex/enable_doors_grasp_handle" value="false"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="true" />
- <node pkg="trex_pr2" type="actions" respawn="true" />
+ <node pkg="trex_ros" type="actions" respawn="true" />
<param name="/trex/update_rate" value="10"/>
<param name="/trex/input_file" value="master.cfg"/>
Modified: pkg/trunk/stacks/trex/trex_pr2/test/stubs.launch
===================================================================
--- pkg/trunk/stacks/trex/trex_pr2/test/stubs.launch 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_pr2/test/stubs.launch 2009-07-13 22:09:16 UTC (rev 18702)
@@ -35,5 +35,5 @@
<param name="/trex/enable_doors_touch_door" value="true"/>
<param name="/trex/enable_doors_grasp_handle" value="true"/>
<node pkg="trex_pr2" type="stub_ros_container" respawn="false" />
- <node pkg="trex_pr2" type="actions" respawn="false" />
+ <node pkg="trex_ros" type="actions" respawn="false" />
</launch>
Modified: pkg/trunk/stacks/trex/trex_ros/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/CMakeLists.txt 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_ros/CMakeLists.txt 2009-07-13 22:09:16 UTC (rev 18702)
@@ -8,7 +8,8 @@
rospack_add_boost_directories()
-set(TREX_FILES src/main.cpp
+set(TREX_FILES src/adapters.cpp
+ src/main.cpp
src/ros_reactor.cpp
src/executive.cpp
src/monitor.cpp
@@ -28,3 +29,6 @@
# and dispense shutdown commands to highlevel controllers (deactivation) if pings are
# not received in time
rospack_add_executable(bin/trexwatchdog src/watch_dog.cpp)
+
+# Implementation of actions
+rospack_add_executable(bin/actions src/actions.cpp)
Modified: pkg/trunk/stacks/trex/trex_ros/include/trex_ros/components.h
===================================================================
--- pkg/trunk/stacks/trex/trex_ros/include/trex_ros/components.h 2009-07-13 22:08:13 UTC (rev 18701)
+++ pkg/trunk/stacks/trex/trex_ros/include/trex_ros/components.h 2009-07-13 22:09:16 UTC (rev 18702)
@@ -1,6 +1,9 @@
/**
* @brief Collection of various components to use
*/
+#ifndef TREX_ROS_COMPONENTS_H_
+#define TREX_ROS_COMPONENTS_H_
+
#include "ros/ros.h"
#include "Constraint.hh"
@@ -90,3 +93,5 @@
const TokenId _source_token;
};
}
+
+#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-07-15 17:26:22
|
Revision: 18854
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18854&view=rev
Author: mariusmuja
Date: 2009-07-15 17:26:19 +0000 (Wed, 15 Jul 2009)
Log Message:
-----------
Fixes to door_handle_detector and outlet_spotting so that thei can be used with the double stereo setup
Modified Paths:
--------------
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-07-15 17:15:45 UTC (rev 18853)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-07-15 17:26:19 UTC (rev 18854)
@@ -234,12 +234,12 @@
void subscribeStereoData()
{
- left_image_sub_ = nh_.subscribe("stereo/left/image_rect", 1, sync_.synchronize(&HandleDetector::leftImageCallback, this));
-// right_caminfo_sub_ = nh_.subscribe("stereo/right/cam_info", 1, sync_.synchronize(&HandleDetector::rightCamInfoCallback, this));
- disparity_sub_ = nh_.subscribe("stereo/disparity", 1, sync_.synchronize(&HandleDetector::disparityImageCallback, this));
- cloud_sub_ = nh_.subscribe("stereo/cloud", 1, sync_.synchronize(&HandleDetector::cloudCallback, this));
- dispinfo_sub_ = nh_.subscribe("stereo/disparity_info", 1, sync_.synchronize(&HandleDetector::dispinfoCallback, this));
-// stereoinfo_sub_ = nh_.subscribe("stereo/stereo_info", 1, sync_.synchronize(&HandleDetector::stereoinfoCallback, this));
+ left_image_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/left/image_rect", 1, sync_.synchronize(&HandleDetector::leftImageCallback, this));
+// right_caminfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/right/cam_info", 1, sync_.synchronize(&HandleDetector::rightCamInfoCallback, this));
+ disparity_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity", 1, sync_.synchronize(&HandleDetector::disparityImageCallback, this));
+ cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, sync_.synchronize(&HandleDetector::cloudCallback, this));
+ dispinfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity_info", 1, sync_.synchronize(&HandleDetector::dispinfoCallback, this));
+// stereoinfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/stereo_info", 1, sync_.synchronize(&HandleDetector::stereoinfoCallback, this));
}
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp 2009-07-15 17:15:45 UTC (rev 18853)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp 2009-07-15 17:26:19 UTC (rev 18854)
@@ -262,11 +262,11 @@
{
- left_image_sub_ = nh_.subscribe("stereo/left/image_rect", 1, sync_.synchronize(&OutletSpotting::leftImageCallback, this));
- left_caminfo_sub_ = nh_.subscribe("stereo/left/cam_info", 1, sync_.synchronize(&OutletSpotting::leftCamInfoCallback, this));
- disparity_sub_ = nh_.subscribe("stereo/disparity", 1, sync_.synchronize(&OutletSpotting::disparityImageCallback, this));
- cloud_sub_ = nh_.subscribe("stereo/cloud", 1, sync_.synchronize(&OutletSpotting::cloudCallback, this));
- dispinfo_sub_ = nh_.subscribe("stereo/disparity_info", 1, sync_.synchronize(&OutletSpotting::dispinfoCallback, this));
+ left_image_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/left/image_rect", 1, sync_.synchronize(&OutletSpotting::leftImageCallback, this));
+ left_caminfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/left/cam_info", 1, sync_.synchronize(&OutletSpotting::leftCamInfoCallback, this));
+ disparity_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity", 1, sync_.synchronize(&OutletSpotting::disparityImageCallback, this));
+ cloud_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/cloud", 1, sync_.synchronize(&OutletSpotting::cloudCallback, this));
+ dispinfo_sub_ = nh_.subscribe(nh_.resolveName("stereo")+"/disparity_info", 1, sync_.synchronize(&OutletSpotting::dispinfoCallback, this));
base_scan_sub_ = nh_.subscribe(base_scan_topic_, 1, &OutletSpotting::baseScanCallback, this);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mar...@us...> - 2009-07-15 17:56:41
|
Revision: 18861
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18861&view=rev
Author: mariusmuja
Date: 2009-07-15 17:56:38 +0000 (Wed, 15 Jul 2009)
Log Message:
-----------
Remaped stereo namespace to narrow_stereo for outlet_spotting and door_handle_detection in milestone2 launch files.
Modified Paths:
--------------
pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/door_handle_detector.xml
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/outlet_coarse_detection/outlet_coarse_detection.launch
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/door_handle_detector.xml
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/door_handle_detector.xml 2009-07-15 17:56:35 UTC (rev 18860)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/launch/door_handle_detector.xml 2009-07-15 17:56:38 UTC (rev 18861)
@@ -3,10 +3,12 @@
<!-- Parameters -->
<include file="$(find door_handle_detector)/launch/door_handle_detector_params.xml"/>
-<!-- Handle detector Camera -->
- <node pkg="door_handle_detector" name="handle_detector_vision" type="handle_detector_vision" respawn="false" output="screen">
- </node>
+ <!-- Handle detector Camera -->
+ <node pkg="door_handle_detector" name="handle_detector_vision" type="handle_detector_vision" respawn="false" output="screen">
+ <remap from="stereo" to="narrow_stereo"/>
+ </node>
+
<!-- Handle detector Laser -->
<node pkg="door_handle_detector" type="handle_detector_laser" name="handle_detector_laser" respawn="true" output="screen" />
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/outlet_coarse_detection/outlet_coarse_detection.launch
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/outlet_coarse_detection/outlet_coarse_detection.launch 2009-07-15 17:56:35 UTC (rev 18860)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/launch/outlet_coarse_detection/outlet_coarse_detection.launch 2009-07-15 17:56:38 UTC (rev 18861)
@@ -2,6 +2,7 @@
<launch>
<node pkg="outlet_detection" name="outlet_spotting" type="outlet_spotting2" respawn="true" output="screen">
+ <remap from="stereo" to="narrow_stereo" />
<param name="display" type="bool" value="False"/>
<param name="save_patches" type="bool" value="False"/>
<param name="base_scan_topic" value="base_scan_marking" />
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-07-16 02:24:30
|
Revision: 18951
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18951&view=rev
Author: sfkwc
Date: 2009-07-16 02:24:28 +0000 (Thu, 16 Jul 2009)
Log Message:
-----------
svn ignores
Property Changed:
----------------
pkg/trunk/stacks/common/manipulation_msgs/msg/
pkg/trunk/stacks/common/robot_actions/msg/
pkg/trunk/stacks/common_msgs/diagnostic_msgs/
pkg/trunk/stacks/common_msgs/diagnostic_msgs/msg/
pkg/trunk/stacks/common_msgs/diagnostic_msgs/srv/
pkg/trunk/stacks/common_msgs/nav_msgs/
pkg/trunk/stacks/common_msgs/nav_msgs/msg/
pkg/trunk/stacks/common_msgs/robot_msgs/msg/
pkg/trunk/stacks/common_msgs/robot_srvs/
pkg/trunk/stacks/common_msgs/robot_srvs/srv/
pkg/trunk/stacks/common_msgs/sensor_msgs/
pkg/trunk/stacks/common_msgs/sensor_msgs/msg/
pkg/trunk/stacks/joystick_drivers/joy/msg/
pkg/trunk/stacks/mechanism/mechanism_msgs/msg/
pkg/trunk/stacks/mechanism/mechanism_msgs/srv/
pkg/trunk/stacks/pr2/pr2_robot_actions/msg/
pkg/trunk/stacks/pr2_drivers/ethercat_hardware/msg/
pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg/
pkg/trunk/stacks/pr2_drivers/pr2_srvs/srv/
pkg/trunk/stacks/visualization_core/visualization_msgs/msg/
Property changes on: pkg/trunk/stacks/common/manipulation_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/common/robot_actions/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/common_msgs/diagnostic_msgs
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
src
Property changes on: pkg/trunk/stacks/common_msgs/diagnostic_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
cpp
lisp
oct
Property changes on: pkg/trunk/stacks/common_msgs/diagnostic_msgs/srv
___________________________________________________________________
Added: svn:ignore
+ java
cpp
lisp
oct
Property changes on: pkg/trunk/stacks/common_msgs/nav_msgs
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
src
Property changes on: pkg/trunk/stacks/common_msgs/nav_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
cpp
lisp
oct
Property changes on: pkg/trunk/stacks/common_msgs/robot_msgs/msg
___________________________________________________________________
Modified: svn:ignore
- cpp
lisp
oct
+ cpp
lisp
oct
java
Property changes on: pkg/trunk/stacks/common_msgs/robot_srvs
___________________________________________________________________
Modified: svn:ignore
- src
+ .build_version
.rosgcov_files
bin
src
Property changes on: pkg/trunk/stacks/common_msgs/robot_srvs/srv
___________________________________________________________________
Added: svn:ignore
+ java
cpp
lisp
oct
Property changes on: pkg/trunk/stacks/common_msgs/sensor_msgs
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
src
Property changes on: pkg/trunk/stacks/common_msgs/sensor_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
cpp
lisp
oct
Property changes on: pkg/trunk/stacks/joystick_drivers/joy/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/mechanism/mechanism_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/mechanism/mechanism_msgs/srv
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/pr2/pr2_robot_actions/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/pr2_drivers/ethercat_hardware/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/pr2_drivers/pr2_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/pr2_drivers/pr2_srvs/srv
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/visualization_core/visualization_msgs/msg
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-07-16 02:26:25
|
Revision: 18952
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18952&view=rev
Author: sfkwc
Date: 2009-07-16 02:26:19 +0000 (Thu, 16 Jul 2009)
Log Message:
-----------
svn ignores
Property Changed:
----------------
pkg/trunk/stacks/geometry/tf/msg/
pkg/trunk/stacks/geometry/tf/srv/
pkg/trunk/stacks/imu_drivers/imu_node/srv/
Property changes on: pkg/trunk/stacks/geometry/tf/msg
___________________________________________________________________
Modified: svn:ignore
- cpp
lisp
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/geometry/tf/srv
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
Property changes on: pkg/trunk/stacks/imu_drivers/imu_node/srv
___________________________________________________________________
Added: svn:ignore
+ java
lisp
oct
cpp
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <sf...@us...> - 2009-07-16 02:35:32
|
Revision: 18956
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18956&view=rev
Author: sfkwc
Date: 2009-07-16 02:35:30 +0000 (Thu, 16 Jul 2009)
Log Message:
-----------
svn ignores
Property Changed:
----------------
pkg/trunk/stacks/common/filters/
pkg/trunk/stacks/common/manipulation_msgs/
pkg/trunk/stacks/common/robot_actions/
pkg/trunk/stacks/common/tinyxml/
pkg/trunk/stacks/geometry/angles/
pkg/trunk/stacks/geometry/bullet/
pkg/trunk/stacks/imu_drivers/3dmgx2_driver/
pkg/trunk/stacks/imu_drivers/imu_node/
pkg/trunk/stacks/mechanism/hardware_interface/
pkg/trunk/stacks/mechanism/kdl_parser/
pkg/trunk/stacks/mechanism/mechanism_control/
pkg/trunk/stacks/mechanism/mechanism_model/
pkg/trunk/stacks/mechanism/mechanism_msgs/
Property changes on: pkg/trunk/stacks/common/filters
___________________________________________________________________
Modified: svn:ignore
- bin
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/common/manipulation_msgs
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/common/robot_actions
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/common/tinyxml
___________________________________________________________________
Modified: svn:ignore
- build
libtinyxml.so
+ .build_version
.rosgcov_files
bin
build
libtinyxml.so
Property changes on: pkg/trunk/stacks/geometry/angles
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/geometry/bullet
___________________________________________________________________
Modified: svn:ignore
- bullet
bullet_svn
+ .build_version
.rosgcov_files
bin
bullet
bullet_svn
Property changes on: pkg/trunk/stacks/imu_drivers/3dmgx2_driver
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/imu_drivers/imu_node
___________________________________________________________________
Modified: svn:ignore
- src
+ .build_version
.rosgcov_files
bin
src
Property changes on: pkg/trunk/stacks/mechanism/hardware_interface
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/mechanism/kdl_parser
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/mechanism/mechanism_control
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/mechanism/mechanism_model
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
Property changes on: pkg/trunk/stacks/mechanism/mechanism_msgs
___________________________________________________________________
Added: svn:ignore
+ .build_version
.rosgcov_files
bin
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <wa...@us...> - 2009-07-20 06:36:54
|
Revision: 19185
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19185&view=rev
Author: wattsk
Date: 2009-07-20 06:36:45 +0000 (Mon, 20 Jul 2009)
Log Message:
-----------
Moved runtime_test to new package, diagnostic_test, and updated expected battery test. Upgraded CSV exporter in diagnostics_analysis
Modified Paths:
--------------
pkg/trunk/stacks/hardware_test/diagnostics_analysis/csv/export_csv.py
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/battery_presence.yaml
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/expected_batteries.launch
pkg/trunk/stacks/pr2_drivers/ocean_battery_driver/manifest.xml
Added Paths:
-----------
pkg/trunk/stacks/hardware_test/diagnostic_test/
pkg/trunk/stacks/hardware_test/diagnostic_test/CMakeLists.txt
pkg/trunk/stacks/hardware_test/diagnostic_test/Makefile
pkg/trunk/stacks/hardware_test/diagnostic_test/manifest.xml
pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/
pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py
pkg/trunk/stacks/hardware_test/diagnostic_test/src/
pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/
pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/__init__.py
pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/expected.py
pkg/trunk/stacks/hardware_test/diagnostic_test/test/
pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_publisher.py
pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_status.yaml
pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_tester.launch
pkg/trunk/stacks/hardware_test/diagnostic_test/test/publish_test.launch
pkg/trunk/stacks/hardware_test/diagnostics_analysis/output/
Removed Paths:
-------------
pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/test_test.py
pkg/trunk/stacks/hardware_test/runtime_monitor/test.yaml
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/CMakeLists.txt
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/CMakeLists.txt (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/CMakeLists.txt 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(diagnostic_test)
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/Makefile
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/Makefile (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/Makefile 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/manifest.xml
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/manifest.xml (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/manifest.xml 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,20 @@
+<package>
+<description brief='A program to test and analyze diagnostics on robot'>
+
+Monitors diagnostics by testing them. Checking battery prescence, checking motors are OK, etc.
+
+</description>
+<author>Kevin Watts</author>
+<license>BSD</license>
+<review status="proposal cleared" notes=""/>
+<url>http://pr.willowgarage.com/wiki/diagnostic_test</url>
+<export>
+<cpp cflags="-I${prefix}/include " lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
+</export>
+<depend package="rospy"/>
+<depend package="diagnostic_msgs"/>
+<sysdepend os="ubuntu" version="7.04-feisty" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="8.04-feisty" package="python-wxgtk2.8"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="libwxgtk2.8-dev"/>
+<sysdepend os="ubuntu" version="8.04-hardy" package="python-wxgtk2.8"/>
+</package>
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,175 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+## A basic node to monitor diagnostics for expected status
+
+PKG = 'diagnostic_test'
+
+import roslib; roslib.load_manifest(PKG)
+
+import sys, time
+import rospy
+from diagnostic_msgs.msg import DiagnosticMessage, DiagnosticStatus, DiagnosticValue, DiagnosticString
+
+NAME = 'diagnostic_test'
+
+latest_messages = {}
+test_name = 'uninitialized'
+package = 'uninitialized'
+last_runtime = 0
+startup_delay = 5.0
+start_time = 0
+
+## Records status as dictionary by labels for strings, values
+## Also stores last time of message, allows stale checks.
+def status_to_map(status):
+ str_map = {}
+ for s in status.strings:
+ str_map[s.label] = s.value;
+ for val in status.values:
+ str_map[val.label] = val.value;
+ str_map["name"]= status.name
+ str_map["message"] = status.message
+ str_map["level"] = status.level
+
+ # Store last time message was recorded
+ str_map["last_time"] = rospy.get_time()
+ return str_map
+
+## The test takes the latest_messages dictionary, the private parameters
+## and the status name (the name of the DiagnosticStatus message
+## it publishes)
+def analyze(test_impl, params):
+ return test_impl.test(latest_messages, params)
+
+def callback(message, args):
+ for s in message.status:
+ latest_messages[s.name] = status_to_map(s)
+ execute_test(args)
+
+## Performs designated test using latest messages. Publishes to /diagnostics
+## Never tests greater than max frequency.
+##@param args (Test implementation, private parameters for test)
+def execute_test(args):
+ global publisher
+ global last_runtime
+
+ if rospy.get_time() < start_time + startup_delay:
+ rospy.logdebug("Waiting to for startup delay")
+ return
+
+ # Don't execute at greater than max frequency
+ time_step = rospy.get_time() - last_runtime
+ if 1.0 / time_step > options.max_freq:
+ return
+ else:
+ last_runtime = rospy.get_time()
+
+ test_impl, params = args
+
+ msg = DiagnosticMessage()
+ msg.status = [analyze(test_impl, params)]
+ publisher.publish(msg)
+
+## Starts up test, subscribes and publishes to diagnostics
+def diagnostic_test(package, test_name):
+ # retrieve the test implementation
+ roslib.load_manifest(package)
+ __import__("%s.%s"%(package, test_name))
+ try:
+ pypkg = sys.modules[package]
+ except KeyError:
+ print >> sys.stderr, "ERROR: cannot locate test package %s"%package
+ rospy.logerr("cannot locate test package %s"%package)
+ sys.exit(1)
+ test_impl = getattr(pypkg, test_name)
+
+ # must be inited before reading parameters
+ rospy.init_node(NAME, anonymous=True)
+ global start_time, last_runtime
+ last_runtime = rospy.get_time()
+ start_time = rospy.get_time()
+
+ # get it's parameters
+ params = rospy.get_param("~")
+
+ rospy.Subscriber("/diagnostics", DiagnosticMessage, callback, (test_impl, params))
+
+ # Publish results in diagnostics
+ global publisher
+ publisher = rospy.Publisher('/diagnostics', DiagnosticMessage)
+
+ # Always executes at greater than the min frequency
+ while not rospy.is_shutdown():
+ if rospy.get_time() - last_runtime > 1/options.min_freq:
+ execute_test((test_impl, params))
+ time.sleep(0.5/options.min_freq)
+
+if __name__ == '__main__':
+ from optparse import OptionParser
+ parser = OptionParser(usage="usage: %prog [options]", prog='runtime_test')
+ parser.add_option("--test", metavar="TEST_NAME",
+ dest="test_name", default='',
+ type="string", help="test name")
+ parser.add_option("--package", metavar="ROS_PACKAGE",
+ dest="package", default='diagnostic_test',
+ type="string", help="package test is in")
+ parser.add_option("--min_freq", metavar="MIN_FREQ",
+ dest="min_freq", default='0.5',
+ type="float", help="Minimum Execution Frequency(Hz)")
+ parser.add_option("--max_freq", metavar="MAX_FREQ",
+ dest="max_freq", default='1.0',
+ type="float", help="Maximum Execution Frequency(Hz)")
+ parser.add_option("--startup_delay", metavar="STARTUP_DELAY",
+ dest="startup_delay", default='10.0',
+ type="float", help="Time to wait before Polling(Seconds)")
+
+
+ options, args = parser.parse_args()
+
+
+ # expected or default
+ package = options.package
+ startup_delay = options.startup_delay
+
+ if options.test_name:
+ test_name = options.test_name
+ else:
+ parser.error("Diagnostic test must be given test to run")
+
+ try:
+ diagnostic_test(package, options.test_name)
+ except KeyboardInterrupt, e:
+ pass
+ print "exiting"
Property changes on: pkg/trunk/stacks/hardware_test/diagnostic_test/scripts/diagnostic_test.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/__init__.py
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/__init__.py (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/__init__.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1 @@
+
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/expected.py
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/expected.py (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/src/diagnostic_test/expected.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,84 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+import rospy
+from diagnostic_msgs.msg import *
+
+stat_dict = { 0: 'OK', 1: 'Warning', 2: 'Error' }
+
+def test(latest_msgs, parameters):
+ status = DiagnosticStatus()
+ status.name = 'Expected Test'
+ status.level = 0
+ status.message = 'OK'
+ status.strings = []
+ status.values = []
+
+ if "name" in parameters:
+ status.name = 'Expected %s' % parameters["name"]
+
+ timeout = 3
+ if "timeout" in parameters:
+ timeout = float(parameters["timeout"])
+
+ if "expected_present" in parameters:
+ for name in parameters["expected_present"]:
+ if name in latest_msgs and rospy.get_time() - latest_msgs[name]["last_time"] < timeout:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Error'
+ status.level = max(status.level, 2)
+ else:
+ msg = 'Missing - Error'
+ status.level = max(status.level, 2)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+
+
+ if "desired_present" in parameters:
+ for name in parameters["desired_present"]:
+ if name in latest_msgs and rospy.get_time() - latest_msgs[name]["last_time"] < timeout:
+ msg = 'OK'
+ elif name in latest_msgs:
+ msg = 'Stale - Warning'
+ status.level = max(status.level, 1)
+ else:
+ msg = 'Missing - Warning'
+ status.level = max(status.level, 1)
+ status.strings.append(DiagnosticString(label = name, value = msg))
+
+ status.message = stat_dict[status.level]
+
+ return status
+
+
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_publisher.py
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_publisher.py (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_publisher.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,70 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+
+# Author: Kevin Watts
+
+# Performs testing on diagnostic_test scripts
+
+import roslib
+roslib.load_manifest('diagnostic_test')
+
+import sys
+import rospy
+from diagnostic_msgs.msg import *
+import time
+
+def make_status_msg(name):
+ status = DiagnosticStatus()
+ status.name = name
+ status.level = 0
+ status.message = 'OK'
+ return status
+
+if __name__ == '__main__':
+ rospy.init_node('diagnostic_test_tester')
+
+ pub = rospy.Publisher('/diagnostics', DiagnosticMessage)
+
+ while not rospy.is_shutdown():
+ msg = DiagnosticMessage()
+
+ msg.status.append(make_status_msg('Expected 1'))
+ msg.status.append(make_status_msg('Expected 2'))
+ msg.status.append(make_status_msg('Desired 1'))
+
+ pub.publish(msg)
+ time.sleep(1.0)
+
+
+
Property changes on: pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_publisher.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_status.yaml
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_status.yaml (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_status.yaml 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,13 @@
+expected_present: [
+ 'Expected 1',
+ 'Expected 2',
+ 'Expected 3']
+
+desired_present: [
+ 'Desired 1',
+ 'Desired 2',
+ 'Desired 3']
+
+timeout: 3
+
+name: Tester
\ No newline at end of file
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_tester.launch
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_tester.launch (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/test/expected_tester.launch 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,7 @@
+<launch>
+ <group ns="diagnostic_test">
+ <node pkg="diagnostic_test" type="diagnostic_test.py" args="--test=expected --min_freq=1.0 --max_freq=2.0 --startup_delay=10.0" output="screen" name="expected_test" >
+ <rosparam command="load" file="$(find diagnostic_test)/test/expected_status.yaml" />
+ </node>
+ </group>
+</launch>
\ No newline at end of file
Added: pkg/trunk/stacks/hardware_test/diagnostic_test/test/publish_test.launch
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostic_test/test/publish_test.launch (rev 0)
+++ pkg/trunk/stacks/hardware_test/diagnostic_test/test/publish_test.launch 2009-07-20 06:36:45 UTC (rev 19185)
@@ -0,0 +1,3 @@
+<launch>
+ <node pkg="diagnostic_test" type="expected_publisher.py" />
+</launch>
\ No newline at end of file
Modified: pkg/trunk/stacks/hardware_test/diagnostics_analysis/csv/export_csv.py
===================================================================
--- pkg/trunk/stacks/hardware_test/diagnostics_analysis/csv/export_csv.py 2009-07-20 01:24:10 UTC (rev 19184)
+++ pkg/trunk/stacks/hardware_test/diagnostics_analysis/csv/export_csv.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -65,12 +65,15 @@
stats[name] = {}
stats[name]['string_fields'] = [s.label for s in status.strings]
stats[name]['float_fields'] = [s.label for s in status.values]
+ stats[name]['level'] = status.level
+ stats[name]['message'] = status.message
file_name = name.replace(' ', '_').replace('(', '').replace(')', '').replace('/', '__').replace('.', '').replace('#', '')
stats[name]['file'] = file(output_dir + '/' + file_name + '.csv', 'w')
fields = stats[name]['string_fields'] + stats[name]['float_fields'];
- stats[name]['file'].write(', '.join(['timestamp'] + [f.replace(',','') for f in fields]) + '\n')
+ stats[name]['file'].write(', '.join(['timestamp'] + ['Level', 'Message'] +
+ [f.replace(',','') for f in fields]) + '\n')
# Need stuff for different string fields
# Store as dictionary, then convert to CSV?
@@ -84,8 +87,11 @@
#print "ERROR, mismatched field names in component %s. Label: %s" %(name, s.label)
#return stats
continue
+
# Should make time machine readable better
- stats[name]['file'].write(', '.join([time.asctime(t)] + [s.value.replace('\n', ' ').replace(',','') for s in status.strings] + [str(s.value) for s in status.values]) + '\n')
+ stats[name]['file'].write(', '.join([time.asctime(t)] + [str(status.level), status.message] +
+ [s.value.replace('\n', ' ').replace(',','') for s in status.strings] +
+ [str(s.value) for s in status.values]) + '\n')
def output(stats):
Deleted: pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-20 01:24:10 UTC (rev 19184)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/scripts/runtime_test 2009-07-20 06:36:45 UTC (rev 19185)
@@ -1,176 +0,0 @@
-#!/usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2008, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above
-# copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other materials provided
-# with the distribution.
-# * Neither the name of the Willow Garage nor the names of its
-# contributors may be used to endorse or promote products derived
-# from this software without specific prior written permission.
-#
-# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
-# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
-# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
-# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
-# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
-# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
-# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
-# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
-# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
-# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-# POSSIBILITY OF SUCH DAMAGE.
-#
-
-## A basic node to monitor diagnostics for expected status
-
-import roslib; roslib.load_manifest('runtime_monitor')
-
-import sys, time
-import rospy
-from diagnostic_msgs.msg import DiagnosticMessage, DiagnosticStatus, DiagnosticValue, DiagnosticString
-
-NAME = 'runtime_test'
-
-# must initialize a node before we call get_time()
-rospy.init_node(NAME, anonymous=True)
-
-latest_messages = {}
-test_name = 'uninitialized'
-status_name = 'Runtime Test'
-package = 'uninitialized'
-last_runtime = 0
-startup_delay = 5.0
-start_time = rospy.get_time()
-
-## Records status as dictionary by labels for strings, values
-## Also stores last time of message, allows stale checks.
-def status_to_map(status):
- str_map = {}
- for s in status.strings:
- str_map[s.label] = s.value;
- for val in status.values:
- str_map[val.label] = val.value;
- str_map["name"]= status.name
- str_map["message"] = status.message
-
- # Store last time message was recorded
- str_map["last_time"] = rospy.get_time()
- return str_map
-
-## The test takes the latest_messages dictionary, the private parameters
-## and the status name (the name of the DiagnosticStatus message
-## it publishes)
-def analyze(test_impl, params):
- return test_impl.test(latest_messages, params, status_name)
-
-def callback(message, args):
- for s in message.status:
- latest_messages[s.name] = status_to_map(s)
- execute_test(args)
-
-def execute_test(args):
- global publisher
- global last_runtime
-
- if rospy.get_time() < start_time + startup_delay:
- rospy.logdebug("Waiting to for startup delay")
- return
-
- # Don't execute at greater than max frequency
- time_step = rospy.get_time() - last_runtime
- if 1.0 / time_step > options.max_freq:
- return
- else:
- last_runtime = rospy.get_time()
-
- test_impl, params = args
-
- msg = DiagnosticMessage()
- msg.status = [analyze(test_impl, params)]
- publisher.publish(msg)
-
-def runtime_test(package, test_name):
- # retrieve the test implementation
- roslib.load_manifest(package)
- __import__("%s.%s"%(package, test_name))
- try:
- pypkg = sys.modules[package]
- except KeyError:
- print >> sys.stderr, "ERROR: cannot locate test package %s"%package
- rospy.logerr("cannot locate test package %s"%package)
- sys.exit(1)
- test_impl = getattr(pypkg, test_name)
-
- # must be inited before reading parameters
- rospy.init_node(NAME, anonymous=True)
- global last_runtime
- last_runtime = rospy.get_time()
-
- # get it's parameters
- params = rospy.get_param("~")
-
- rospy.Subscriber("/diagnostics", DiagnosticMessage, callback, (test_impl, params))
-
- # Publish results in diagnostics
- global publisher
- publisher = rospy.Publisher('/diagnostics', DiagnosticMessage)
-
- # Always executes at greater than the min frequency
- while not rospy.is_shutdown():
- if rospy.get_time() - last_runtime > 1/options.min_freq:
- execute_test((test_impl, params))
- time.sleep(0.5/options.min_freq)
-
-if __name__ == '__main__':
- from optparse import OptionParser
- parser = OptionParser(usage="usage: %prog [options]", prog='runtime_test')
- parser.add_option("--test", metavar="TEST_NAME",
- dest="test_name", default='',
- type="string", help="test name")
- parser.add_option("--name", metavar="STATUS_NAME",
- dest="status_name", default='Runtime Test',
- type="string", help="Name of status message")
- parser.add_option("--package", metavar="ROS_PACKAGE",
- dest="package", default='runtime_monitor',
- type="string", help="package test is in")
- parser.add_option("--min_freq", metavar="min_frequency",
- dest="min_freq", default='0.5',
- type="float", help="Minimum Execution Frequency(Hz)")
- parser.add_option("--max_freq", metavar="max_frequency",
- dest="max_freq", default='1.0',
- type="float", help="Maximum Execution Frequency(Hz)")
- parser.add_option("--startup_delay", metavar="startup_delay",
- dest="startup_delay", default='5.0',
- type="float", help="Time to wait before Polling(Seconds)")
-
-
- options, args = parser.parse_args()
-
-
- # expected or default
- package = options.package
- startup_delay = options.startup_delay
-
- status_name = options.status_name
-
- if options.test_name:
- test_name = options.test_name
- else:
- parser.error("you must give me a test to run")
-
- try:
- runtime_test(package, options.test_name)
- except KeyboardInterrupt, e:
- pass
- print "exiting"
Deleted: pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py
===================================================================
--- pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-20 01:24:10 UTC (rev 19184)
+++ pkg/trunk/stacks/hardware_test/runtime_monitor/src/runtime_monitor/expected.py 2009-07-20 06:36:45 UTC (rev 19185)
@@ -1,80 +0,0 @@
-#!/usr/bin/env python
-# Software License Agreement (BSD License)
-#
-# Copyright (c) 2008, Willow Garage, Inc.
-# All rights reserved.
-#
-# Redistribution and use in source and binary forms, with or without
-# modification, are permitted provided that the following conditions
-# are met:
-#
-# * Redistributions of source code must retain the above copyright
-# notice, this list of conditions and the following disclaimer.
-# * Redistributions in binary form must reproduce the above
-# copyright notice, this list of conditions and the following
-# disclaimer in the documentation and/or other mate...
[truncated message content] |
|
From: <rob...@us...> - 2009-07-21 21:41:33
|
Revision: 19330
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19330&view=rev
Author: rob_wheeler
Date: 2009-07-21 21:41:32 +0000 (Tue, 21 Jul 2009)
Log Message:
-----------
Don't publish as global topics
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/stacks/pr2_drivers/ethercat_hardware/src/wg0x.cpp
Modified: pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-07-21 21:33:38 UTC (rev 19329)
+++ pkg/trunk/stacks/mechanism/mechanism_control/src/mechanism_control.cpp 2009-07-21 21:41:32 UTC (rev 19330)
@@ -56,8 +56,8 @@
current_controllers_list_(0),
used_by_realtime_(-1),
pub_diagnostics_(node_, "/diagnostics", 1),
- pub_joints_(node_, "/joint_states", 1),
- pub_mech_state_(node_, "/mechanism_state", 1),
+ pub_joints_(node_, "joint_states", 1),
+ pub_mech_state_(node_, "mechanism_state", 1),
last_published_state_(realtime_gettime()),
last_published_diagnostics_(realtime_gettime())
{
Modified: pkg/trunk/stacks/pr2_drivers/ethercat_hardware/src/wg0x.cpp
===================================================================
--- pkg/trunk/stacks/pr2_drivers/ethercat_hardware/src/wg0x.cpp 2009-07-21 21:33:38 UTC (rev 19329)
+++ pkg/trunk/stacks/pr2_drivers/ethercat_hardware/src/wg0x.cpp 2009-07-21 21:41:32 UTC (rev 19330)
@@ -228,9 +228,9 @@
if (fw_major_ >= 1)
{
- topic = "/accelerometer/";
+ topic = "accelerometer";
if (!actuator->name_.empty())
- topic += actuator->name_;
+ topic += "/" + actuator->name_;
accel_publisher_ = new realtime_tools::RealtimePublisher<ethercat_hardware::AccelerometerState>(ros::NodeHandle(), topic, 1);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-07-24 19:59:39
|
Revision: 19598
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19598&view=rev
Author: jfaustwg
Date: 2009-07-24 19:59:31 +0000 (Fri, 24 Jul 2009)
Log Message:
-----------
Move pr2_dashboard to the pr2 stack
Added Paths:
-----------
pkg/trunk/stacks/pr2/pr2_dashboard/
Removed Paths:
-------------
pkg/trunk/stacks/visualization/pr2_dashboard/
Property changes on: pkg/trunk/stacks/pr2/pr2_dashboard
___________________________________________________________________
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/visualization/pr2_dashboard:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
/pkg/branches/josh/rviz_selection/pr2_dashboard:12152-13640
/pkg/branches/josh/visualization_scratch/pr2_dashboard:14041-14428
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <jfa...@us...> - 2009-07-24 20:18:43
|
Revision: 19603
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19603&view=rev
Author: jfaustwg
Date: 2009-07-24 20:18:36 +0000 (Fri, 24 Jul 2009)
Log Message:
-----------
Move image_view into the opencv stack
Added Paths:
-----------
pkg/trunk/stacks/opencv/image_view/
Removed Paths:
-------------
pkg/trunk/stacks/visualization/image_view/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mp...@us...> - 2009-07-27 23:43:00
|
Revision: 19770
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19770&view=rev
Author: mppics
Date: 2009-07-27 23:42:46 +0000 (Mon, 27 Jul 2009)
Log Message:
-----------
clean up gripper transmission block (removed pid)
move gripper coefficients screw_reduction and gear_ratio into transmission block.
removed pr2_gripper_alpha2, replace with macro variable for origin (add x-offset of 0.003m) when referring to
pr2_gripper.
update pr2_gripper_transmission:
* to parse the coefficients.
* Jacobian was off by a factor of (2*PI)^2 /2
Modified Paths:
--------------
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h
pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/l_arm.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/l_gripper.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/pr2.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/pre.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/prf.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/prg.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_arm.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_forearm_gripper.xacro.xml
pkg/trunk/stacks/pr2/pr2_defs/robots/r_gripper.xacro.xml
Modified: pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/pr2_gripper_transmission.h 2009-07-27 23:42:46 UTC (rev 19770)
@@ -47,6 +47,7 @@
#include "tinyxml/tinyxml.h"
#include "mechanism_model/transmission.h"
#include "mechanism_model/robot.h"
+//#include <fstream>
namespace mechanism {
@@ -54,7 +55,7 @@
{
public:
PR2GripperTransmission() {}
- virtual ~PR2GripperTransmission() {}
+ virtual ~PR2GripperTransmission() {/*myfile.close();*/}
bool initXml(TiXmlElement *config, Robot *robot);
@@ -78,7 +79,7 @@
void computeGapStates(double MR,double MR_dot,double MT,
double &theta,double &dtheta_dMR,double &dt_dtheta,double &dt_dMR,double &gap_size,double &gap_velocity,double &gap_effort);
void inverseGapStates(double theta,double &MR, double &dMR_dtheta,double &dtheta_dt,double &dMR_dt);
-
+ //std::ofstream myfile;
void getRateFromMaxRateJoint(
std::vector<JointState*>& js, std::vector<Actuator*>& as,
int &maxRateJointIndex,double &rate);
@@ -89,11 +90,11 @@
// SOME CONSTANTS
// the default theta0 when gap size is 0 is needed to assign passive joint angles
//
+ double screw_reduction_; // in meters/revolution
+ double gear_ratio_;
static const double theta0_ = 2.97571*M_PI/180.0; // convert to radians
static const double phi0_ = 29.98717*M_PI/180.0; // convert to radians
- static const double gear_ratio_ = 29.16; //729.0/25.0;
static const double t0_ = 0; //-0.19543/1000.0; // convert to meters
- static const double screw_reduction_ = 2.0/1000.0; // convert to meters
static const double L0_ = 34.70821/1000.0; // convert to meters
static const double coef_h_ = 5.200/1000.0; // convert to meters
static const double coef_a_ = 67.56801/1000.0; // convert to meters
@@ -107,8 +108,6 @@
extern const double PR2GripperTransmission::t0_ ;
extern const double PR2GripperTransmission::theta0_ ;
extern const double PR2GripperTransmission::phi0_ ;
-extern const double PR2GripperTransmission::gear_ratio_ ;
-extern const double PR2GripperTransmission::screw_reduction_ ;
extern const double PR2GripperTransmission::L0_ ;
extern const double PR2GripperTransmission::coef_h_ ;
extern const double PR2GripperTransmission::coef_a_ ;
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/pr2_gripper_transmission.cpp 2009-07-27 23:42:46 UTC (rev 19770)
@@ -36,6 +36,7 @@
#include <numeric>
#include <angles/angles.h>
+
namespace mechanism {
ROS_REGISTER_TRANSMISSION(PR2GripperTransmission)
@@ -44,7 +45,7 @@
{
const char *name = config->Attribute("name");
name_ = name ? name : "";
-
+ //myfile.open("transmission_data.txt");
TiXmlElement *ael = config->FirstChildElement("actuator");
const char *actuator_name = ael ? ael->Attribute("name") : NULL;
if (!actuator_name || !robot->getActuator(actuator_name))
@@ -75,6 +76,25 @@
}
gap_mechanical_reduction_ = atof(joint_reduction);
+ // get the screw drive reduction
+ const char *screw_reduction_str = j->Attribute("screw_reduction");
+ if (screw_reduction_str == NULL)
+ {
+ ROS_ERROR("PR2GripperTransmission's joint \"%s\" was not given a screw drive reduction.", joint_name);
+ return false;
+ }
+ screw_reduction_ = atof(screw_reduction_str);
+ ROS_INFO("screw drive reduction. %f", screw_reduction_);
+
+ // get the gear_ratio reduction
+ const char *gear_ratio_str = j->Attribute("gear_ratio");
+ if (gear_ratio_str == NULL)
+ {
+ ROS_ERROR("PR2GripperTransmission's joint \"%s\" was not given a gear_ratio.", joint_name);
+ return false;
+ }
+ gear_ratio_ = atof(gear_ratio_str);
+ ROS_INFO("gear_ratio reduction. %f", gear_ratio_);
}
for (TiXmlElement *j = config->FirstChildElement("passive_joint"); j; j = j->NextSiblingElement("passive_joint"))
@@ -137,14 +157,12 @@
//
// get the effort at the gripper gap based on torque at the motor
// gap effort = motor torque * dmotor_theta/dt
- // = I * motor_theta_ddot * dmotor_theta_dt
- // = I * MR_ddot * (2*pi) * dmotor_theta_dt
- // = I * MR_ddot * (2*pi) * dMR_dt / (2*pi)
- // = MT / dt_dMR
+ // = MT * dmotor_theta_dt
+ // = MT * dMR_dt / (2*pi)
+ // = MT / dt_dMR * 2*pi
//
- // here MT is defined as I*MR_ddot
- //
- gap_effort = MT / dt_dMR;
+ gap_effort = MT / dt_dMR / rad2mr_ ;
+ //ROS_WARN("debug: %f %f %f",gap_effort,MT,dt_dMR,rad2mr_);
}
///////////////////////////////////////////////////////////
@@ -188,65 +206,6 @@
dMR_dt = dMR_dtheta * dtheta_dt; // remember, here, t is gap_size
}
-///////////////////////////////////////////////////////////
-/// assign joint position, velocity, effort from actuator state
-/// all passive joints are assigned by single actuator state through mimic?
-void PR2GripperTransmission::propagatePosition(
- std::vector<Actuator*>& as, std::vector<JointState*>& js)
-{
-
- ROS_ASSERT(as.size() == 1);
- ROS_ASSERT(js.size() == passive_joints_.size() + 1); // passive joints and 1 gap joint
-
- /// \brief motor revolutions = encoder value * gap_mechanical_reduction_ * rad2mr_
- /// motor revolutions = motor angle(rad) / (2*pi)
- /// = theta / (2*pi)
- double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// \brief motor revolustions per second = motor angle rate (rad per second) / (2*pi)
- double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// \brief gripper motor torque: originally in newton-meters, but we convert it to Nm*(MR/rad)
- /// motor torque = actuator_state->last_meausured_effort
- /// motor torque = I * theta_ddot
- /// MT = I * MR_ddot (my definition)
- /// = I * theta_ddot / (2*pi)
- /// = motot torque / (2*pi)
- double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * rad2mr_ ;
-
- /// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
- double theta, dtheta_dMR, dt_dtheta, dt_dMR;
- /// information on the fictitious joint: gap_joint
- double gap_size,gap_velocity,gap_effort;
-
- // compute gap position, velocity, applied_effort from actuator states
- computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
-
- // assign joint states
- for (unsigned int i = 0; i < js.size(); ++i)
- {
- if (js[i]->joint_->name_ == gap_joint_)
- {
- // assign gap joint
- js[i]->position_ = gap_size*2.0; // function engineering's transmission give half the total gripper size
- js[i]->velocity_ = gap_velocity;
- js[i]->applied_effort_ = gap_effort;
- }
- else
- {
- // find the passive joint name
- std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
- if (it != passive_joints_.end())
- {
- // assign passive joints
- js[i]->position_ = theta - theta0_;
- js[i]->velocity_ = dtheta_dMR * MR_dot ;
- js[i]->applied_effort_ = MT / dtheta_dMR ;
- }
- }
- }
-}
-
void PR2GripperTransmission::getRateFromMaxRateJoint(
std::vector<JointState*>& js, std::vector<Actuator*>& as,
int &max_rate_joint_index,double &rate)
@@ -261,15 +220,15 @@
std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
if (it != passive_joints_.end())
{
- if (abs(js[i]->velocity_) > max_rate)
+ if (fabs(js[i]->velocity_) > max_rate)
{
- max_rate = abs(js[i]->velocity_) ;
+ max_rate = fabs(js[i]->velocity_) ;
max_rate_joint_index = i;
}
}
}
- assert(max_rate_joint_index < js.size());
+ assert(max_rate_joint_index < (int)js.size());
rate = js[max_rate_joint_index]->velocity_;
}
@@ -282,7 +241,7 @@
// obtain the physical location of passive joints in sim, and average them
angle = 0.0;
- double min_rate = 1.0e16; // a ridiculously large value
+ double min_rate = 1000000000000.0; // a ridiculously large value
torque = 0.0;
min_rate_joint_index = js.size();
@@ -295,21 +254,87 @@
//if (it == passive_joints_.begin())
if (it != passive_joints_.end())
{
- if (abs(js[i]->velocity_) < min_rate)
+ if (fabs(js[i]->velocity_) < min_rate)
{
- min_rate = abs(js[i]->velocity_) ;
+ min_rate = fabs(js[i]->velocity_) ;
min_rate_joint_index = i;
}
}
}
- assert(min_rate_joint_index < js.size()); // some joint rate better be smaller than 1.0e16
+ assert(min_rate_joint_index < (int)js.size()); // some joint rate better be smaller than 1.0e16
angle = angles::shortest_angular_distance(theta0_,js[min_rate_joint_index]->position_) + theta0_;
rate = js[min_rate_joint_index]->velocity_;
torque = js[min_rate_joint_index]->applied_effort_;
}
+///////////////////////////////////////////////////////////
+/// assign joint position, velocity, effort from actuator state
+/// all passive joints are assigned by single actuator state through mimic?
+void PR2GripperTransmission::propagatePosition(
+ std::vector<Actuator*>& as, std::vector<JointState*>& js)
+{
+
+ ROS_ASSERT(as.size() == 1);
+ ROS_ASSERT(js.size() == passive_joints_.size() + 1); // passive joints and 1 gap joint
+
+ /// \brief motor revolutions = encoder value * gap_mechanical_reduction_ * rad2mr_
+ /// motor revolutions = motor angle(rad) / (2*pi)
+ /// = theta / (2*pi)
+ double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+ /// \brief motor revolustions per second = motor angle rate (rad per second) / (2*pi)
+ double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+ ///
+ /// old MT definition - obsolete
+ ///
+ /// but we convert it to Nm*(MR/rad)
+ /// motor torque = actuator_state->last_meausured_effort
+ /// motor torque = I * theta_ddot
+ /// MT = I * MR_ddot (my definition)
+ /// = I * theta_ddot / (2*pi)
+ /// = motot torque / (2*pi)
+ //double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_ * rad2mr_ ;
+
+
+ /// \brief gripper motor torque: received from hardware side in newton-meters
+ double MT = as[0]->state_.last_measured_effort_ / gap_mechanical_reduction_;
+
+ /// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
+ double theta, dtheta_dMR, dt_dtheta, dt_dMR;
+ /// information on the fictitious joint: gap_joint
+ double gap_size,gap_velocity,gap_effort;
+
+ // compute gap position, velocity, applied_effort from actuator states
+ computeGapStates(MR,MR_dot,MT,theta,dtheta_dMR, dt_dtheta, dt_dMR,gap_size,gap_velocity,gap_effort);
+
+ // assign joint states
+ for (unsigned int i = 0; i < js.size(); ++i)
+ {
+ if (js[i]->joint_->name_ == gap_joint_)
+ {
+ // assign gap joint
+ js[i]->position_ = gap_size*2.0; // function engineering's transmission give half the total gripper size
+ js[i]->velocity_ = gap_velocity*2.0;
+ js[i]->applied_effort_ = gap_effort/2.0;
+ }
+ else
+ {
+ // find the passive joint name, set passive joint states
+ std::vector<std::string>::iterator it = std::find(passive_joints_.begin(),passive_joints_.end(),js[i]->joint_->name_);
+ if (it != passive_joints_.end())
+ {
+ // assign passive joint states
+ js[i]->position_ = theta - theta0_;
+ js[i]->velocity_ = dtheta_dMR * MR_dot;
+ js[i]->applied_effort_ = MT / dtheta_dMR / rad2mr_;
+ }
+ }
+ }
+}
+
// this is needed for simulation, so we can recover encoder value given joint angles
void PR2GripperTransmission::propagatePositionBackwards(
std::vector<JointState*>& js, std::vector<Actuator*>& as)
@@ -343,11 +368,10 @@
/// = theta_dot * dMR_dtheta * gap_mechanical_reduction_ / rad2mr
as[0]->state_.velocity_ = joint_rate * dMR_dtheta * gap_mechanical_reduction_ / rad2mr_ ;
- /// state effort = MT * gap_mechanical_reduction_ / rad2mr
- /// = I * MR_ddot * gap_mechanical_reduction_ / rad2mr
- /// = MT * gap_mechanical_reduction_ / rad2mr
- /// = gap_effort * dt_dMR * gap_mechanical_reduction_ / rad2mr
- as[0]->state_.last_measured_effort_ = gap_effort / dMR_dt * gap_mechanical_reduction_ / rad2mr_ ;
+ /// motor torque = inverse of getting gap effort from motor torque
+ /// = gap_effort * dt_dMR / (2*pi) * gap_mechanical_reduction_
+ /// = gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_
+ as[0]->state_.last_measured_effort_ = gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_;
return;
}
}
@@ -384,10 +408,10 @@
{
if (js[i]->joint_->name_ == gap_joint_)
{
- double gap_effort = js[i]->commanded_effort_; /// Newtons
- /// actuator commanded effort = MT * gap_mechanical_reduction_ / rad2mr_
- /// = gap_dffort / dMR_dt * gap_mechanical_reduction_ / rad2mr_
- as[0]->command_.effort_ = gap_effort / dMR_dt * gap_mechanical_reduction_ / rad2mr_ ;
+ double gap_effort = js[i]->commanded_effort_; /// Newtons
+ /// actuator commanded effort = gap_dffort / dMR_dt / (2*pi) * gap_mechanical_reduction_
+ as[0]->command_.effort_ = 2.0 * gap_effort / dMR_dt * rad2mr_ * gap_mechanical_reduction_; // divide by 2 because torque is transmitted to 2 fingers
+ //ROS_WARN("debug: %f %f %f",gap_effort,dMR_dt,rad2mr_);
break;
}
}
@@ -407,7 +431,7 @@
/// \brief taken from propagatePosition()
double MR = as[0]->state_.position_ / gap_mechanical_reduction_ * rad2mr_ ;
double MR_dot = as[0]->state_.velocity_ / gap_mechanical_reduction_ * rad2mr_ ;
- double MT = as[0]->command_.effort_ / gap_mechanical_reduction_ * rad2mr_ ;
+ double MT = as[0]->command_.effort_ / gap_mechanical_reduction_;
/// internal theta state, gripper closed it is theta0_. same as finger joint angles + theta0_.
double theta, dtheta_dMR, dt_dtheta, dt_dMR;
@@ -424,6 +448,7 @@
{
// propagate fictitious joint effort backwards
js[i]->commanded_effort_ = gap_effort;
+ //ROS_WARN("debug2: %f %f %f",gap_effort,dt_dMR,rad2mr_);
}
else
{
@@ -446,13 +471,13 @@
// FIXME: hackery, due to transmission values, MT is too large for the damping available
// with the given time step size in sim, so until implicit damping is implemented,
- // we'll scale MT with inverse of velocity
- //double scale = exp(-abs(js[i]->velocity_*1.0));
+ // we'll scale MT with inverse of velocity to some power
int max_joint_rate_index;
- double scale,max_joint_rate;
+ double scale=1.0,rate_threshold=0.5;
+ double max_joint_rate;
getRateFromMaxRateJoint(js, as, max_joint_rate_index, max_joint_rate);
- scale = (abs(max_joint_rate)>0.0) ? 1./abs(max_joint_rate*0000001.0) : 1.0;
-
+ if (fabs(max_joint_rate)>rate_threshold) scale /= pow(fabs(max_joint_rate/rate_threshold),2.0);
+ //std::cout << "rate " << max_joint_rate << " absrate " << fabs(max_joint_rate) << " scale " << scale << std::endl;
// sum joint torques from actuator motor and mimic constraint and convert to joint torques
js[i]->commanded_effort_ = scale*MT / dtheta_dMR;
}
Modified: pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-27 23:30:10 UTC (rev 19769)
+++ pkg/trunk/stacks/pr2/pr2_defs/defs/gripper_defs.xml 2009-07-27 23:42:46 UTC (rev 19770)
@@ -415,13 +415,13 @@
- <macro name="pr2_gripper" params="side parent">
+ <macro name="pr2_gripper" params="side parent *origin gear_ratio screw_reduction">
<joint name="${side}_gripper_palm_joint" type="fixed" />
<link name="${side}_gripper_palm_link">
<parent name="${parent}" />
<joint name="${side}_gripper_palm_joint" />
- <origin xyz="0 0 0" rpy="0 0 0" />
+ <insert_block name="origin" />
<inertial>
<mass value="0.58007" />
<com xyz="0.06623 0.00053 -0.00119" />
@@ -609,19 +609,11 @@
<!-- gazebo_mimic_pid is for sim only. -->
<transmission type="PR2GripperTransmission" name="${side}_gripper_trans" >
<actuator name="${side}_gripper_motor" />
- <gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
- <passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_tip_joint" >
- <gazebo_mimic_pid p="0.000000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_l_finger_tip_joint" >
- <gazebo_mimic_pid p="0.000000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
+ <gap_joint name="${side}_gripper_joint" screw_reduction="${screw_reduction}" gear_ratio="${gear_ratio}" mechanical_reduction="1.0" />
+ <passive_joint name="${side}_gripper_l_finger_joint" />
+ <passive_joint name="${side}_gripper_r_finger_joint" />
+ <passive_joint name="${side}_gripper_r_finger_tip_joint" />
+ <passive_joint name="${side}_gripper_l_finger_tip_joint" />
</transmission>
<!--
@@ -643,232 +635,13 @@
</macro>
-<!-- Alpha 2.0 gripper. -->
-<!-- Only change is that gripper is moved out in the x-dir 3mm from wrist. -->
- <macro name="pr2_gripper_alpha2" params="side parent">
-
- <joint name="${side}_gripper_palm_joint" type="fixed" />
- <link name="${side}_gripper_palm_link">
- <parent name="${parent}" />
- <joint name="${side}_gripper_palm_joint" />
- <!-- New origin for palm joint for Alpha 2.0 gripper -->
- <origin xyz="0.003 0 0" rpy="0 0 0" />
- <inertial>
- <mass value="0.58007" />
- <com xyz="0.06623 0.00053 -0.00119" />
- <inertia ixx="0.00035223921" ixy="-0.00001580476" ixz="-0.00000091750"
- iyy="0.00067741312" iyz="-0.00000059554"
- izz="0.00086563316" />
- </inertial>
-
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Red" />
- </map>
- <geometry name="${side}_gripper_palm_visual">
- <mesh filename="gripper_palm.stlb" />
- </geometry>
- <map name="${side}_gripper_palm_visual_mesh" flag="ogre">
- <elem key="mesh" value="gripper_palm.mesh" />
- </map>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_palm_collision">
- <mesh filename="convex/gripper_palm_convex.stlb" />
- </geometry>
- <map name="${side}_gripper_palm_collision_mesh" flag="ogre">
- <elem key="mesh" value="convex/gripper_palm_convex.mesh" />
- </map>
- <verbose value="Yes" />
- <map flag="collision" name="mesh">
- <elem key="type" value="visual"/>
- </map>
- </collision>
- <map name="${side}_gripper_palm_sensor" flag="gazebo">
- <verbatim key="${side}_gripper_palm_bumper_sensor">
- <sensor:contact name="${side}_gripper_palm_contact_sensor">
- <geom>${side}_gripper_palm_collision</geom>
- <updateRate>100.0</updateRate>
- <controller:ros_bumper name="${side}_gripper_palm_ros_bumper_controller" plugin="libros_bumper.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bumperTopicName>${side}_gripper_palm_bumper</bumperTopicName>
- <interface:bumper name="${side}_gripper_palm_ros_bumper_iface" />
- </controller:ros_bumper>
- </sensor:contact>
- </verbatim>
- </map>
- <map name="${side}_gripper_palm_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
-
-
-
- <!-- floating link for simulating gripper constraints -->
- <joint name="${side}_gripper_float_joint" type="prismatic">
- <limit min="-0.05" max="0.001" effort="0"
- safety_length_min="0.0" safety_length_max="0.0" />
- <axis xyz="1 0 0" />
- <joint_properties damping="10.0" />
- </joint>
- <link name="${side}_gripper_float_link">
- <parent name="${side}_gripper_palm_link" />
- <joint name="${side}_gripper_float_joint" />
- <origin xyz="0.05 0 0" rpy="0 0 0" />
- <inertial>
- <mass value="0.01" />
- <com xyz="0 0 0" />
- <inertia ixx="0.0001" ixy="0" ixz="0"
- iyy="0.0001" iyz="0"
- izz="0.0001" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Red" />
- </map>
- <geometry name="${side}_gripper_float_visual">
- <box size="0.1 0.1 0.1" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <geometry name="${side}_gripper_float_collision">
- <box size="0.001 0.001 0.001" />
- </geometry>
- <verbose value="Yes" />
- <map flag="collision" name="mesh">
- <elem key="type" value="visual"/>
- </map>
- </collision>
- <map name="${side}_gripper_float_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
- <map name="${side}_fake_gripper_slider_joint" flag="gazebo">
- <verbatim key="${side}_fake_gripper_slider_joint">
- <joint:slider name="${side}_gripper_l_finger_tip_slider_joint">
- <body1>${side}_gripper_l_finger_tip_link</body1>
- <body2>${side}_gripper_float_link</body2>
- <anchor>${side}_gripper_l_finger_tip_link</anchor>
- <axis>0 1 0</axis>
- <anchorOffset>0 0 0</anchorOffset>
- </joint:slider>
- <joint:slider name="${side}_gripper_r_finger_tip_slider_joint">
- <body1>${side}_gripper_r_finger_tip_link</body1>
- <body2>${side}_gripper_float_link</body2>
- <anchor>${side}_gripper_r_finger_tip_link</anchor>
- <axis>0 1 0</axis>
- <anchorOffset>0 0 0</anchorOffset>
- </joint:slider>
- </verbatim>
- </map>
-
-
-
-
- <map name="sensor" flag="gazebo">
- <verbatim key="p3d_${side}_gripper_palm">
- <controller:ros_p3d name="p3d_${side}_gripper_palm_controller" plugin="libros_p3d.so">
- <alwaysOn>true</alwaysOn>
- <updateRate>100.0</updateRate>
- <bodyName>${side}_gripper_palm_link</bodyName>
- <topicName>${side}_gripper_palm_pose_ground_truth</topicName>
- <xyzOffsets>0 0 0</xyzOffsets>
- <rpyOffsets>0 0 0</rpyOffsets>
- <gaussianNoise>0.0</gaussianNoise>
- <frameName>map</frameName>
- <interface:position name="p3d_${side}_gripper_palm_position_iface" />
- </controller:ros_p3d>
- </verbatim>
- </map>
-
- <joint name="${side}_gripper_tool_joint" type="fixed" />
- <link name="${side}_gripper_tool_frame">
- <parent name="${side}_gripper_palm_link" />
- <joint name="${side}_gripper_tool_joint" />
- <origin xyz="0.18 0 0" rpy="0 0 0" />
-
- <!-- TODO: all that follows is made-up and useless, but the old parser crashes without it -->
- <inertial>
- <mass value="0" />
- <com xyz="0 0 0" />
- <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
- </inertial>
- <visual>
- <origin xyz="0 0 0" rpy="0 0 0" />
- <map name="gazebo_material" flag="gazebo">
- <elem key="material" value="PR2/Grey" />
- </map>
- <geometry name="${side}_gripper_tool_visual">
- <box size="0.001 0.001 0.001" />
- </geometry>
- </visual>
- <collision>
- <origin xyz="-0.1 0 0" rpy="0 0 0" /> <!-- move it out of the way -->
- <geometry name="${side}_gripper_tool_collision">
- <box size="0.001 0.001 0.001" />
- </geometry>
- </collision>
- <map name="${side}_gripper_tool_gravity" flag="gazebo">
- <elem key="turnGravityOff">true</elem>
- </map>
- </link>
-
- <pr2_finger prefix="${side}_gripper" parent="${side}_gripper_palm_link" />
-
- <!-- [lr]_gripper_joint position is the gap_size/2 -->
- <!-- [lr]_gripper_joint velocity is the gap linear velocity -->
- <!-- [lr]_gripper_joint effort is the gap linear force -->
- <!-- Please refer to function engineering spreadsheet 090224_link_data.xls for -->
- <!-- the transmission function. -->
- <!-- Please refer to mechanism_model/src/pr2_gripper_transmission.cpp for implementation.-->
- <!-- gazebo_mimic_pid is for sim only. -->
- <transmission type="PR2GripperTransmission" name="${side}_gripper_trans" >
- <actuator name="${side}_gripper_motor" />
- <gap_joint name="${side}_gripper_joint" mechanical_reduction="1.0" />
- <passive_joint name="${side}_gripper_l_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_joint" >
- <gazebo_mimic_pid p="0.0000" i="0.000" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_r_finger_tip_joint" >
- <gazebo_mimic_pid p="0.00000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- <passive_joint name="${side}_gripper_l_finger_tip_joint" >
- <gazebo_mimic_pid p="0.00000" i="0.0" d="0.0" iClamp="0.0" />
- </passive_joint>
- </transmission>
-
- <!--
- min="${gripper_min_angle}" max="${gripper_max_angle}"
- -->
- <!-- fictitous joint that represents the gripper gap -->
- <!-- effort is the linear force at the gripper gap
- velocity limit is the linear velocity limit at the gripper gap
- try and introduce a very stiff spring
- -->
- <joint name="${side}_gripper_joint" type="prismatic">
- <axis xyz="0 0 1" />
- <limit effort="100.0" velocity="0.02"
- min="0.0" max="0.09"
- k_velocity="500.0" k_position="20.0"
- safety_length_min="0.0" safety_length_max="0.001" />
- </joint>
-
- </macro>
-
<!-- Calibration -->
...
[truncated message content] |
|
From: <mar...@us...> - 2009-07-29 08:33:09
|
Revision: 19989
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19989&view=rev
Author: mariusmuja
Date: 2009-07-29 08:33:01 +0000 (Wed, 29 Jul 2009)
Log Message:
-----------
Changes to outlet_spotting and handle_detection
Modified Paths:
--------------
pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
Modified: pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp
===================================================================
--- pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-07-29 07:47:56 UTC (rev 19988)
+++ pkg/trunk/stacks/semantic_mapping/door_handle_detector/src/handle_detector_vision.cpp 2009-07-29 08:33:01 UTC (rev 19989)
@@ -163,7 +163,7 @@
bool got_images_;
- double image_timeout_;
+ double timeout_;
ros::Time start_image_wait_;
@@ -182,7 +182,7 @@
nh_.param("~min_height", min_height_, 0.8);
nh_.param("~max_height", max_height_, 1.2);
nh_.param("~frames_no", frames_no_, 10);
- nh_.param("~timeout", image_timeout_, 3.0); // timeout (in seconds) until an image must be received, otherwise abort
+ nh_.param("~timeout", timeout_, 3.0); // timeout (in seconds) until an image must be received, otherwise abort
nh_.param("~display", display_, false);
stringstream ss;
ss << getenv("ROS_ROOT") << "/../ros-pkg/mapping/door_handle_detector/data/";
@@ -836,7 +836,10 @@
while (!got_images_ && !preempted_) {
data_cv_.wait(lock);
}
- if (preempted_) break;
+ if (preempted_) {
+ ROS_INFO("OutletSpotter: detect loop preempted, stereo data not received");
+ return false;
+ }
if(display_){
// show original disparity
@@ -1007,6 +1010,16 @@
ros::Rate r(10);
while (nh_.ok())
{
+ data_lock_.lock();
+ if (!(have_images_ && have_cloud_point_)) {
+ if ((ros::Time::now()-start_image_wait_) > ros::Duration(timeout_)) {
+ preempted_ = true;
+ data_cv_.notify_all();
+ }
+ }
+ data_lock_.unlock();
+
+
if (display_) {
int key = cvWaitKey(10)&0x00FF;
if(key == 27) //ESC
Modified: pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp
===================================================================
--- pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp 2009-07-29 07:47:56 UTC (rev 19988)
+++ pkg/trunk/stacks/visual_feature_detectors/outlet_detection/src/outlet_spotting2.cpp 2009-07-29 08:33:01 UTC (rev 19989)
@@ -1457,7 +1457,7 @@
data_cv_.wait(images_lock);
}
if (preempt_) {
- ROS_INFO("OutletSpotter: detect loop preempted");
+ ROS_INFO("OutletSpotter: detect loop preempted, stereo or base laser not received");
return false;
}
ROS_INFO("OutletSpotter: received images and base laser data, performing detection");
@@ -1497,7 +1497,7 @@
while (nh_.ok())
{
data_lock_.lock();
- if (!have_images_ && !have_cloud_point_) {
+ if (!(have_images_ && have_cloud_point_)) {
if ((ros::Time::now()-start_image_wait_) > ros::Duration(timeout_)) {
preempt_ = true;
data_cv_.notify_all();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bla...@us...> - 2009-07-30 05:12:30
|
Revision: 20139
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20139&view=rev
Author: blaisegassend
Date: 2009-07-30 05:12:22 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Created the driver_common stack.
Added Paths:
-----------
pkg/trunk/stacks/driver_common/
pkg/trunk/stacks/driver_common/stack.xml
Copied: pkg/trunk/stacks/driver_common/stack.xml (from rev 19807, pkg/trunk/stacks/camera_drivers/stack.xml)
===================================================================
--- pkg/trunk/stacks/driver_common/stack.xml (rev 0)
+++ pkg/trunk/stacks/driver_common/stack.xml 2009-07-30 05:12:22 UTC (rev 20139)
@@ -0,0 +1,25 @@
+<stack name="camera_drivers" version="0.1">
+ <description brief="Framework used by all drivers.">
+
+ This stack contains a framework that will be used by all the driver
+ packages. Functionality includes:
+
+ - Classes for helping to publish consistent diagnostics (each topic
+ will publish certain diagnostics, each driver will publish certain
+ diagnostics, etc.), and to make diagnostics less tedious to write.
+
+ - Classes for helping to run self-tests and publish their results.
+
+ - A dynamic reconfiguration system to allow driver parameters to be
+ changed without taking down the driver, and to autogenerate parameter
+ documentation.
+
+ - A base class for sensors to provide a consistent state machine
+ (retries, error handling, etc.) and interface.
+
+ </description>
+ <author>Maintained by Blaise Gassend</author>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/driver_common</url>
+ <depend stack="ros" version="0.7"/>
+</stack>
Property changes on: pkg/trunk/stacks/driver_common/stack.xml
___________________________________________________________________
Added: svn:mergeinfo
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bla...@us...> - 2009-07-30 05:59:43
|
Revision: 20148
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20148&view=rev
Author: blaisegassend
Date: 2009-07-30 05:59:35 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
Created the ethercat_drivers stack.
Added Paths:
-----------
pkg/trunk/stacks/ethercat_drivers/
pkg/trunk/stacks/ethercat_drivers/stack.xml
Added: pkg/trunk/stacks/ethercat_drivers/stack.xml
===================================================================
--- pkg/trunk/stacks/ethercat_drivers/stack.xml (rev 0)
+++ pkg/trunk/stacks/ethercat_drivers/stack.xml 2009-07-30 05:59:35 UTC (rev 20148)
@@ -0,0 +1,11 @@
+<stack name="camera_drivers" version="0.1">
+ <description brief="drivers related to ethercat">
+ This stack contains drivers for the ethercat system and the peripherals
+ that connect to it: motor control boards, fingertip sensors, texture
+ projector, hand accelerometer.
+ </description>
+ <author>Maintained by Blaise Gassend</author>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/ethercat_drivers</url>
+ <depend stack="ros" version="0.7"/>
+</stack>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <mee...@us...> - 2009-07-30 22:12:23
|
Revision: 20200
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20200&view=rev
Author: meeussen
Date: 2009-07-30 22:12:14 +0000 (Thu, 30 Jul 2009)
Log Message:
-----------
move bfl from the estimation stack to the common stack
Added Paths:
-----------
pkg/trunk/stacks/common/bfl/
Removed Paths:
-------------
pkg/trunk/stacks/estimation/bfl/
Property changes on: pkg/trunk/stacks/common/bfl
___________________________________________________________________
Added: svn:ignore
+ bfl-boost
installed
patched
rospack_nosubdirs
Added: svn:mergeinfo
+ /pkg/branches/gazebo-branch-merge/estimation/bfl:15683-15684,15739-15794,15797-15820,15822-15839,15852-15870,15983-16008,16010-16016,16129-16141,16145-16169,16245-16262,16274-16334
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ei...@us...> - 2009-07-31 00:51:46
|
Revision: 20224
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20224&view=rev
Author: eitanme
Date: 2009-07-31 00:51:37 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Changing the name of the "nav" stack to the "navigation" stack... I think its much better
Added Paths:
-----------
pkg/trunk/stacks/navigation/
Removed Paths:
-------------
pkg/trunk/stacks/nav/
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <bla...@us...> - 2009-07-31 19:01:31
|
Revision: 20290
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20290&view=rev
Author: blaisegassend
Date: 2009-07-31 19:01:22 +0000 (Fri, 31 Jul 2009)
Log Message:
-----------
Created the sound_drivers stack and moved sound_play into it.
Added Paths:
-----------
pkg/trunk/stacks/sound_drivers/
pkg/trunk/stacks/sound_drivers/sound_play/
pkg/trunk/stacks/sound_drivers/stack.xml
Property changes on: pkg/trunk/stacks/sound_drivers/sound_play
___________________________________________________________________
Added: svn:mergeinfo
+
Added: pkg/trunk/stacks/sound_drivers/stack.xml
===================================================================
--- pkg/trunk/stacks/sound_drivers/stack.xml (rev 0)
+++ pkg/trunk/stacks/sound_drivers/stack.xml 2009-07-31 19:01:22 UTC (rev 20290)
@@ -0,0 +1,11 @@
+<stack name="sound_drivers" version="0.1">
+ <description brief="Stack containing drivers to play/record sound.">
+
+ This stack contains sound-related drivers.
+
+ </description>
+ <author>Maintained by Blaise Gassend</author>
+ <review status="unreviewed" notes=""/>
+ <url>http://pr.willowgarage.com/wiki/sound_drivers</url>
+ <depend stack="ros" version="0.7"/>
+</stack>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2009-08-03 21:44:55
|
Revision: 20531
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20531&view=rev
Author: tfoote
Date: 2009-08-03 21:44:45 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
fixing Makefiles to allow make -C to work correctly
r30001@axx (orig r20529): tfoote | 2009-08-03 14:39:31 -0700
fixing kdl makefile
r30002@axx (orig r20530): tfoote | 2009-08-03 14:42:10 -0700
fixing opencv_latest Makefile
Modified Paths:
--------------
pkg/trunk/stacks/geometry/kdl/Makefile
pkg/trunk/stacks/opencv/opencv_latest/Makefile
Modified: pkg/trunk/stacks/geometry/kdl/Makefile
===================================================================
--- pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 21:42:10 UTC (rev 20530)
+++ pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 21:44:45 UTC (rev 20531)
@@ -10,7 +10,7 @@
BOOST_INCLUDE =$(shell rosboost-cfg --include_dirs)
EIGEN2_INCLUDE_DIR=`rospack find eigen`/build/eigen2/
TINYXML_INCLUDE_DIR=`rospack find tinyxml`/include/
-CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR)/ \
+CMAKE_ARGS = -DCMAKE_INSTALL_PREFIX=`rospack find kdl`/$(INSTALL_DIR)/ \
-DPYTHON_BINDINGS=ON \
-DBUILD_MODELS=OFF \
-DENABLE_TESTS=ON \
Modified: pkg/trunk/stacks/opencv/opencv_latest/Makefile
===================================================================
--- pkg/trunk/stacks/opencv/opencv_latest/Makefile 2009-08-03 21:42:10 UTC (rev 20530)
+++ pkg/trunk/stacks/opencv/opencv_latest/Makefile 2009-08-03 21:44:45 UTC (rev 20531)
@@ -14,7 +14,7 @@
CMAKE = cmake
CMAKE_ARGS = -D CMAKE_BUILD_TYPE=RELEASE \
- -D CMAKE_INSTALL_PREFIX=$(PWD)/$(INSTALL_DIR) \
+ -D CMAKE_INSTALL_PREFIX=`rospack find opencv_latest`/$(INSTALL_DIR) \
-D BUILD_PYTHON_SUPPORT=OFF
installed: $(SVN_DIR) patched
@@ -22,8 +22,8 @@
cd $(SVN_DIR)/build && $(CMAKE) $(CMAKE_ARGS) ..
cd $(SVN_DIR)/build && make $(ROS_PARALLEL_JOBS) && make install
@echo "patch opencv.pc to pass -Wl,-rpath,-L{libdir}"
- -mv $(PWD)/$(INSTALL_DIR)/lib/pkgconfig/opencv.pc $(PWD)/$(INSTALL_DIR)/lib/pkgconfig/opencv.bak
- sed 's%Libs: -L$${libdir} -lcxcore%Libs: -Wl,-rpath,$${libdir} -L$${libdir} -lcxcore%g' $(PWD)/$(INSTALL_DIR)/lib/pkgconfig/opencv.bak > $(PWD)/$(INSTALL_DIR)/lib/pkgconfig/opencv.pc
+ -mv `rospack find opencv_latest`/$(INSTALL_DIR)/lib/pkgconfig/opencv.pc $(PWD)/$(INSTALL_DIR)/lib/pkgconfig/opencv.bak
+ sed 's%Libs: -L$${libdir} -lcxcore%Libs: -Wl,-rpath,$${libdir} -L$${libdir} -lcxcore%g' `rospack find opencv_latest`/$(INSTALL_DIR)/lib/pkgconfig/opencv.bak > `rospack find opencv_latest`/$(INSTALL_DIR)/lib/pkgconfig/opencv.pc
touch installed
clean:
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X