Merge pull request #120 from furushchev/remap

add joy_remap and its sample

joy/CMakeLists.txt

@@ -29,6 +29,6 @@ install(TARGETS joy_node
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 endif(HAVE_LINUX_JOYSTICK_H)
 
-install(DIRECTORY migration_rules
+install(DIRECTORY migration_rules scripts config launch
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-  )
+  USE_SOURCE_PERMISSIONS)

joy/config/ps4joy.yaml

@@ -0,0 +1,47 @@
+mappings:
+  axes:
+    - axes[0]
+    - axes[1]
+    - axes[2]
+    - axes[5]
+    - axes[6] * 2.0
+    - axes[7] * 2.0
+    - axes[8] * -2.0
+    - 0.0
+    - max(axes[10], 0.0) * -1.0
+    - min(axes[9], 0.0)
+    - min(axes[10], 0.0)
+    - max(axes[9], 0.0) * -1.0
+    - (axes[3] - 1.0) * 0.5
+    - (axes[4] - 1.0) * 0.5
+    - buttons[4] * -1.0
+    - buttons[5] * -1.0
+    - buttons[3] * -1.0
+    - buttons[2] * -1.0
+    - buttons[1] * -1.0
+    - buttons[0] * -1.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+  buttons:
+    - buttons[8]
+    - buttons[10]
+    - buttons[11]
+    - buttons[9]
+    - max(axes[10], 0.0)
+    - min(axes[9], 0.0) * -1.0
+    - min(axes[10], 0.0) * -1.0
+    - max(axes[9], 0.0)
+    - buttons[6]
+    - buttons[7]
+    - buttons[4]
+    - buttons[5]
+    - buttons[3]
+    - buttons[2]
+    - buttons[1]
+    - buttons[0]
+    - buttons[12]

joy/launch/ps4joy.launch

@@ -0,0 +1,12 @@
+<launch>
+  <!-- assumes ds4drv is running -->
+  <node name="joy_node" pkg="joy" type="joy_node">
+    <remap from="joy" to="joy_orig"/>
+  </node>
+  <!-- remap joy to emulate ps3joy mappings -->
+  <node name="joy_remap" pkg="joy" type="joy_remap.py">
+    <remap from="joy_in" to="joy_orig"/>
+    <remap from="joy_out" to="joy"/>
+    <rosparam command="load" file="$(find joy)/config/ps4joy.yaml"/>
+  </node>
+</launch>

joy/scripts/joy_remap.py

@@ -0,0 +1,121 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+# Author: furushchev <furushchev@jsk.imi.i.u-tokyo.ac.jp>
+
+import ast
+import operator as op
+import rospy
+import traceback
+from sensor_msgs.msg import Joy
+
+
+class RestrictedEvaluator(object):
+    def __init__(self):
+        self.operators = {
+            ast.Add: op.add,
+            ast.Sub: op.sub,
+            ast.Mult: op.mul,
+            ast.Div: op.truediv,
+            ast.BitXor: op.xor,
+            ast.USub: op.neg,
+        }
+        self.functions = {
+            'abs': lambda x: abs(x),
+            'max': lambda *x: max(*x),
+            'min': lambda *x: min(*x),
+        }
+
+    def _reval_impl(self, node, variables):
+        if isinstance(node, ast.Num):
+            return node.n
+        elif isinstance(node, ast.BinOp):
+            op = self.operators[type(node.op)]
+            return op(self._reval_impl(node.left, variables),
+                      self._reval_impl(node.right, variables))
+        elif isinstance(node, ast.UnaryOp):
+            op = self.operators[type(node.op)]
+            return op(self._reval_impl(node.operand, variables))
+        elif isinstance(node, ast.Call) and node.func.id in self.functions:
+            func = self.functions[node.func.id]
+            args = [self._reval_impl(n, variables) for n in node.args]
+            return func(*args)
+        elif isinstance(node, ast.Name) and node.id in variables:
+            return variables[node.id]
+        elif isinstance(node, ast.Subscript) and node.value.id in variables:
+            var = variables[node.value.id]
+            idx = node.slice.value.n
+            try:
+                return var[idx]
+            except IndexError:
+                raise IndexError("Variable '%s' out of range: %d >= %d" % (node.value.id, idx, len(var)))
+        else:
+            raise TypeError("Unsupported operation: %s" % node)
+
+    def reval(self, expr, variables):
+        expr = str(expr)
+        if len(expr) > 1000:
+            raise ValueError("The length of an expression must not be more than 1000 characters")
+        try:
+            return self._reval_impl(ast.parse(expr, mode='eval').body, variables)
+        except Exception as e:
+            rospy.logerr(traceback.format_exc())
+            raise e
+
+
+class JoyRemap(object):
+    def __init__(self):
+        self.evaluator = RestrictedEvaluator()
+        self.mappings = self.load_mappings("~mappings")
+        self.warn_remap("joy_out")
+        self.pub = rospy.Publisher(
+            "joy_out", Joy, queue_size=1)
+        self.warn_remap("joy_in")
+        self.sub = rospy.Subscriber(
+            "joy_in", Joy, self.callback,
+            queue_size=rospy.get_param("~queue_size", None))
+
+    def load_mappings(self, ns):
+        btn_remap = rospy.get_param(ns + "/buttons", [])
+        axes_remap = rospy.get_param(ns + "/axes", [])
+        rospy.loginfo("loaded remapping: %d buttons, %d axes" % (len(btn_remap), len(axes_remap)))
+        return {"buttons": btn_remap, "axes": axes_remap}
+
+    def warn_remap(self, name):
+        if name == rospy.remap_name(name):
+            rospy.logwarn("topic '%s' is not remapped" % name)
+
+    def callback(self, in_msg):
+        out_msg = Joy(header=in_msg.header)
+        map_axes = self.mappings["axes"]
+        map_btns = self.mappings["buttons"]
+        out_msg.axes = [0.0] * len(map_axes)
+        out_msg.buttons = [0] * len(map_btns)
+        in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons}
+        for i, exp in enumerate(map_axes):
+            try:
+                out_msg.axes[i] = self.evaluator.reval(exp, in_dic)
+            except NameError as e:
+                rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e)
+            except UnboundLocalError as e:
+                rospy.logerr("Wrong form: %s" % e)
+            except Exception as e:
+                raise e
+
+        for i, exp in enumerate(map_btns):
+            try:
+                if self.evaluator.reval(exp, in_dic) > 0:
+                    out_msg.buttons[i] = 1
+            except NameError as e:
+                rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e)
+            except UnboundLocalError as e:
+                rospy.logerr("Wrong form: %s" % e)
+            except Exception as e:
+                raise e
+
+        self.pub.publish(out_msg)
+
+
+if __name__ == '__main__':
+    rospy.init_node("joy_remap")
+    n = JoyRemap()
+    rospy.spin()