mouse_teleop.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 #
00004 # Copyright (c) 2015 Enrique Fernandez
00005 # Released under the BSD License.
00006 #
00007 # Authors:
00008 #   * Enrique Fernandez
00009 
00010 import Tkinter
00011 
00012 import rospy
00013 from geometry_msgs.msg import Twist, Vector3
00014 
00015 import numpy
00016 
00017 
00018 class MouseTeleop():
00019     def __init__(self):
00020         # Retrieve params:
00021         self._frequency = rospy.get_param('~frequency', 0.0)
00022         self._scale = rospy.get_param('~scale', 1.0)
00023         self._holonomic = rospy.get_param('~holonomic', False)
00024 
00025         # Create twist publisher:
00026         self._pub_cmd = rospy.Publisher('mouse_vel', Twist, queue_size=100)
00027 
00028         # Initialize twist components to zero:
00029         self._v_x = 0.0
00030         self._v_y = 0.0
00031         self._w   = 0.0
00032 
00033         # Initialize mouse position (x, y) to None (unknown); it's initialized
00034         # when the mouse button is pressed on the _start callback that handles
00035         # that event:
00036         self._x = None
00037         self._y = None
00038 
00039         # Create window:
00040         self._root = Tkinter.Tk()
00041         self._root.title('Mouse Teleop')
00042 
00043         # Make window non-resizable:
00044         self._root.resizable(0, 0)
00045 
00046         # Create canvas:
00047         self._canvas = Tkinter.Canvas(self._root, bg='white')
00048 
00049         # Create canvas objects:
00050         self._canvas.create_arc(0, 0, 0, 0, fill='red', outline='red',
00051                 width=1, style=Tkinter.PIESLICE, start=90.0, tag='w')
00052         self._canvas.create_line(0, 0, 0, 0, fill='blue', width=4, tag='v_x')
00053 
00054         if self._holonomic:
00055             self._canvas.create_line(0, 0, 0, 0,
00056                     fill='blue', width=4, tag='v_y')
00057 
00058         # Create canvas text objects:
00059         self._text_v_x = Tkinter.StringVar()
00060         if self._holonomic:
00061             self._text_v_y = Tkinter.StringVar()
00062         self._text_w   = Tkinter.StringVar()
00063 
00064         self._label_v_x = Tkinter.Label(self._root,
00065                 anchor=Tkinter.W, textvariable=self._text_v_x)
00066         if self._holonomic:
00067             self._label_v_y = Tkinter.Label(self._root,
00068                     anchor=Tkinter.W, textvariable=self._text_v_y)
00069         self._label_w = Tkinter.Label(self._root,
00070                 anchor=Tkinter.W, textvariable=self._text_w)
00071 
00072         if self._holonomic:
00073             self._text_v_x.set('v_x = %0.2f m/s' % self._v_x)
00074             self._text_v_y.set('v_y = %0.2f m/s' % self._v_y)
00075             self._text_w.set(  'w   = %0.2f deg/s' % self._w)
00076         else:
00077             self._text_v_x.set('v = %0.2f m/s' % self._v_x)
00078             self._text_w.set(  'w = %0.2f deg/s' % self._w)
00079 
00080         self._label_v_x.pack()
00081         if self._holonomic:
00082             self._label_v_y.pack()
00083         self._label_w.pack()
00084 
00085         # Bind event handlers:
00086         self._canvas.bind('<Button-1>', self._start)
00087         self._canvas.bind('<ButtonRelease-1>', self._release)
00088 
00089         self._canvas.bind('<Configure>', self._configure)
00090 
00091         if self._holonomic:
00092             self._canvas.bind('<B1-Motion>', self._mouse_motion_linear)
00093             self._canvas.bind('<Shift-B1-Motion>', self._mouse_motion_angular)
00094 
00095             self._root.bind('<Shift_L>', self._change_to_motion_angular)
00096             self._root.bind('<KeyRelease-Shift_L>',
00097                     self._change_to_motion_linear)
00098         else:
00099             self._canvas.bind('<B1-Motion>', self._mouse_motion_angular)
00100 
00101         self._canvas.pack()
00102 
00103         # If frequency is positive, use synchronous publishing mode:
00104         if self._frequency > 0.0:
00105             # Create timer for the given frequency to publish the twist:
00106             period = rospy.Duration(1.0 / self._frequency)
00107 
00108             self._timer = rospy.Timer(period, self._publish_twist)
00109 
00110         # Start window event manager main loop:
00111         self._root.mainloop()
00112 
00113     def __del__(self):
00114         if self._frequency > 0.0:
00115             self._timer.shutdown()
00116 
00117         self._root.quit()
00118 
00119     def _start(self, event):
00120         self._x, self._y = event.y, event.x
00121 
00122         self._y_linear = self._y_angular = 0
00123 
00124         self._v_x = self._v_y = self._w = 0.0
00125 
00126     def _release(self, event):
00127         self._v_x = self._v_y = self._w = 0.0
00128 
00129         self._send_motion()
00130 
00131     def _configure(self, event):
00132         self._width, self._height = event.height, event.width
00133 
00134         self._c_x = self._height / 2.0
00135         self._c_y = self._width  / 2.0
00136 
00137         self._r = min(self._height, self._width) * 0.25
00138 
00139     def _mouse_motion_linear(self, event):
00140         self._v_x, self._v_y = self._relative_motion(event.y, event.x)
00141 
00142         self._send_motion()
00143 
00144     def _mouse_motion_angular(self, event):
00145         self._v_x, self._w = self._relative_motion(event.y, event.x)
00146 
00147         self._send_motion()
00148 
00149     def _update_coords(self, tag, x0, y0, x1, y1):
00150         x0 += self._c_x
00151         y0 += self._c_y
00152 
00153         x1 += self._c_x
00154         y1 += self._c_y
00155 
00156         self._canvas.coords(tag, (x0, y0, x1, y1))
00157 
00158     def _draw_v_x(self, v):
00159         x = -v * float(self._width)
00160 
00161         self._update_coords('v_x', 0, 0, 0, x)
00162 
00163     def _draw_v_y(self, v):
00164         y = -v * float(self._height)
00165 
00166         self._update_coords('v_y', 0, 0, y, 0)
00167 
00168     def _draw_w(self, w):
00169         x0 = y0 = -self._r
00170         x1 = y1 =  self._r
00171 
00172         self._update_coords('w', x0, y0, x1, y1)
00173 
00174         yaw = w * numpy.rad2deg(self._scale)
00175 
00176         self._canvas.itemconfig('w', extent=yaw)
00177 
00178     def _send_motion(self):
00179         v_x = self._v_x * self._scale
00180         v_y = self._v_y * self._scale
00181         w   = self._w   * self._scale
00182 
00183         linear  = Vector3(v_x, v_y, 0.0)
00184         angular = Vector3(0.0, 0.0,   w)
00185 
00186         self._draw_v_x(self._v_x)
00187         if self._holonomic:
00188             self._draw_v_y(self._v_y)
00189         self._draw_w(self._w)
00190 
00191         if self._holonomic:
00192             self._text_v_x.set('v_x = %0.2f m/s' % self._v_x)
00193             self._text_v_y.set('v_y = %0.2f m/s' % self._v_y)
00194             self._text_w.set(  'w   = %0.2f deg/s' % numpy.rad2deg(self._w))
00195         else:
00196             self._text_v_x.set('v = %0.2f m/s' % self._v_x)
00197             self._text_w.set(  'w = %0.2f deg/s' % numpy.rad2deg(self._w))
00198 
00199         twist = Twist(linear, angular)
00200         self._pub_cmd.publish(twist)
00201 
00202     def _publish_twist(self, event):
00203         self._send_motion()
00204 
00205     def _relative_motion(self, x, y):
00206         dx = self._x - x
00207         dy = self._y - y
00208 
00209         dx /= float(self._width)
00210         dy /= float(self._height)
00211 
00212         dx = max(-1.0, min(dx, 1.0))
00213         dy = max(-1.0, min(dy, 1.0))
00214 
00215         return dx, dy
00216 
00217     def _change_to_motion_linear(self, event):
00218         if self._y is not None:
00219             y = event.x
00220 
00221             self._y_angular = self._y - y
00222             self._y         = self._y_linear + y
00223 
00224     def _change_to_motion_angular(self, event):
00225         if self._y is not None:
00226             y = event.x
00227 
00228             self._y_linear = self._y - y
00229             self._y        = self._y_angular + y
00230 
00231 
00232 def main():
00233     rospy.init_node('mouse_teleop')
00234 
00235     MouseTeleop()
00236 
00237 if __name__ == '__main__':
00238     try:
00239         main()
00240     except rospy.ROSInterruptException:
00241         pass


mouse_teleop
Author(s): Enrique Fernandez
autogenerated on Sun Apr 23 2017 02:40:16