remi_joy_teleop.py

"""
   Licensed under the Apache License, Version 2.0 (the "License");
   you may not use this file except in compliance with the License.
   You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

   Unless required by applicable law or agreed to in writing, software
   distributed under the License is distributed on an "AS IS" BASIS,
   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   See the License for the specific language governing permissions and
   limitations under the License.
"""

import remi.gui as gui
from remi import start, App
import rospy
from geometry_msgs.msg import Twist

MAX_LINEAR_SPEED = 1.0
MAX_ANGULAR_SPEED = 1.0
PUB_PERIOD = 1.0/10.0  # 10hz

class JoystickWidget(gui.Svg):
    def __init__(self, width, height):
        super(JoystickWidget, self).__init__(width, height)
        self.width = width
        self.height = height
        # self.maxDragLength = min(width, height)/3.0*2.0
        self.maxDragLength = min(width, height)/3.0
        self.startPointIndicator = gui.SvgCircle(0, 0, 20)
        self.startPointIndicator.set_fill('rgb(255,200,50)')
        self.startPointIndicator.set_stroke(1, 'white')
        self.endPointIndicator = gui.SvgCircle(0, 0, 10)
        self.endPointIndicator.set_fill('rgb(200,255,50)')
        self.endPointIndicator.set_stroke(1, 'white')
        self.pathLine = gui.SvgLine(0, 0, 0, 0)
        self.startX = 0
        self.startY = 0
        self.append('path_line', self.pathLine)
        self.append('start_point', self.startPointIndicator)
        self.append('end_point', self.endPointIndicator)
        self.drag_state = False
        self.set_on_mouseup_listener(self, 'mouseup')
        self.set_on_mousedown_listener(self, 'mousedown')
        self.set_on_mousemove_listener(self, 'mousemove')
        self.set_on_touchend_listener(self, 'mouseup')
        self.set_on_touchstart_listener(self, 'mousedown')
        self.set_on_touchmove_listener(self, 'mousemove')

        self.EVENT_ONMOVE = "ONJOYSTICKMOVE"
        self.reset_joystick(0, 0)

    def mousemove(self, x, y):
        if self.drag_state:
            self.endPointIndicator.set_position(x, y)
            self.pathLine.set_p2(x, y)
            moveX = min(abs(x-self.startX),self.maxDragLength)/self.maxDragLength * (-1 if x < self.startX else 1)
            moveY = min(abs(y-self.startY),self.maxDragLength)/self.maxDragLength * (1 if y < self.startY else -1)
            params = [moveX,moveY]
            self.eventManager.propagate(self.EVENT_ONMOVE, params)
        
    def mousedown(self, x, y):
        self.drag_state = True
        #self.startPointIndicator.set_position(x, y)
        self.startPointIndicator.set_position(self.width/2, self.height/2)
        #self.pathLine.set_p1(x, y)
        self.pathLine.set_p1(self.width/2, self.height/2)
        # self.startX = x
        # self.startY = y
        self.startX = self.width/2
        self.startY = self.height/2

    def mouseup(self, x, y):
        self.reset_joystick(x, y)

    def reset_joystick(self, x, y):
        self.drag_state = False
        self.startPointIndicator.set_position(self.width/2, self.height/2)
        self.endPointIndicator.set_position(self.width/2, self.height/2)
        self.pathLine.set_coords(self.width/2, self.height/2, self.width/2, self.height/2)
        params = [0,0]
        self.eventManager.propagate(self.EVENT_ONMOVE, params)

    def set_onmove_listener(self, listener, funcname):
        self.eventManager.register_listener(self.EVENT_ONMOVE, listener, funcname)


class MyApp(App):

    def __init__(self, *args):
        super(MyApp, self).__init__(*args)

    def main(self, name='world'):
        self.twist_pub = rospy.Publisher('/key_vel', Twist, queue_size=1)
        rospy.Timer(rospy.Duration(PUB_PERIOD), self.pub_twist)
        self.wid = gui.Widget(620, 650, False, 10)

        self.infoLabel = gui.Label(300, 30, "coords")
        self.last_x = 0.0
        self.last_y = 0.0
        self.joystick = JoystickWidget(600, 600)
        self.joystick.style['background-color'] = 'rgb(255,255,255)'
        self.joystick.set_onmove_listener(self, "joystick_moved")

        self.wid.append('infolabel', self.infoLabel)
        self.wid.append('joystick', self.joystick)
        self.wid.set_on_mousemove_listener(self.joystick, 'reset_joystick')

        # returning the root widget
        return self.wid

    def joystick_moved(self, x, y):  # 0 <= x <= 1     0 <= y <= 1
        self.infoLabel.set_text("move x:%s  y:%s" % (x, y))
        self.last_x = x
        self.last_y = y

    def pub_twist(self, timerevent):
        if self.joystick.drag_state:
            t = Twist()
            if self.last_y >= 0:
                t.linear.x = self.last_y * MAX_LINEAR_SPEED
            else:
                t.linear.x = self.last_y * MAX_ANGULAR_SPEED / 2.0
            t.angular.z = -self.last_x * MAX_ANGULAR_SPEED
            self.twist_pub.publish(t)


if __name__ == "__main__":
    rospy.init_node('remi_joy')
    start(MyApp, address='0.0.0.0')