00001
00002
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
00079
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
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
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
00161
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:"
00196 print ' <origin xyz="' + pos_part + '" rpy="' + ori_part + '" />'
00197 print "\n---------------------------"
00198
00199 def makeArrow(self, msg):
00200 marker = Marker()
00201
00202
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
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()