Add joystick controller based on nipple js

rosboard/handlers.py

@@ -17,6 +17,7 @@ def set_extra_headers(self, path):
 
 class ROSBoardSocketHandler(tornado.websocket.WebSocketHandler):
     sockets = set()
+    joy_msg = None
 
     def initialize(self, node):
         # store the instance of the ROS node that created this WebSocketHandler so we can access it later
@@ -186,6 +187,12 @@ def on_message(self, message):
                 self.node.remote_subs[topic_name].remove(self.id)
             except KeyError:
                 print("KeyError trying to remove sub")
+            self.node.sync_subs()
+
+        # Joy
+        elif argv[0] == ROSBoardSocketHandler.JOY_MSG:
+            ROSBoardSocketHandler.joy_msg = argv[1]
+
 
 ROSBoardSocketHandler.MSG_PING = "p";
 ROSBoardSocketHandler.MSG_PONG = "q";
@@ -198,3 +205,5 @@ def on_message(self, message):
 ROSBoardSocketHandler.PING_SEQ = "s";
 ROSBoardSocketHandler.PONG_SEQ = "s";
 ROSBoardSocketHandler.PONG_TIME = "t";
+
+ROSBoardSocketHandler.JOY_MSG = "j";

rosboard/html/index.html

@@ -8,7 +8,8 @@
         <link rel="stylesheet" href="css/uPlot.min.css">
         <link rel="stylesheet" href="css/leaflet.css">
         <link href="css/index.css" media="all" rel="stylesheet" type="text/css">
-
+
+        <script type="text/javascript" src="https://cdnjs.cloudflare.com/ajax/libs/nipplejs/0.8.7/nipplejs.js"></script>
         <script type="text/javascript" src="js/json5.min.js"></script>
         <script type="text/javascript" src="js/uPlot.iife.min.js"></script>
         <script type="text/javascript" src="js/jquery.transit.min.js"></script>

rosboard/html/js/index.js

@@ -14,6 +14,7 @@ importJsOnce("js/viewers/PolygonViewer.js");
 importJsOnce("js/viewers/DiagnosticViewer.js");
 importJsOnce("js/viewers/TimeSeriesPlotViewer.js");
 importJsOnce("js/viewers/PointCloud2Viewer.js");
+importJsOnce("js/viewers/JoystickController.js");
 
 // GenericViewer must be last
 importJsOnce("js/viewers/GenericViewer.js");

rosboard/html/js/transports/WebSocketV1Transport.js

@@ -7,6 +7,8 @@ class WebSocketV1Transport {
       this.onTopics = onTopics ? onTopics.bind(this) : null;
       this.onSystem = onSystem ? onSystem.bind(this) : null;
       this.ws = null;
+      this.joystickX = 0.0;
+      this.joystickY = 0.0;
     }
 
     connect() {
@@ -50,6 +52,10 @@ class WebSocketV1Transport {
         else if(wsMsgType === WebSocketV1Transport.MSG_TOPICS && that.onTopics) that.onTopics(data[1]);
         else if(wsMsgType === WebSocketV1Transport.MSG_SYSTEM && that.onSystem) that.onSystem(data[1]);
         else console.log("received unknown message: " + wsmsg.data);
+
+        this.send(JSON.stringify([WebSocketV1Transport.JOY_MSG, {
+          ["x"]: that.joystickX.toFixed(3),
+          ["y"]: that.joystickY.toFixed(3),}]));
       }
     }
 
@@ -64,6 +70,11 @@ class WebSocketV1Transport {
     unsubscribe({topicName}) {
       this.ws.send(JSON.stringify([WebSocketV1Transport.MSG_UNSUB, {topicName: topicName}]));
     }
+
+    update_joy({joystickX, joystickY}) {
+      this.joystickX = joystickX;
+      this.joystickY = joystickY;
+    }
   }
 
   WebSocketV1Transport.MSG_PING = "p";
@@ -76,4 +87,6 @@ class WebSocketV1Transport {
 
   WebSocketV1Transport.PING_SEQ= "s";
   WebSocketV1Transport.PONG_SEQ = "s";
-  WebSocketV1Transport.PONG_TIME = "t";
+  WebSocketV1Transport.PONG_TIME = "t";
+
+  WebSocketV1Transport.JOY_MSG = "j";

rosboard/html/js/viewers/JoystickController.js

@@ -0,0 +1,59 @@
+"use strict";
+
+class JoystickController extends Viewer {
+  /**
+    * Gets called when Viewer is first initialized.
+    * @override
+  **/
+  onCreate() {
+    this.viewer = $('<div></div>')
+      .css({'font-size': '11pt'
+    , "filter": "invert(100%) saturate(50%)"})
+      .appendTo(this.card.content);
+
+    this.joyId = "joy-" + Math.floor(Math.random()*10000);
+    this.joy = $('<div id="' + this.joyId + '"></div>')
+      .css({
+        "height": "250px",
+      })
+      .appendTo(this.viewer);
+
+    var options = {
+        zone: document.getElementById(this.joyId),
+        mode: 'semi',
+        color: 'blue', 
+        size: 150,
+        catchDistance: 300,
+    };
+    var manager = nipplejs.create(options);
+    manager.on('start', function(evt, data) {
+      let joystickX = 0.0;
+      let joystickY = 0.0;
+      currentTransport.update_joy({joystickX, joystickY});
+    }).on('end', function(evt, data) {
+      let joystickX = 0.0;
+      let joystickY = 0.0;
+      currentTransport.update_joy({joystickX, joystickY});
+    }).on('move', function(evt, data) {
+      let radian = data['angle']['radian'];
+      let distance = data['distance'];
+      let joystickX = Math.max(Math.min(Math.cos(radian)/75*distance, 1), -1);
+      let joystickY = -1*Math.max(Math.min(Math.sin(radian)/75*distance , 1), -1);
+      currentTransport.update_joy({joystickX, joystickY});
+    });
+  }
+
+  onData(msg) {
+    this.card.title.text(msg._topic_name);
+  }
+}
+
+JoystickController.friendlyName = "JoystickController";
+
+JoystickController.supportedTypes = [
+    "geometry_msgs/msg/Twist",
+];
+
+JoystickController.maxUpdateRate = 10.0;
+
+Viewer.registerViewer(JoystickController);

rosboard/rosboard.py

@@ -16,6 +16,7 @@
     print("ROS not detected. Please source your ROS environment\n(e.g. 'source /opt/ros/DISTRO/setup.bash')")
     exit(1)
 
+from geometry_msgs.msg import Twist
 from rosgraph_msgs.msg import Log
 
 from rosboard.serialization import ros2dict
@@ -57,8 +58,10 @@ def __init__(self, node_name = "rosboard_node"):
             # ros2 docs don't explain why but we need this magic.
             self.sub_rosout = rospy.Subscriber("/rosout", Log, lambda x:x)
 
+        self.twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=100)
+
         tornado_settings = {
-            'debug': True, 
+            'debug': True,
             'static_path': os.path.join(os.path.dirname(os.path.realpath(__file__)), 'html')
         }
 
@@ -91,6 +94,9 @@ def __init__(self, node_name = "rosboard_node"):
         # loop to keep track of latencies and clock differences for each socket
         threading.Thread(target = self.pingpong_loop, daemon = True).start()
 
+        # loop to send client joy message to ros topic
+        threading.Thread(target = self.joy_loop, daemon = True).start()
+
         self.lock = threading.Lock()
 
         rospy.loginfo("ROSboard listening on :%d" % self.port)
@@ -105,7 +111,7 @@ def get_msg_class(self, msg_type):
         or
             "std_msgs/msg/Int32"
         it imports the message class into Python and returns the class, i.e. the actual std_msgs.msg.Int32
-        
+
         Returns none if the type is invalid (e.g. if user hasn't bash-sourced the message package).
         """
         try:
@@ -122,6 +128,20 @@ def get_msg_class(self, msg_type):
             rospy.logerr(str(e))
             return None
 
+    def joy_loop(self):
+        """
+        Sending joy message from client
+        """
+        twist = Twist()
+        while True:
+            time.sleep(0.1)
+            if not isinstance(ROSBoardSocketHandler.joy_msg, dict):
+                continue
+            if 'x' in ROSBoardSocketHandler.joy_msg and 'y' in ROSBoardSocketHandler.joy_msg:
+                twist.linear.x = -float(ROSBoardSocketHandler.joy_msg['y']) * 3.0
+                twist.angular.z = -float(ROSBoardSocketHandler.joy_msg['x']) * 2.0
+            self.twist_pub.publish(twist)
+
     def pingpong_loop(self):
         """
         Loop to send pings to all active sockets every 5 seconds.
@@ -244,7 +264,7 @@ def sync_subs(self):
         except Exception as e:
             rospy.logwarn(str(e))
             traceback.print_exc()
-        
+
         self.lock.release()
 
     def on_system_stats(self, system_stats):
@@ -331,7 +351,7 @@ def on_ros_msg(self, msg, topic_info):
         # log last time we received data on this topic
         self.last_data_times_by_topic[topic_name] = t
 
-        # broadcast it to the listeners that care    
+        # broadcast it to the listeners that care
         self.event_loop.add_callback(
             ROSBoardSocketHandler.broadcast,
             [ROSBoardSocketHandler.MSG_MSG, ros_msg_dict]