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

[<vij...@us...>]

Revision: 9567
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=9567&view=rev
Author:   vijaypradeep
Date:     2009-01-16 22:04:43 +0000 (Fri, 16 Jan 2009)

Log Message:
-----------
Working on python arm_commander scripts for calibration

Modified Paths:
--------------
    pkg/trunk/calibration/kinematic_calibration/manifest.xml
    pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch

Added Paths:
-----------
    pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
    pkg/trunk/calibration/kinematic_calibration/src/goto_next.py

Modified: pkg/trunk/calibration/kinematic_calibration/manifest.xml
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/manifest.xml	2009-01-16 21:53:38 UTC (rev 9566)
+++ pkg/trunk/calibration/kinematic_calibration/manifest.xml	2009-01-16 22:04:43 UTC (rev 9567)
@@ -12,6 +12,7 @@
   <depend package="image_msgs" />
   <depend package="newmat10"/>
   <depend package="rosthread"/>
+  <depend package="pr2_mechanism_controllers" />
   <export>
     <cpp cflags="-I${prefix}/msg/cpp"/>
   </export>

Added: pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py	                        (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py	2009-01-16 22:04:43 UTC (rev 9567)
@@ -0,0 +1,105 @@
+#!/usr/bin/env python
+import rostools
+rostools.update_path('kinematic_calibration')
+
+import rospy
+import sys
+from roscpp.msg import Empty
+from pr2_mechanism_controllers.srv import *
+from pr2_mechanism_controllers.msg import *
+from robot_msgs.msg import JointCmd
+
+cmd_count = 0
+
+def goto_next_callback(data, callback_args):
+	arm_cmd = callback_args[0]
+	start_topic = callback_args[1]
+	head_cmd = callback_args[2]
+	head_pub = callback_args[3]
+	
+	global cmd_count
+	print "Callback Called"
+	print "  Waiting for Service:\n  (%s)..." % start_topic
+	rospy.wait_for_service(start_topic)
+	print "  Service Found!"
+	print "  Sending Cmd #%u" % cmd_count
+	if cmd_count < len(arm_cmd) :
+		print "  *** Arm Command ***"
+		print arm_cmd[cmd_count]
+		start_srv = rospy.ServiceProxy(start_topic, TrajectoryStart)
+		joint_traj = JointTraj([JointTrajPoint(arm_cmd[cmd_count],0)])
+		start_resp = start_srv.call(TrajectoryStartRequest(joint_traj,0,1))
+		print "Response:"
+		print "  Traj ID: %u" % start_resp.trajectoryid
+		print "  Timestamps:"
+		print start_resp.timestamps
+
+		print "  ***  Head Command ***"		
+		print head_cmd[cmd_count]
+		head_pub.publish(JointCmd(['head_pan_joint', 'head_tilt_joint'],[0.0,0.0],head_cmd[cmd_count],[0.0, 0.0],[0.0, 0.0]))
+
+	else :
+		print "No More Commands left in queue!"
+
+
+	print "************"
+	cmd_count = cmd_count + 1
+        
+
+if __name__ == '__main__':
+	print "Running python code"
+	rospy.init_node('arm_commander', sys.argv, anonymous=False)
+
+	headers_string = rospy.get_param('~joint_headers')
+        print "Got headers from param:\n%s" % headers_string
+	commands_string = rospy.get_param('~joint_commands')
+        print "Got commands from param:\n%s" % commands_string
+
+	headers = headers_string.split()
+
+	cmd_all = [[float(x) for x in cur_line.split()] for cur_line in commands_string.split("\n")]
+	cmd = [x for x in cmd_all if len(x)==len(headers)]
+	print "Commands"
+	print cmd
+
+	arm_controller = 'right_arm_trajectory_controller'
+	arm_query_topic = arm_controller + '/TrajectoryQuery'
+	arm_start_topic = arm_controller + '/TrajectoryStart'
+
+	print "Waiting Query Service: " + arm_query_topic
+	rospy.wait_for_service(arm_query_topic)
+	print "Found Query Service!"
+
+        query_srv = rospy.ServiceProxy(arm_query_topic, TrajectoryQuery)
+        query_resp = query_srv.call(TrajectoryQueryRequest(0))
+	head_publisher = rospy.Publisher('head_controller/set_command_array', JointCmd)
+
+	print "Headers"
+	print headers
+
+	print "Queried Jointnames:"
+	print query_resp.jointnames
+
+	print "***** Remapping Arm Commands *****"
+	arm_mapping = [headers.index(x) for x in query_resp.jointnames]
+        print "Mapping:"
+	print arm_mapping
+        arm_cmd = [[y[x] for x in arm_mapping] for y in cmd]
+	print "First remapped arm cmd:"
+	print arm_cmd[0]
+	print "Successfully Remapped Arm Commands"
+
+	print "***** Remapping Head Commands *****"
+	head_mapping = [headers.index(x) for x in ['HeadPan', 'HeadTilt']]
+	head_cmd = [[y[x] for x in head_mapping] for y in cmd]
+	print "First remapped head cmd:"
+	print head_cmd[0]
+	print "Successfully Remapped Head Commands"
+
+	sub = rospy.Subscriber("~goto_next", Empty, goto_next_callback,
+			       [arm_cmd, arm_start_topic, head_cmd, head_publisher])
+
+	print "***** Ready to Send Trajectories *****"
+
+	rospy.spin()
+


Property changes on: pkg/trunk/calibration/kinematic_calibration/src/arm_commander.py
___________________________________________________________________
Added: svn:executable
   + *

Added: pkg/trunk/calibration/kinematic_calibration/src/goto_next.py
===================================================================
--- pkg/trunk/calibration/kinematic_calibration/src/goto_next.py	                        (rev 0)
+++ pkg/trunk/calibration/kinematic_calibration/src/goto_next.py	2009-01-16 22:04:43 UTC (rev 9567)
@@ -0,0 +1,18 @@
+#!/usr/bin/env python
+
+import rostools; rostools.update_path('kinematic_calibration') 
+import sys
+import rospy
+from roscpp.msg import Empty
+from time import sleep
+
+if __name__ == '__main__':
+	prefix = 'arm_commander'
+	topic = prefix + '/goto_next'
+	command_publisher = rospy.Publisher(topic, Empty)
+    	rospy.init_node('goto_next', anonymous=True)
+	sleep(1)
+	cmd = Empty()
+	command_publisher.publish( cmd )
+    	sleep(1)
+	print 'Command sent!'


Property changes on: pkg/trunk/calibration/kinematic_calibration/src/goto_next.py
___________________________________________________________________
Added: svn:executable
   + *

Modified: pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch
===================================================================
--- pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch	2009-01-16 21:53:38 UTC (rev 9566)
+++ pkg/trunk/demos/pr2_gazebo/pr2_default_controllers.launch	2009-01-16 22:04:43 UTC (rev 9567)
@@ -2,8 +2,6 @@
     <param name="base_controller/odom_publish_rate" value="10" />
 
     <!--node pkg="mechanism_control" type="mech.py" args="sp $(find pr2_gazebo)/controllers_2dnav_test.xml" respawn="false" output="screen" /-->
-    <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
-    <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/r_arm_default_controller.xml" respawn="false" output="screen" /> <!-- load default arm controller -->
     <!--node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_base.xml" output="screen"/-->
     <node pkg="mechanism_control" type="spawner.py" args="$(find pr2_alpha)/controllers_base.xml" output="screen"/>
     <node pkg="mechanism_control" type="mech.py" args="sp $(find wg_robot_description)/pr2_prototype1/controllers_head_torso_gazebo.xml" output="screen"/>


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