[Personalrobots-commit] SF.net SVN: personalrobots:[7047] pkg/trunk

[<stu...@us...>]

Revision: 7047
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=7047&view=rev
Author:   stuglaser
Date:     2008-11-20 05:51:07 +0000 (Thu, 20 Nov 2008)

Log Message:
-----------
effect.py quickly brings up an effort controller on any joint, allowing you to command the joint easily

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
    pkg/trunk/mechanism/mechanism_control/scripts/spawner.py

Added Paths:
-----------
    pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py

Copied: pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py (from rev 7046, pkg/trunk/mechanism/mechanism_control/scripts/spawner.py)
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py	                        (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/effect.py	2008-11-20 05:51:07 UTC (rev 7047)
@@ -0,0 +1,81 @@
+#! /usr/bin/python
+# 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, Inc. 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.
+
+# This script brings up an effort controller on your joint of choice
+# and allows you to type in the desired efforts.
+#
+# Author: Stuart Glaser
+
+CONTROLLER_NAME = 'quick_effort_controller'
+
+import sys
+
+import rostools
+rostools.update_path('robot_mechanism_controllers')
+rostools.update_path('mechanism_control')
+import rospy
+from std_msgs.msg import *
+from mechanism_control import mechanism
+from robot_srvs.srv import SpawnController, KillController
+
+
+def xml_for(joint):
+    return "\
+<controller name=\"%s\" type=\"JointEffortControllerNode\">\
+<joint name=\"%s\" />\
+</controller>" % (CONTROLLER_NAME, joint)
+
+def main():
+    if len(sys.argv) < 2:
+        print "Usage:  effect.py <joint>"
+        sys.exit(1)
+    joint = sys.argv[1]
+
+    rospy.init_node('effect', anonymous=True)
+    rospy.wait_for_service('spawn_controller')
+    spawn_controller = rospy.ServiceProxy('spawn_controller', SpawnController)
+    kill_controller = rospy.ServiceProxy('kill_controller', KillController)
+
+    resp = spawn_controller(xml_for(joint))
+    if len(resp.ok) < 1 or not ord(resp.ok[0]):
+        print "Failed to spawn effort controller"
+        sys.exit(1)
+
+    pub = rospy.Publisher("/%s/set_command" % CONTROLLER_NAME, Float64)
+
+    try:
+        print "Enter efforts:"
+        while not rospy.is_shutdown():
+            effort = float(sys.stdin.readline().strip())
+            pub.publish(Float64(effort))
+    finally:
+        kill_controller(CONTROLLER_NAME)
+
+
+if __name__ == '__main__':
+    main()

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp	2008-11-20 05:10:12 UTC (rev 7046)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_effort_controller.cpp	2008-11-20 05:51:07 UTC (rev 7047)
@@ -69,7 +69,7 @@
   TiXmlElement *j = config->FirstChildElement("joint");
   if (!j)
   {
-    fprintf(stderr, "JointPositionController was not given a joint\n");
+    fprintf(stderr, "JointEffortController was not given a joint\n");
     return false;
   }
 

Modified: pkg/trunk/mechanism/mechanism_control/scripts/spawner.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/spawner.py	2008-11-20 05:10:12 UTC (rev 7046)
+++ pkg/trunk/mechanism/mechanism_control/scripts/spawner.py	2008-11-20 05:51:07 UTC (rev 7047)
@@ -60,7 +60,7 @@
 
     spawned = []
     for i in range(len(resp.ok)):
-        if resp.ok[i]:
+        if ord(resp.ok[i]):
             spawned.append(resp.name[i])
         else:
             print "Failed to spawn %s" % resp.name[i]


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