tf_interactive_marker.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 """
00004 Created on 15/01/15
00005 @author: Sammy Pfeiffer
00006 
00007 # Software License Agreement (BSD License)
00008 #
00009 # Copyright (c) 2016, PAL Robotics, S.L.
00010 # All rights reserved.
00011 #
00012 # Redistribution and use in source and binary forms, with or without
00013 # modification, are permitted provided that the following conditions
00014 # are met:
00015 #
00016 #  * Redistributions of source code must retain the above copyright
00017 #    notice, this list of conditions and the following disclaimer.
00018 #  * Redistributions in binary form must reproduce the above
00019 #    copyright notice, this list of conditions and the following
00020 #    disclaimer in the documentation and/or other materials provided
00021 #    with the distribution.
00022 #  * Neither the name of Willow Garage, Inc. nor the names of its
00023 #    contributors may be used to endorse or promote products derived
00024 #    from this software without specific prior written permission.
00025 #
00026 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00027 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00028 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00029 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00030 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00031 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00032 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00033 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00034 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00035 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00036 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00037 # POSSIBILITY OF SUCH DAMAGE.
00038 
00039 tf_interactive_marker.py contains a commandline
00040 node that sets up a 6DOF Interactive Marker at /posestamped_im/update
00041 and a PoseStamped topic at /posestamped and a
00042 TF transform broadcaster with the pose of the interactive marker.
00043 It prints on the commandline the current commandline transform and URDF
00044 transform as you move the interactive marker.
00045 
00046 You can stop publishing transforms doing right click
00047 and click on Stop Publish transform.
00048 
00049 As the help says:
00050 Generate an interactive marker to setup your transforms.
00051 Usage:
00052 ./tf_interactive_marker.py from_frame to frame position (x,y,z) orientation (r,p,y) or (x,y,z,w)
00053 ./tf_interactive_marker.py <from_frame> <to_frame> x y z r p y [deg]
00054 ./tf_interactive_marker.py <from_frame> <to_frame> x y z x y z w
00055 For example (they do all the same):
00056 ./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 90.0 deg
00057 ./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 1.57
00058 ./tf_interactive_marker.py base_footprint new_frame 1.0 0.0 1.0 0.0 0.7071 0.7071 0.0
00059 
00060 The output looks like:
00061 
00062 Static transform publisher command (with roll pitch yaw):
00063   rosrun tf static_transform_publisher 1.0 0.0 1.0 0.0 -0.0 1.57 base_footprint new_frame 50
00064 
00065 Static transform publisher command (with quaternion):
00066   rosrun tf static_transform_publisher 1.0 0.0 1.0 0.0 0.7068 0.7074 0.0 base_footprint new_frame 50
00067 
00068 Roslaunch line of static transform publisher (rpy):
00069   <node name="from_base_footprint_to_new_frame_static_tf" pkg="tf" type="static_transform_publisher" args="1.0 0.0 1.0 0.0 -0.0 1.57 base_footprint new_frame 50" />
00070 
00071 URDF format:
00072   <origin xyz="1.0 0.0 1.0" rpy="0.0 -0.0 1.57" />
00073 
00074 Example usage video: https://www.youtube.com/watch?v=C9BbFv-C9Zo
00075 
00076 """
00077 
00078 # based on https://github.com/ros-visualization/visualization_tutorials/blob/indigo-devel/interactive_marker_tutorials/scripts/basic_controls.py
00079 # and https://github.com/ros-visualization/visualization_tutorials/blob/indigo-devel/interactive_marker_tutorials/scripts/menu.py
00080 
00081 import sys
00082 from copy import deepcopy
00083 import rospy
00084 from interactive_markers.interactive_marker_server import InteractiveMarkerServer
00085 from interactive_markers.menu_handler import MenuHandler
00086 from visualization_msgs.msg import InteractiveMarkerControl, Marker, InteractiveMarker, \
00087     InteractiveMarkerFeedback, InteractiveMarkerUpdate, InteractiveMarkerPose, MenuEntry
00088 from geometry_msgs.msg import Point, Pose, PoseStamped, Vector3, Quaternion
00089 import tf
00090 from tf.transformations import quaternion_from_euler, euler_from_quaternion
00091 from math import radians
00092 
00093 
00094 class InteractiveMarkerPoseStampedPublisher():
00095 
00096     def __init__(self, from_frame, to_frame, position, orientation):
00097         self.server = InteractiveMarkerServer("posestamped_im")
00098         o = orientation
00099         r, p, y = euler_from_quaternion([o.w, o.x, o.y, o.z])
00100         rospy.loginfo("Publishing transform and PoseStamped from: " +
00101                       from_frame + " to " + to_frame +
00102                       " at " + str(position.x) + " " + str(position.y) + " " + str(position.z) +
00103                       " and orientation " + str(o.x) + " " + str(o.y) +
00104                       " " + str(o.z) + " " + str(o.w) + " or rpy " +
00105                       str(r) + " " + str(p) + " " + str(y))
00106         self.menu_handler = MenuHandler()
00107         self.from_frame = from_frame
00108         self.to_frame = to_frame
00109         # Transform broadcaster
00110         self.tf_br = tf.TransformBroadcaster()
00111 
00112         self.pub = rospy.Publisher('/posestamped', PoseStamped, queue_size=1)
00113         rospy.loginfo("Publishing posestampeds at topic: " + str(self.pub.name))
00114         pose = Pose()
00115         pose.position = position
00116         pose.orientation = orientation
00117         self.last_pose = pose
00118         self.print_commands(pose)
00119         self.makeGraspIM(pose)
00120 
00121         self.server.applyChanges()
00122 
00123     def processFeedback(self, feedback):
00124         """
00125         :type feedback: InteractiveMarkerFeedback
00126         """
00127         s = "Feedback from marker '" + feedback.marker_name
00128         s += "' / control '" + feedback.control_name + "'"
00129 
00130         mp = ""
00131         if feedback.mouse_point_valid:
00132             mp = " at " + str(feedback.mouse_point.x)
00133             mp += ", " + str(feedback.mouse_point.y)
00134             mp += ", " + str(feedback.mouse_point.z)
00135             mp += " in frame " + feedback.header.frame_id
00136 
00137         if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK:
00138             rospy.logdebug(s + ": button click" + mp + ".")
00139 
00140         elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT:
00141             rospy.logdebug(s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + ".")
00142             if feedback.menu_entry_id == 1:
00143                 rospy.logdebug("Start publishing transform...")
00144                 if self.tf_br is None:
00145                     self.tf_br = tf.TransformBroadcaster()
00146             elif feedback.menu_entry_id == 2:
00147                 rospy.logdebug("Stop publishing transform...")
00148                 self.tf_br = None
00149 
00150             # When clicking this event triggers!
00151         elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE:
00152             rospy.logdebug(s + ": pose changed")
00153 
00154         elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN:
00155             rospy.logdebug(s + ": mouse down" + mp + ".")
00156 
00157         elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP:
00158             rospy.logdebug(s + ": mouse up" + mp + ".")
00159 
00160         # TODO: Print here the commands
00161         # tf static transform
00162         self.print_commands(feedback.pose)
00163 
00164 
00165         self.last_pose = deepcopy(feedback.pose)
00166 
00167         self.server.applyChanges()
00168 
00169     def print_commands(self, pose, decimals=4):
00170         p = pose.position
00171         o = pose.orientation
00172 
00173         print "\n---------------------------"
00174         print "Static transform publisher command (with roll pitch yaw):"
00175         common_part = "rosrun tf static_transform_publisher "
00176         pos_part = str(round(p.x, decimals)) + " " + str(round(p.y, decimals)) + " "+ str(round(p.z, decimals))
00177         r, p, y = euler_from_quaternion([o.w, o.x, o.y, o.z])
00178         ori_part = str(round(r, decimals)) + " " + str(round(p, decimals)) + " " + str(round(y, decimals))
00179         static_tf_cmd = common_part + pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50"
00180         print "  " + static_tf_cmd
00181         print
00182         print "Static transform publisher command (with quaternion):"
00183         ori_q = str(round(o.x, decimals)) + " " + str(round(o.y, decimals)) + " " + str(round(o.z, decimals)) + " " + str(round(o.w, decimals))
00184         static_tf_cmd = common_part + pos_part + " " + ori_q + " " + self.from_frame + " " + self.to_frame + " 50"
00185         print "  " + static_tf_cmd
00186         print
00187 
00188         print "Roslaunch line of static transform publisher (rpy):"
00189         node_name = "from_" + self.from_frame + "_to_" + self.to_frame + "_static_tf"
00190         roslaunch_part = '  <node name="' + node_name + '" pkg="tf" type="static_transform_publisher" args="' +\
00191                          pos_part + " " + ori_part + " " + self.from_frame + " " + self.to_frame + " 50" + '" />'
00192         print roslaunch_part
00193         print
00194 
00195         print "URDF format:"  # <origin xyz="0.04149 -0.01221 0.001" rpy="0 0 0" />
00196         print '  <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
00197         print "\n---------------------------"
00198 
00199     def makeArrow(self, msg):
00200         marker = Marker()
00201 
00202         # An arrow that's squashed to easier view the orientation on roll
00203         marker.type = Marker.ARROW
00204         marker.scale.x = 0.15
00205         marker.scale.y = 0.05
00206         marker.scale.z = 0.05
00207         marker.color.r = 1.0
00208         marker.color.g = 0.0
00209         marker.color.b = 0.0
00210         marker.color.a = 1.0
00211 
00212         return marker
00213 
00214     def makeBoxControl(self, msg):
00215         control = InteractiveMarkerControl()
00216         control.always_visible = True
00217         control.markers.append(self.makeArrow(msg))
00218         msg.controls.append(control)
00219         return control
00220 
00221     def makeGraspIM(self, pose):
00222         """
00223         :type pose: Pose
00224         """
00225         int_marker = InteractiveMarker()
00226         int_marker.header.frame_id = self.from_frame
00227         int_marker.pose = pose
00228         int_marker.scale = 0.3
00229 
00230         int_marker.name = "6dof_eef"
00231         int_marker.description = "transform from " + self.from_frame + " to " + self.to_frame
00232 
00233         # insert a box, well, an arrow
00234         self.makeBoxControl(int_marker)
00235         int_marker.controls[0].interaction_mode = InteractiveMarkerControl.MOVE_ROTATE_3D
00236 
00237         control = InteractiveMarkerControl()
00238         control.orientation.w = 1
00239         control.orientation.x = 1
00240         control.orientation.y = 0
00241         control.orientation.z = 0
00242         control.name = "rotate_x"
00243         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00244         int_marker.controls.append(control)
00245 
00246         control = InteractiveMarkerControl()
00247         control.orientation.w = 1
00248         control.orientation.x = 1
00249         control.orientation.y = 0
00250         control.orientation.z = 0
00251         control.name = "move_x"
00252         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00253         int_marker.controls.append(control)
00254 
00255         control = InteractiveMarkerControl()
00256         control.orientation.w = 1
00257         control.orientation.x = 0
00258         control.orientation.y = 1
00259         control.orientation.z = 0
00260         control.name = "rotate_z"
00261         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00262         int_marker.controls.append(control)
00263 
00264         control = InteractiveMarkerControl()
00265         control.orientation.w = 1
00266         control.orientation.x = 0
00267         control.orientation.y = 1
00268         control.orientation.z = 0
00269         control.name = "move_z"
00270         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00271         int_marker.controls.append(control)
00272 
00273         control = InteractiveMarkerControl()
00274         control.orientation.w = 1
00275         control.orientation.x = 0
00276         control.orientation.y = 0
00277         control.orientation.z = 1
00278         control.name = "rotate_y"
00279         control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
00280         int_marker.controls.append(control)
00281 
00282         control = InteractiveMarkerControl()
00283         control.orientation.w = 1
00284         control.orientation.x = 0
00285         control.orientation.y = 0
00286         control.orientation.z = 1
00287         control.name = "move_y"
00288         control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
00289         int_marker.controls.append(control)
00290 
00291         control = InteractiveMarkerControl()
00292         control.orientation.w = 1
00293         control.orientation.x = 0
00294         control.orientation.y = 0
00295         control.orientation.z = 1
00296         control.name = "move_3d"
00297         control.interaction_mode = InteractiveMarkerControl.MOVE_3D
00298         int_marker.controls.append(control)
00299 
00300         self.menu_handler.insert("Publish transform",
00301                                  callback=self.processFeedback)
00302         self.menu_handler.insert("Stop publishing transform",
00303                                  callback=self.processFeedback)
00304 
00305         self.server.insert(int_marker, self.processFeedback)
00306         self.menu_handler.apply(self.server, int_marker.name)
00307 
00308     def run(self):
00309         r = rospy.Rate(20)
00310         while not rospy.is_shutdown():
00311             if self.tf_br is not None:
00312                 pos = self.last_pose.position
00313                 ori = self.last_pose.orientation
00314                 self.tf_br.sendTransform(
00315                                          (pos.x, pos.y, pos.z),
00316                                          (ori.x, ori.y, ori.z, ori.w),
00317                                          rospy.Time.now(),
00318                                          self.to_frame,
00319                                          self.from_frame
00320                                          )
00321             ps = PoseStamped()
00322             ps.pose = self.last_pose
00323             ps.header.frame_id = self.from_frame
00324             ps.header.stamp = rospy.Time.now()
00325             self.pub.publish(ps)
00326             r.sleep()
00327 
00328 
00329 def usage():
00330     print "Generate an interactive marker to setup your transforms."
00331     print "Usage:"
00332     print sys.argv[0] + " from_frame to frame position (x,y,z) orientation (r,p,y) or (x,y,z,w)"
00333     print sys.argv[0] + " <from_frame> <to_frame> x y z r p y [deg]"
00334     print sys.argv[0] + " <from_frame> <to_frame> x y z x y z w"
00335     print "For example (they all do the same):"
00336     print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 90.0 deg"
00337     print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 1.57"
00338     print sys.argv[0] + " base_footprint new_frame 1.0 0.0 1.0 0.0 0.0 0.7071 0.7071"
00339     print "You can stop publishing transforms in the right click menu of the interactive marker."
00340 
00341 if __name__ == "__main__":
00342     rospy.init_node("tf_interactive_marker", anonymous=True)
00343     cleaned_args = rospy.myargv(sys.argv)
00344     if "-h" in cleaned_args or "--help" in cleaned_args:
00345         usage()
00346         exit()
00347     if len(cleaned_args) not in [9, 10]:
00348         print "Arguments error."
00349         usage()
00350         exit()
00351     print len(cleaned_args)
00352     from_frame = cleaned_args[1]
00353     to_frame = cleaned_args[2]
00354     px, py, pz = [float(item) for item in cleaned_args[3:6]]
00355     position = Point(px, py, pz)
00356     if len(cleaned_args) == 9:
00357         r, p, y = [float(item) for item in cleaned_args[6:]]
00358         quat = quaternion_from_euler(r, p, y)
00359         orientation = Quaternion(quat[1], quat[2], quat[3], quat[0])
00360     elif cleaned_args[-1] == 'deg':
00361         r, p, y = [radians(float(item)) for item in cleaned_args[6:-1]]
00362         quat = quaternion_from_euler(r, p, y)
00363         orientation = Quaternion(quat[1], quat[2], quat[3], quat[0])
00364     else:
00365         x, y, z, w = [float(item) for item in cleaned_args[-4:]]
00366         orientation = Quaternion(x, y, z, w)
00367 
00368     ig = InteractiveMarkerPoseStampedPublisher(from_frame, to_frame,
00369                                                position, orientation)
00370     ig.run()


tf_keyboard_cal
Author(s): Dave Coleman , Andy McEvoy
autogenerated on Fri Mar 18 2016 03:24:31