Merge branch 'transfer2' of github.com-myrobotlab:MyRobotLab/myrobotl…

…ab into inmoov-and-statemachine-1

src/main/java/org/myrobotlab/service/InMoov2.java

@@ -1309,6 +1309,52 @@ public void onMoveTorso(Map<String, Double> map) {
     }
   }
 
+  public void onMoveHead(Map<String, Double> map) {
+    InMoov2Head head = (InMoov2Head) getPeer("head");
+    if (head != null) {
+      head.onMove(map);
+    }
+  }
+
+  public void onMoveLeftArm(Map<String, Double> map) {
+    InMoov2Arm leftArm = (InMoov2Arm) getPeer("leftArm");
+    if (leftArm != null) {
+      leftArm.onMove(map);
+    }
+  }
+
+  public void onMoveLeftHand(Map<String, Double> map) {
+    InMoov2Hand leftHand = (InMoov2Hand) getPeer("leftHand");
+    if (leftHand != null) {
+      leftHand.onMove(map);
+    }
+  }
+
+  // public Message publishPython(String method, Object...data) {
+  // return Message.createMessage(getName(), getName(), method, data);
+  // }
+
+  public void onMoveRightArm(Map<String, Double> map) {
+    InMoov2Arm rightArm = (InMoov2Arm) getPeer("rightArm");
+    if (rightArm != null) {
+      rightArm.onMove(map);
+    }
+  }
+
+  public void onMoveRightHand(Map<String, Double> map) {
+    InMoov2Hand rightHand = (InMoov2Hand) getPeer("rightHand");
+    if (rightHand != null) {
+      rightHand.onMove(map);
+    }
+  }
+
+  public void onMoveTorso(Map<String, Double> map) {
+    InMoov2Torso torso = (InMoov2Torso) getPeer("torso");
+    if (torso != null) {
+      torso.onMove(map);
+    }
+  }
+
 
 
 //  public Message publishPython(String method, Object...data) {

src/main/java/org/myrobotlab/service/WebXR.java

@@ -4,15 +4,17 @@
 import java.util.Map;
 
 import org.myrobotlab.framework.Service;
+import org.myrobotlab.kinematics.Point;
 import org.myrobotlab.logging.Level;
 import org.myrobotlab.logging.LoggerFactory;
 import org.myrobotlab.logging.LoggingFactory;
 import org.myrobotlab.math.MapperSimple;
 import org.myrobotlab.service.config.WebXRConfig;
+import org.myrobotlab.service.data.Event;
 import org.myrobotlab.service.data.Pose;
 import org.slf4j.Logger;
 
-public class WebXR extends Service {
+public class WebXR extends Service<WebXRConfig> {
 
   private static final long serialVersionUID = 1L;
 
@@ -22,66 +24,130 @@ public WebXR(String n, String id) {
     super(n, id);
   }
 
-  public Pose publishPose(Pose pose) {
-    log.warn("publishPose {}", pose);
-    System.out.println(pose.toString());
+  public Event publishEvent(Event event) {
+    if (log.isDebugEnabled()) {
+      log.debug("publishEvent {}", event);
+    }
+
+    String path = String.format("event.%s.%s", event.meta.get("handedness"), event.type);
+    if (event.value != null) {
+      path = path + "." + event.value.toString();
+    }
 
+    if (config.eventMappings.containsKey(path)) {
+      // TODO - future might be events -> message that goes to ServoMixer .e.g mixer.playGesture("closeHand")
+      // or sadly Python execute script for total chaos :P
+      invoke("publishJointAngles", config.eventMappings.get(path));
+    }
+
+    return event;
+  }
+
+  /**
+   * Pose is the x,y,z and pitch, roll, yaw of all the devices WebXR found.
+   * Hopefully, this includes headset, and hand controllers.
+   * WebXRConfig processes a mapping between these values (usually in radians) to
+   * servo positions, and will then publish JointAngles for servos.
+   * 
+   * @param pose
+   * @return
+   */
+  public Pose publishPose(Pose pose) {
+    if (log.isDebugEnabled()) {
+      log.debug("publishPose {}", pose);
+    }    
     // process mappings config into joint angles
-    Map<String, Double> map = new HashMap<>();  
+    Map<String, Double> map = new HashMap<>();
 
-    WebXRConfig c = (WebXRConfig)config;
     String path = String.format("%s.orientation.roll", pose.name);
-    if (c.mappings.containsKey(path)) {
-      Map<String, MapperSimple> mapper = c.mappings.get(path);
-      for (String name: mapper.keySet()) {
+    if (config.controllerMappings.containsKey(path)) {
+      Map<String, MapperSimple> mapper = config.controllerMappings.get(path);
+      for (String name : mapper.keySet()) {
         map.put(name, mapper.get(name).calcOutput(pose.orientation.roll));
       }
     }
-    
+
     path = String.format("%s.orientation.pitch", pose.name);
-    if (c.mappings.containsKey(path)) {
-      Map<String, MapperSimple> mapper = c.mappings.get(path);
-      for (String name: mapper.keySet()) {
+    if (config.controllerMappings.containsKey(path)) {
+      Map<String, MapperSimple> mapper = config.controllerMappings.get(path);
+      for (String name : mapper.keySet()) {
         map.put(name, mapper.get(name).calcOutput(pose.orientation.pitch));
       }
     }
 
     path = String.format("%s.orientation.yaw", pose.name);
-    if (c.mappings.containsKey(path)) {
-      Map<String, MapperSimple> mapper = c.mappings.get(path);
-      for (String name: mapper.keySet()) {
+    if (config.controllerMappings.containsKey(path)) {
+      Map<String, MapperSimple> mapper = config.controllerMappings.get(path);
+      for (String name : mapper.keySet()) {
         map.put(name, mapper.get(name).calcOutput(pose.orientation.yaw));
       }
     }
-
-    invoke("publishJointAngles", map);
 
+    InverseKinematics3D ik = (InverseKinematics3D)Runtime.getService("ik3d");
+    if (ik != null && pose.name.equals("left")) {
+      ik.setCurrentArm("left", InMoov2Arm.getDHRobotArm("i01", "left"));
+
+      ik.centerAllJoints("left");
+
+      for (int i = 0; i < 1000; ++i) {
+
+        ik.centerAllJoints("left");
+        ik.moveTo("left", 0, 0.0+ i * 0.02, 0.0);
+
+
+        // ik.moveTo(pose.name, new Point(0, -200, 50));
+      }
+
+      // map name
+      // and then map all position and rotation too :P
+      Point p = new Point(70 + pose.position.x, -550 + pose.position.y, pose.position.z);
+
+      ik.moveTo(pose.name, p);
+    }
+
+    if (map.size() > 0) {
+      invoke("publishJointAngles", map);
+    }
+
     // TODO - publishQuaternion
     // invoke("publishQuaternion", map);
-    
+
     return pose;
   }
-
 
-  public Map<String, Double> publishJointAngles(Map<String, Double> map){    
-    return map;    
+  public Map<String, Double> publishJointAngles(Map<String, Double> map) {
+    for (String name: map.keySet()) {
+      log.info("{}.moveTo {}", name, map.get(name));
+    }
+    return map;
   }
-  
+
   public static void main(String[] args) {
     try {
 
       LoggingFactory.init(Level.INFO);
+
+      // identical to command line start
+      // Runtime.startConfig("inmoov2");
+      Runtime.main(new String[] { "--log-level", "info", "-s", "webgui", "WebGui", "intro", "Intro", "python", "Python" });
+      boolean done = true;
+      if (done)
+        return;
+
+      Runtime.startConfig("webxr");
+      boolean done2 = true;
+      if (done2)
+        return;
 
-      Runtime.start("webxr", "WebXr");
+      Runtime.start("webxr", "WebXR");
       WebGui webgui = (WebGui) Runtime.create("webgui", "WebGui");
       // webgui.setSsl(true);
       webgui.autoStartBrowser(false);
       webgui.startService();
       Runtime.start("vertx", "Vertx");
-      InMoov2 i01 = (InMoov2)Runtime.start("i01", "InMoov2");
+      InMoov2 i01 = (InMoov2) Runtime.start("i01", "InMoov2");
       i01.startPeer("simulator");
 
-
     } catch (Exception e) {
       log.error("main threw", e);
     }

src/main/java/org/myrobotlab/vertx/ApiVerticle.java

@@ -1,21 +1,13 @@
 package org.myrobotlab.vertx;
 
-import java.lang.reflect.Method;
-
-import org.myrobotlab.codec.CodecUtils;
-import org.myrobotlab.framework.MethodCache;
-import org.myrobotlab.framework.interfaces.ServiceInterface;
-import org.myrobotlab.service.Runtime;
 import org.myrobotlab.service.config.VertxConfig;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;
 
 import io.vertx.core.AbstractVerticle;
-import io.vertx.core.Handler;
 import io.vertx.core.http.HttpMethod;
 import io.vertx.core.http.HttpServer;
 import io.vertx.core.http.HttpServerOptions;
-import io.vertx.core.http.ServerWebSocket;
 import io.vertx.core.net.SelfSignedCertificate;
 import io.vertx.ext.web.Router;
 import io.vertx.ext.web.handler.CorsHandler;

src/main/java/org/myrobotlab/vertx/WebSocketHandler.java

@@ -13,6 +13,7 @@
 import io.vertx.core.http.ServerWebSocket;
 
 /**
+ * Minimal Handler for all websocket messages coming from the react js client.
  * 
  * TODO - what else besides text messages - websocket binary streams ???  text stream ?
  * 
@@ -23,7 +24,14 @@ public class WebSocketHandler implements Handler<ServerWebSocket> {
 
   public final static Logger log = LoggerFactory.getLogger(WebSocketHandler.class);
 
+  /**
+   * reference to the MRL Vertx service / websocket and http server
+   */
   transient private org.myrobotlab.service.Vertx service = null;
+
+  /**
+   * reference to the websocket text message handler
+   */
   TextMessageHandler textMessageHandler = null;
 
   public static class TextMessageHandler implements Handler<String> {
@@ -58,8 +66,10 @@ public void handle(String json) {
           return;
         }
 
+        // FIXME - probably shouldn't be invoking, probable should be putting
+        // the message on the out queue ... not sure
         ServiceInterface si = Runtime.getService(msg.name);
-        Object ret = method.invoke(si, params);
+        // Object ret = method.invoke(si, params);
 
         // put msg on mrl msg bus :)
         // service.in(msg); <- NOT DECODE PARAMS !!
@@ -69,6 +79,11 @@ public void handle(String json) {
         // } else {
         // ctx.writeTextMessage("ping"); Useful is writing back
         // }
+
+        // replace with typed parameters
+        msg.data = params;
+        // queue the message
+        si.in(msg);
 
       } catch (Exception e) {
         service.error(e);