Merge branch 'main' of github.com:cmusatyalab/steeleagle into main

droneDSL/compile/src/main/grammar/BotParser.bnf

@@ -19,7 +19,7 @@ file ::= task mission
 
 // defining the task
 task ::= TASK_KW <<braced task_decl*>>
-task_decl ::= (TASK_DETECT_KW | TASK_TRACK_KW) task_name task_body
+task_decl ::= (TASK_DETECT_KW | TASK_TRACK_KW | TASK_AVOID_KW) task_name task_body
 task_body ::= <<braced <<commaSep attributes>>>>
 private attributes ::= attribute*
 attribute ::=  ID <<coloned attribute_expr>>

droneDSL/compile/src/main/grammar/BotPsiLexer.flex

@@ -34,6 +34,7 @@ IDENTIFIER=[α-ωa-zA-Z_][α-ωa-zA-Z0-9_'-]*
   Task                { return TASK_KW; }
   Detect              { return TASK_DETECT_KW; }
   Track               { return TASK_TRACK_KW; }
+  Avoid               { return TASK_AVOID_KW; }
   Mission             { return MISSION_KW; }
   Transition          { return TRANSITION_KW; }
   Start               { return MISSION_START_KW; }

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java

@@ -0,0 +1,37 @@
+package org.droneDSL.compile.codeGen.concrete;
+
+import kala.collection.immutable.ImmutableSeq;
+
+public class AvoidTask extends Task {
+  public float speed;
+  public String model;
+
+  public AvoidTask(String taskID, ImmutableSeq<Point> wayPoints, float speed, String model) {
+    super(taskID);
+    this.wayPoints = wayPoints;
+    this.speed = speed;
+    this.model = model;
+  }
+
+  @Override
+  public void debugPrint() {
+    System.out.println("speed :" + speed);
+    System.out.println("model :" + model);
+  }
+
+  @Override
+  public String generateDefineTaskCode() {
+    var waypointsStr = wayPoints.joinToString(",", "[", "]", Point::toJson);
+    return """
+                # TASK%s
+                task_attr_%s = {}
+                task_attr_%s["speed"] = "%s"
+                task_attr_%s["model"] = "%s"
+                task_attr_%s["coords"] = "%s"
+        """.formatted(taskID, taskID, taskID, speed, taskID, model, taskID, waypointsStr)
+           + this.generateTaskTransCode() +
+           """
+                 task_arg_map["%s"] = TaskArguments(TaskType.Avoid, transition_attr_%s, task_attr_%s)
+         """.formatted(taskID, taskID, taskID);
+  }
+}

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/DetectTask.java

@@ -12,18 +12,6 @@ public class DetectTask extends Task {
   public HSV lowerBound;
   public HSV upperBound;
 
-  public ImmutableSeq<Point> wayPoints;
-
-  public record Point(double x, double y, double z) {
-    public String toJson() {
-      return String.format("{'lng': %s, 'lat': %s, 'alt': %s}", x, y, z);
-    }
-  }
-  public record HSV(int h, int s, int v) {
-    public String toString() {
-      return String.format("[%s, %s, %s]", h, s, v);
-    }
-  }
 
   public DetectTask(String taskID, ImmutableSeq<Point> wayPoints, float gimbalPitch, float droneRotation, int sampleRate, int hoverDelay, String model, HSV lowerBound, HSV upperBound) {
     super(taskID);
@@ -37,6 +25,7 @@ public DetectTask(String taskID, ImmutableSeq<Point> wayPoints, float gimbalPitc
     this.upperBound = upperBound;
   }
 
+  @Override
   public void debugPrint() {
     System.out.println("gimbal_pitch :" + gimbalPitch);
     System.out.println("drone_rotation :" + droneRotation);

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/MissionPlan.java

@@ -44,12 +44,12 @@ def start_transit(triggered_event):
             @staticmethod
             def define_mission(transitMap, task_arg_map):
                 #define transition
-                logger.info("MissionController: define the transitMap\\n")
+                logger.info("MissionCreator: define the transitMap\\n")
                 transitMap["start"] = MissionCreator.start_transit        
         """);
     missionTask.append(String.format("""
                 # define task
-                logger.info("MissionController: define the tasks\\n")
+                logger.info("MissionCreator: define the tasks\\n")
         """, this.startTaskID));
 
 
@@ -89,11 +89,15 @@ def define_mission(transitMap, task_arg_map):
       }
     });
 
+    missionTask.append(String.format("""
+                logger.info("MissionCreator: finish defining the tasks\\n")
+        """));
+
 
     staticMethod.append("""
             @staticmethod
             def default_transit(triggered_event):
-                logger.info(f"MissionController: no matched up transition, triggered event {triggered_event}\\n", triggered_event)
+                logger.info(f"MissionCreator: no matched up transition, triggered event {triggered_event}\\n", triggered_event)
         """);
 
     missionTrans.append("""

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Parse.java

@@ -18,7 +18,8 @@
 public interface Parse {
   enum TaskKind {
     Detect,
-    Track
+    Track,
+    Avoid
   }
 
   class AttributeMap {
@@ -103,6 +104,40 @@ static Tuple2<String, Task> createTask(GenericNode<? extends GenericNode<?>> tas
         );
         yield Tuple.of(taskID, trackTask);
       }
+      case Avoid -> {
+
+        var speed = attrMap.get("speed").child(BotPsiElementTypes.NUMBER).tokenText();
+        var model = attrMap.get("model").child(BotPsiElementTypes.NAME).tokenText();
+
+
+        // waypoints
+        var isWayPointsVar = attrMap.get("way_points").peekChild(BotPsiElementTypes.ANGLE_BRACKED);
+        ImmutableSeq<DetectTask.Point> wayPoints;
+        if (isWayPointsVar == null) {
+          wayPoints = attrMap.get("way_points").child(BotPsiElementTypes.SQUARE_BRACKED).childrenOfType(BotPsiElementTypes.PAREN).
+              map(point -> {
+                var nums = point.child(BotPsiElementTypes.TUPLE).childrenOfType(BotPsiElementTypes.NUMBER)
+                    .map(t -> t.tokenText().toFloat())
+                    .toImmutableSeq();
+                return new DetectTask.Point(nums.get(0), nums.get(1), nums.get(2));
+              })
+              .toImmutableSeq();
+        } else {
+          var wayPointsID = attrMap.get("way_points").child(BotPsiElementTypes.ANGLE_BRACKED).child(BotPsiElementTypes.NAME).tokenText().toString();
+          wayPoints = Seq.wrapJava(waypointsMap.get(wayPointsID))
+              .map(pt -> new DetectTask.Point(Double.parseDouble(pt.longitude()), Double.parseDouble(pt.latitude()), Double.parseDouble(pt.altitude())));
+        }
+
+
+        // construct new task
+        var avoidTask = new AvoidTask(
+            taskID,
+            wayPoints,
+            speed.toFloat(),
+            model.toString()
+        );
+        yield Tuple.of(taskID, avoidTask);
+      }
 
     };
   }
@@ -149,13 +184,18 @@ static String createMission(GenericNode<? extends GenericNode<?>> missionContent
   @NotNull
   private static AttributeMap createMap(GenericNode<? extends GenericNode<?>> task) {
     var isDetect = task.peekChild(BotPsiElementTypes.TASK_DETECT_KW);
+    var isTrack = task.peekChild(BotPsiElementTypes.TASK_TRACK_KW);
     var attrMap = new AttributeMap();
     task.child(BotPsiElementTypes.TASK_BODY).childrenOfType(BotPsiElementTypes.ATTRIBUTE)
         .forEach(attr -> attrMap.content.put(attr.child(BotPsiElementTypes.ID).tokenText(), attr.child(BotPsiElementTypes.ATTRIBUTE_EXPR)));
+
+
     if (isDetect != null) {
       attrMap.kind = TaskKind.Detect;
-    } else {
+    } else if (isTrack!= null){
       attrMap.kind = TaskKind.Track;
+    } else{
+      attrMap.kind = TaskKind.Avoid;
     }
     return attrMap;
   }

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Task.java

@@ -1,5 +1,6 @@
 package org.droneDSL.compile.codeGen.concrete;
 
+import kala.collection.immutable.ImmutableSeq;
 import org.jetbrains.annotations.Nullable;
 
 import java.util.ArrayList;
@@ -15,9 +16,20 @@ public Task(String taskID) {
     this.transitions = new ArrayList<>();
   }
 
-  public List<Transition> transitions;
 
+  public ImmutableSeq<Point> wayPoints;
+  public record Point(double x, double y, double z) {
+    public String toJson() {
+      return String.format("{'lng': %s, 'lat': %s, 'alt': %s}", x, y, z);
+    }
+  }
+  public record HSV(int h, int s, int v) {
+    public String toString() {
+      return String.format("[%s, %s, %s]", h, s, v);
+    }
+  }
 
+  public List<Transition> transitions;
   public record Transition<T>(
       String condID,
       @Nullable T condArg,
@@ -26,6 +38,8 @@ public record Transition<T>(
   ) {
   }
 
+
+
   public abstract void debugPrint();
 
   public abstract String generateDefineTaskCode();

droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/TrackTask.java

@@ -9,11 +9,6 @@ public class TrackTask extends Task {
   public HSV lowerBound;
   public HSV upperBound;
 
-  public record HSV(int h, int s, int v) {
-    public String toString() {
-      return String.format("[%s, %s, %s]", h, s, v);
-    }
-  }
 
   public TrackTask(String taskID, float gimbalPitch, String target_class, String model, HSV lower_bound, HSV upper_bound) {
     super(taskID);
@@ -24,6 +19,7 @@ public TrackTask(String taskID, float gimbalPitch, String target_class, String m
     this.upperBound = upper_bound;
   }
 
+  @Override
   public void debugPrint() {
     System.out.println("gimbal_pitch :" + gimbalPitch);
     System.out.println("model :" + model);

droneDSL/python/interfaces/DroneItf.py

@@ -106,6 +106,14 @@ async def getLng(self):
     async def getHeading(self):
         pass
 
+    @abstractmethod
+    async def getSpeedNED(self):
+        pass
+
+    @abstractmethod
+    async def getSpeedRel(self):
+        pass
+
     @abstractmethod
     async def getRelAlt(self):
         pass

droneDSL/python/interfaces/Task.py

@@ -13,13 +13,14 @@
 class TaskType(Enum):
     Detect = 1
     Track = 2
+    Avoid = 3
 
 class TaskArguments():
     def __init__(self, task_type, transitions_attributes, task_attributes):
         self.task_type = task_type
         self.task_attributes = task_attributes
         self.transitions_attributes = transitions_attributes
-
+        
 class Task(ABC):
 
     def __init__(self, drone, cloudlet, task_id, trigger_event_queue, task_args):

droneDSL/python/mission/MissionController.py

@@ -2,6 +2,7 @@
 from  interfaces.Task import TaskType      
 from task_defs.TrackTask import TrackTask
 from task_defs.DetectTask import DetectTask
+from task_defs.AvoidTask import AvoidTask
 from mission.MissionCreator import MissionCreator
 from mission.TaskRunner import TaskRunner
 import queue
@@ -34,6 +35,9 @@ def create_task(self, task_id):
             elif (self.task_arg_map[task_id].task_type == TaskType.Track):
                 logger.info('MC: Track task')
                 return TrackTask(self.drone, self.cloudlet, task_id, self.trigger_event_queue, self.task_arg_map[task_id])
+            elif (self.task_arg_map[task_id].task_type == TaskType.Avoid):
+                logger.info('MC: Avoid task')
+                return AvoidTask(self.drone, self.cloudlet, task_id, self.trigger_event_queue, self.task_arg_map[task_id])
         return None
 
     def next_task(self, current_task_id, triggered_event):