Add orientation, changed goal-model to use protobuf types.

- update relast to (currently) inofficial version able to parse FQN for types in NTA tokens

ros2rag.common/config.yaml

@@ -21,16 +21,18 @@ panda_mqtt_connector:
       Link4: "panda::panda_link4"
       Link5: "panda::panda_link5"
       Link6: "panda::panda_link6"
-      Link7: "panda::panda_link7"
-      # LeftFinger: "panda::panda_leftfinger"
+      EndEffector: "panda::panda_link7"
       RightFinger: "panda::panda_rightfinger"
   end_effectors:
     panda:
-      EndEffector: "panda::panda_leftfinger"
+      LeftFinger: "panda::panda_leftfinger"
   goal_poses:
     - position: "0.6 0 0.6"
-      wait: "5000"
+      orientation: "0 1 1 0"
+      work: "5000"
     - position: "-0.6 0.0 0.6"
-      wait: "3000"
+      orientation: "0 1 1 0"
+      work: "3000"
     - position: "0.6 0.0 0.6"
-      wait: "4000"
+      orientation: "0 1 1 0"
+      work: "4000"

ros2rag.common/src/main/java/de/tudresden/inf/st/ros2rag/common/DataConfiguration.java

@@ -34,6 +34,7 @@ public class DataConfiguration {
   }
   public static class DataWorkPose {
     public String position;
-    public String wait;
+    public String orientation;
+    public String work;
   }
 }

ros2rag.goal/src/main/jastadd/Computation.jrag

@@ -10,7 +10,17 @@ aspect Computation {
     for (WorkPose workPose : getWorkPoseList()) {
       MoveToStep moveToStep = new MoveToStep();
       moveToStep.setId(index++);
-      moveToStep.setPosition(workPose.getPosition());
+      moveToStep.setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+          .setX(workPose.getPositionX())
+          .setY(workPose.getPositionY())
+          .setZ(workPose.getPositionZ())
+          .build());
+      moveToStep.setOrientation(plan.TrajectoryOuterClass.Trajectory.Orientation.newBuilder()
+          .setX(workPose.getOrientationX())
+          .setY(workPose.getOrientationY())
+          .setZ(workPose.getOrientationZ())
+          .setW(workPose.getOrientationW())
+          .build());
       lastStep.setSuccessor(moveToStep);
       result.addStep(moveToStep);
 
@@ -60,18 +70,33 @@ aspect Computation {
     return getId();
   }
 
-  syn DoublePosition Workflow.getNextPosition() {
-    return currentStep().nextPosition();
+  syn plan.TrajectoryOuterClass.Trajectory Workflow.getNextTrajectory() {
+    return currentStep().nextTrajectory();
   }
-  syn DoublePosition Step.nextPosition();
+  syn plan.TrajectoryOuterClass.Trajectory Step.nextTrajectory();
   // next position should not be called before step is advanced
-  eq StartStep.nextPosition() = DoublePosition.of(Double.NaN, Double.NaN, Double.NaN);
-  eq MoveToStep.nextPosition() = getPosition();
-  eq WorkStep.nextPosition() = getPredecessor().nextPosition();
-  eq FinishedStep.nextPosition() = getPredecessor().nextPosition();
+  eq StartStep.nextTrajectory() = plan.TrajectoryOuterClass.Trajectory.newBuilder()
+      .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+          .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+              .setX(Double.NaN)
+              .setY(Double.NaN)
+              .setZ(Double.NaN)
+              .build())
+          .build())
+      .build();
+  eq MoveToStep.nextTrajectory() = plan.TrajectoryOuterClass.Trajectory.newBuilder()
+      .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+          .setPosition(getPosition())
+          .setOrientation(getOrientation())
+          .setMode(plan.TrajectoryOuterClass.Trajectory.PlanningMode.FLUID)
+          .build())
+      .setLoop(true)
+      .build();
+  eq WorkStep.nextTrajectory() = getPredecessor().nextTrajectory();
+  eq FinishedStep.nextTrajectory() = getPredecessor().nextTrajectory();
 
   private static final double MoveToStep.DELTA = 0.1;
-  private boolean MoveToStep.isNear(DoublePosition one, DoublePosition other) {
+  private boolean MoveToStep.isNear(plan.TrajectoryOuterClass.Trajectory.Position one, plan.TrajectoryOuterClass.Trajectory.Position other) {
     return Math.abs(one.getX() - other.getX()) < DELTA &&
       Math.abs(one.getY() - other.getY()) < DELTA &&
       Math.abs(one.getZ() - other.getZ()) < DELTA;

ros2rag.goal/src/main/jastadd/GoalModel.relast

 GoalModel ::= WorkPose* /Workflow/ RobotState ;
 
-RobotState ::= <CurrentPosition:DoublePosition> <LastUpdate:long> ;
+RobotState ::= <CurrentPosition:plan.TrajectoryOuterClass.Trajectory.Position> <LastUpdate:long> ;
 
-WorkPose ::= <Position:DoublePosition> <Duration:long> ;  // Position in [m,m,m], Duration in ms
+WorkPose ::= <PositionX:double> <PositionY:double> <PositionZ:double> <OrientationX:double> <OrientationY:double> <OrientationZ:double> <OrientationW:double> <Duration:long> ;  // Positions in m, Duration in ms
 
-Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextPosition:DoublePosition>/ <CurrentStep:LastUpdatedInteger> ;  // NextPosition in [m,m,m]; CurrentStepStart in ms; ReadyForThisStep and CurrentStep are step ids
+Workflow ::= Step* /<ReadyForThisStep:int>/ /<NextTrajectory:plan.TrajectoryOuterClass.Trajectory>/ <CurrentStep:LastUpdatedInteger> ;  // ReadyForThisStep and CurrentStep are step ids
 
 abstract Step ::= <Id:int> ;  // Started in ms
-MoveToStep : Step ::= <Position:DoublePosition> ;  // Position in [m,m,m]
+MoveToStep : Step ::= <Position:plan.TrajectoryOuterClass.Trajectory.Position> <Orientation:plan.TrajectoryOuterClass.Trajectory.Orientation> ;  // Position in [m,m,m]
 WorkStep : Step ::= <Duration:long> ;  // Duration in ms
 StartStep : Step ;
 FinishedStep : Step ;

ros2rag.goal/src/main/jastadd/GoalModel.ros2rag

@@ -2,20 +2,20 @@
  * Version 2020-05-28
  */
 // --- update definitions ---
-read RobotState.CurrentPosition using ParseRobotState, RobotStateToDoublePosition ;
+read RobotState.CurrentPosition using ParseRobotState, RobotStateToTrajectoryPosition ;
 read RobotState.LastUpdate using TickWhenLinkState ;
 
 // Those two roles together encode an attribute-driven rewrite (via mqtt)
 write Workflow.ReadyForThisStep ;
 read Workflow.CurrentStep using ParseLastUpdatedInteger ;
 
-write Workflow.NextPosition using CreateTrajectoryMessage, SerializeTrajectory ;
+write Workflow.NextTrajectory using SerializeTrajectory ;
 
 // --- dependency definitions ---
 Workflow.ReadyForThisStep canDependOn RobotState.LastUpdate as LastUpdateDependency ;
 Workflow.ReadyForThisStep canDependOn RobotState.CurrentPosition as PositionDependency ;
 
-Workflow.NextPosition canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
+Workflow.NextTrajectory canDependOn Workflow.CurrentStep as OnStepChangeDependency ;
 
 // --- mapping definitions ---
 ParseRobotState maps byte[] bytes to robot.RobotStateOuterClass.RobotState {:
@@ -33,24 +33,29 @@ SerializeTrajectory maps plan.TrajectoryOuterClass.Trajectory t to byte[] {:
   return t.toByteArray();
 :}
 
-RobotStateToDoublePosition maps robot.RobotStateOuterClass.RobotState rs to DoublePosition {:
+RobotStateToTrajectoryPosition maps robot.RobotStateOuterClass.RobotState rs to plan.TrajectoryOuterClass.Trajectory.Position {:
 //  System.out.println("RobotStateToDoublePosition");
   robot.RobotStateOuterClass.RobotState.Position p = rs.getPosition();
-  return DoublePosition.of(p.getX(), p.getY(), p.getZ());
+  return plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+                 .setX(p.getX())
+                 .setY(p.getY())
+                 .setZ(p.getZ())
+                 .build();
 :}
 
-CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Trajectory {:
-  // compute intermediate points at ROS side, not here
-  return plan.TrajectoryOuterClass.Trajectory.newBuilder()
-      .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
-          .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
-              .setX(dp.getX())
-              .setY(dp.getY())
-              .setZ(dp.getZ())
-              .build())
-          .build())
-      .build();
-:}
+// CreateTrajectoryMessage maps DoublePosition dp to plan.TrajectoryOuterClass.Trajectory {:
+//   // compute intermediate points at ROS side, not here
+//   return plan.TrajectoryOuterClass.Trajectory.newBuilder()
+//       .addPose(plan.TrajectoryOuterClass.Trajectory.Pose.newBuilder()
+//           .setPosition(plan.TrajectoryOuterClass.Trajectory.Position.newBuilder()
+//               .setX(dp.getX())
+//               .setY(dp.getY())
+//               .setZ(dp.getZ())
+//               .build())
+//           .build())
+//       .setLooping(true);
+//       .build();
+// :}
 
 ParseLastUpdatedInteger maps int value to LastUpdatedInteger {:
   return LastUpdatedInteger.of(value);

ros2rag.goal/src/main/jastadd/Printing.jrag

@@ -2,7 +2,9 @@ aspect Printing {
   syn String Step.prettyPrint() = getId() + ": " + prettyPrint0() + " ->[" + (getSuccessor() == null ? "/" : getSuccessor().getId()) + "]";
   syn String Step.prettyPrint0();
   eq StartStep.prettyPrint0() = "Start";
-  eq MoveToStep.prettyPrint0() = "Move to " + getPosition();
+  eq MoveToStep.prettyPrint0() = String.format("Move to pos(% .5f,% .5f,% .5f) ori(%.1f, %.1f, %.1f, %.1f)",
+    getPosition().getX(),getPosition().getY(),getPosition().getZ(),
+    getOrientation().getX(),getOrientation().getY(),getOrientation().getZ(),getOrientation().getW());
   eq WorkStep.prettyPrint0() = "Work " + getDuration();
   eq FinishedStep.prettyPrint0() = "Finish";
 }

ros2rag.goal/src/main/java/de/tudresden/inf/st/ros2rag/goal/GoalMain.java

@@ -6,9 +6,11 @@ import de.tudresden.inf.st.ros2rag.common.Util;
 import de.tudresden.inf.st.ros2rag.goal.ast.*;
 import org.apache.logging.log4j.LogManager;
 import org.apache.logging.log4j.Logger;
+import plan.TrajectoryOuterClass;
 
 import java.io.File;
 import java.io.IOException;
+import java.util.Arrays;
 import java.util.concurrent.CountDownLatch;
 import java.util.concurrent.TimeUnit;
 
@@ -35,17 +37,26 @@ public class GoalMain {
 
     for (DataWorkPose dataWorkPose : config.goal_poses) {
       WorkPose workPose = new WorkPose();
-      double[] position = {0, 0, 0};
-      String[] positionSplit = dataWorkPose.position.split(" ");
-      for (int i = 0; i < positionSplit.length; i++) {
-        position[i] = Double.parseDouble(positionSplit[i]);
+      String[] position = dataWorkPose.position.split(" ");
+      String[] orientation = dataWorkPose.orientation.split(" ");
+      if (position.length != 3) {
+        logger.error("Found invalid work pose position: {}", Arrays.toString(position));
       }
-      workPose.setPosition(DoublePosition.of(position[0], position[1], position[2]));
-      workPose.setDuration(Long.parseLong(dataWorkPose.wait));
+      if (orientation.length != 4) {
+        logger.error("Found invalid work pose orientation: {}", Arrays.toString(orientation));
+      }
+      workPose.setPositionX(Double.parseDouble(position[0]));
+      workPose.setPositionY(Double.parseDouble(position[1]));
+      workPose.setPositionZ(Double.parseDouble(position[2]));
+      workPose.setOrientationX(Double.parseDouble(orientation[0]));
+      workPose.setOrientationY(Double.parseDouble(orientation[1]));
+      workPose.setOrientationZ(Double.parseDouble(orientation[2]));
+      workPose.setOrientationW(Double.parseDouble(orientation[3]));
+      workPose.setDuration(Long.parseLong(dataWorkPose.work));
       model.addWorkPose(workPose);
     }
     RobotState robotState = new RobotState();
-    robotState.setCurrentPosition(DoublePosition.of(0, 0, 0));
+    robotState.setCurrentPosition(TrajectoryOuterClass.Trajectory.Position.newBuilder().build());
     robotState.setLastUpdate(0);
     model.setRobotState(robotState);
 
@@ -76,7 +87,7 @@ public class GoalMain {
       }
     }, config);
     // next position is not initialized, so don't send it
-    model.getWorkflow().connectNextPosition(config.topics.trajectory, false);
+    model.getWorkflow().connectNextTrajectory(config.topics.trajectory, false);
     model.getWorkflow().connectCurrentStep(config.topics.nextStep);
     // initial next step is sent, as soon as this is received, the workflow starts
     model.getWorkflow().connectReadyForThisStep(config.topics.nextStep, true);