From ceec887384ff1a1335701252c26bb5192ee0a254 Mon Sep 17 00:00:00 2001 From: xianglic Date: Wed, 8 May 2024 10:18:41 -0400 Subject: [PATCH 1/2] add avoid task in language --- .../compile/src/main/grammar/BotParser.bnf | 2 +- .../compile/src/main/grammar/BotPsiLexer.flex | 1 + .../compile/codeGen/concrete/AvoidTask.java | 37 ++++++++++++++++ .../compile/codeGen/concrete/DetectTask.java | 13 +----- .../compile/codeGen/concrete/Parse.java | 44 ++++++++++++++++++- .../compile/codeGen/concrete/Task.java | 16 ++++++- .../compile/codeGen/concrete/TrackTask.java | 6 +-- .../compile/parser/BotPsiElementTypes.java | 1 + .../droneDSL/compile/parser/BotPsiParser.java | 6 +-- droneDSL/python/interfaces/Task.py | 1 + droneDSL/python/mission/MissionController.py | 4 ++ 11 files changed, 107 insertions(+), 24 deletions(-) create mode 100644 droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java diff --git a/droneDSL/compile/src/main/grammar/BotParser.bnf b/droneDSL/compile/src/main/grammar/BotParser.bnf index fde0ebe9..f30b41dd 100644 --- a/droneDSL/compile/src/main/grammar/BotParser.bnf +++ b/droneDSL/compile/src/main/grammar/BotParser.bnf @@ -19,7 +19,7 @@ file ::= task mission // defining the task task ::= TASK_KW <> -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 ::= <>>> private attributes ::= attribute* attribute ::= ID <> diff --git a/droneDSL/compile/src/main/grammar/BotPsiLexer.flex b/droneDSL/compile/src/main/grammar/BotPsiLexer.flex index 4383cf56..295b928c 100644 --- a/droneDSL/compile/src/main/grammar/BotPsiLexer.flex +++ b/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; } diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java new file mode 100644 index 00000000..1379105c --- /dev/null +++ b/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 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["waypoints"] = "%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); + } +} diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/DetectTask.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/DetectTask.java index 26121036..1f104a47 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/DetectTask.java +++ b/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 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 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 wayPoints, float gimbalPitc this.upperBound = upperBound; } + @Override public void debugPrint() { System.out.println("gimbal_pitch :" + gimbalPitch); System.out.println("drone_rotation :" + droneRotation); diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Parse.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Parse.java index 111ccbb1..00263c76 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Parse.java +++ b/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 createTask(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 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> missionContent @NotNull private static AttributeMap createMap(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; } diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Task.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Task.java index c7ce54b1..bfa40a76 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/Task.java +++ b/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 transitions; + public ImmutableSeq 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 transitions; public record Transition( String condID, @Nullable T condArg, @@ -26,6 +38,8 @@ public record Transition( ) { } + + public abstract void debugPrint(); public abstract String generateDefineTaskCode(); diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/TrackTask.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/TrackTask.java index f965b4ff..68c165e0 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/TrackTask.java +++ b/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); diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiElementTypes.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiElementTypes.java index c0a1d897..06ea0a97 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiElementTypes.java +++ b/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiElementTypes.java @@ -37,6 +37,7 @@ public interface BotPsiElementTypes { IElementType RBRACE = new BotPsiTokenType("RBRACE"); IElementType RPAREN = new BotPsiTokenType("RPAREN"); IElementType RSQUA = new BotPsiTokenType("RSQUA"); + IElementType TASK_AVOID_KW = new BotPsiTokenType("TASK_AVOID_KW"); IElementType TASK_DETECT_KW = new BotPsiTokenType("TASK_DETECT_KW"); IElementType TASK_KW = new BotPsiTokenType("TASK_KW"); IElementType TASK_TRACK_KW = new BotPsiTokenType("TASK_TRACK_KW"); diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiParser.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiParser.java index 1c696b7d..74e808a5 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiParser.java +++ b/droneDSL/compile/src/main/java/org/droneDSL/compile/parser/BotPsiParser.java @@ -343,10 +343,9 @@ public static boolean task_body(PsiBuilder b, int l) { } /* ********************************************************** */ - // (TASK_DETECT_KW | TASK_TRACK_KW) task_name task_body + // (TASK_DETECT_KW | TASK_TRACK_KW | TASK_AVOID_KW) task_name task_body public static boolean task_decl(PsiBuilder b, int l) { if (!recursion_guard_(b, l, "task_decl")) return false; - if (!nextTokenIs(b, "", TASK_DETECT_KW, TASK_TRACK_KW)) return false; boolean r; Marker m = enter_section_(b, l, _NONE_, TASK_DECL, ""); r = task_decl_0(b, l + 1); @@ -356,12 +355,13 @@ public static boolean task_decl(PsiBuilder b, int l) { return r; } - // TASK_DETECT_KW | TASK_TRACK_KW + // TASK_DETECT_KW | TASK_TRACK_KW | TASK_AVOID_KW private static boolean task_decl_0(PsiBuilder b, int l) { if (!recursion_guard_(b, l, "task_decl_0")) return false; boolean r; r = consumeToken(b, TASK_DETECT_KW); if (!r) r = consumeToken(b, TASK_TRACK_KW); + if (!r) r = consumeToken(b, TASK_AVOID_KW); return r; } diff --git a/droneDSL/python/interfaces/Task.py b/droneDSL/python/interfaces/Task.py index 44560c25..d9eb058c 100644 --- a/droneDSL/python/interfaces/Task.py +++ b/droneDSL/python/interfaces/Task.py @@ -13,6 +13,7 @@ class TaskType(Enum): Detect = 1 Track = 2 + Avoid = 3 class TaskArguments(): def __init__(self, task_type, transitions_attributes, task_attributes): diff --git a/droneDSL/python/mission/MissionController.py b/droneDSL/python/mission/MissionController.py index aaecb1b4..940b8050 100644 --- a/droneDSL/python/mission/MissionController.py +++ b/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): From f918f0ae631a4359c3935d0328b75f94dee04f40 Mon Sep 17 00:00:00 2001 From: xianglic Date: Tue, 14 May 2024 11:57:20 -0400 Subject: [PATCH 2/2] integration of avoid task --- cnc/protocol/cnc_pb2.py | 489 ++++++++++++++++++ .../compile/codeGen/concrete/AvoidTask.java | 2 +- .../compile/codeGen/concrete/MissionPlan.java | 10 +- droneDSL/python/interfaces/Task.py | 2 +- droneDSL/python/task_defs/AvoidTask.py | 112 ++++ onboard/python/interfaces | 2 +- 6 files changed, 611 insertions(+), 6 deletions(-) create mode 100644 cnc/protocol/cnc_pb2.py create mode 100644 droneDSL/python/task_defs/AvoidTask.py diff --git a/cnc/protocol/cnc_pb2.py b/cnc/protocol/cnc_pb2.py new file mode 100644 index 00000000..041982a4 --- /dev/null +++ b/cnc/protocol/cnc_pb2.py @@ -0,0 +1,489 @@ +# -*- coding: utf-8 -*- +# Generated by the protocol buffer compiler. DO NOT EDIT! +# source: cnc.proto + +from google.protobuf import descriptor as _descriptor +from google.protobuf import message as _message +from google.protobuf import reflection as _reflection +from google.protobuf import symbol_database as _symbol_database +# @@protoc_insertion_point(imports) + +_sym_db = _symbol_database.Default() + + + + +DESCRIPTOR = _descriptor.FileDescriptor( + name='cnc.proto', + package='cnc', + syntax='proto3', + serialized_options=b'\n\025edu.cmu.cs.steeleagleB\006Protos', + create_key=_descriptor._internal_create_key, + serialized_pb=b'\n\tcnc.proto\x12\x03\x63nc\"O\n\x08Location\x12\x0c\n\x04name\x18\x01 \x01(\t\x12\x10\n\x08latitude\x18\x02 \x01(\x01\x12\x11\n\tlongitude\x18\x03 \x01(\x01\x12\x10\n\x08\x61ltitude\x18\x04 \x01(\x01\"S\n\x04PCMD\x12\x0b\n\x03gaz\x18\x01 \x01(\x05\x12\x0b\n\x03yaw\x18\x02 \x01(\x05\x12\r\n\x05pitch\x18\x03 \x01(\x05\x12\x0c\n\x04roll\x18\x04 \x01(\x05\x12\x14\n\x0cgimbal_pitch\x18\x05 \x01(\x05\"\x96\x01\n\x07\x43ommand\x12\x14\n\x0c\x66or_drone_id\x18\x01 \x01(\t\x12\x12\n\nscript_url\x18\x02 \x01(\t\x12\x0c\n\x04halt\x18\x03 \x01(\x08\x12\x17\n\x04pcmd\x18\x04 \x01(\x0b\x32\t.cnc.PCMD\x12\x0f\n\x07takeoff\x18\x05 \x01(\x08\x12\x0c\n\x04land\x18\x06 \x01(\x08\x12\x0e\n\x06manual\x18\x07 \x01(\x08\x12\x0b\n\x03rth\x18\x08 \x01(\x08\"`\n\x0b\x44roneStatus\x12\x0f\n\x07\x62\x61ttery\x18\x01 \x01(\x03\x12\x14\n\x0cgimbal_pitch\x18\x02 \x01(\x01\x12\x0f\n\x07\x62\x65\x61ring\x18\x03 \x01(\x01\x12\x0c\n\x04rssi\x18\x04 \x01(\x03\x12\x0b\n\x03mag\x18\x05 \x01(\x03\"&\n\x03HSV\x12\t\n\x01H\x18\x01 \x01(\x03\x12\t\n\x01S\x18\x02 \x01(\x03\x12\t\n\x01V\x18\x03 \x01(\x03\"\xa4\x02\n\x06\x45xtras\x12\x13\n\x0bregistering\x18\x01 \x01(\x08\x12\x10\n\x08\x64rone_id\x18\x02 \x01(\t\x12\x14\n\x0c\x63ommander_id\x18\x03 \x01(\t\x12\x19\n\x03\x63md\x18\x04 \x01(\x0b\x32\x0c.cnc.Command\x12\x1f\n\x08location\x18\x05 \x01(\x0b\x32\r.cnc.Location\x12 \n\x06status\x18\x06 \x01(\x0b\x32\x10.cnc.DroneStatus\x12\x17\n\x0f\x64\x65tection_model\x18\x07 \x01(\t\x12\"\n\x0blower_bound\x18\x08 \x01(\x0b\x32\x08.cnc.HSVH\x00\x88\x01\x01\x12\"\n\x0bupper_bound\x18\t \x01(\x0b\x32\x08.cnc.HSVH\x01\x88\x01\x01\x42\x0e\n\x0c_lower_boundB\x0e\n\x0c_upper_boundB\x1f\n\x15\x65\x64u.cmu.cs.steeleagleB\x06Protosb\x06proto3' +) + + + + +_LOCATION = _descriptor.Descriptor( + name='Location', + full_name='cnc.Location', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='name', full_name='cnc.Location.name', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='latitude', full_name='cnc.Location.latitude', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='longitude', full_name='cnc.Location.longitude', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='altitude', full_name='cnc.Location.altitude', index=3, + number=4, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=18, + serialized_end=97, +) + + +_PCMD = _descriptor.Descriptor( + name='PCMD', + full_name='cnc.PCMD', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='gaz', full_name='cnc.PCMD.gaz', index=0, + number=1, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='yaw', full_name='cnc.PCMD.yaw', index=1, + number=2, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pitch', full_name='cnc.PCMD.pitch', index=2, + number=3, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='roll', full_name='cnc.PCMD.roll', index=3, + number=4, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='gimbal_pitch', full_name='cnc.PCMD.gimbal_pitch', index=4, + number=5, type=5, cpp_type=1, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=99, + serialized_end=182, +) + + +_COMMAND = _descriptor.Descriptor( + name='Command', + full_name='cnc.Command', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='for_drone_id', full_name='cnc.Command.for_drone_id', index=0, + number=1, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='script_url', full_name='cnc.Command.script_url', index=1, + number=2, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='halt', full_name='cnc.Command.halt', index=2, + number=3, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='pcmd', full_name='cnc.Command.pcmd', index=3, + number=4, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='takeoff', full_name='cnc.Command.takeoff', index=4, + number=5, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='land', full_name='cnc.Command.land', index=5, + number=6, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='manual', full_name='cnc.Command.manual', index=6, + number=7, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='rth', full_name='cnc.Command.rth', index=7, + number=8, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=185, + serialized_end=335, +) + + +_DRONESTATUS = _descriptor.Descriptor( + name='DroneStatus', + full_name='cnc.DroneStatus', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='battery', full_name='cnc.DroneStatus.battery', index=0, + number=1, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='gimbal_pitch', full_name='cnc.DroneStatus.gimbal_pitch', index=1, + number=2, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='bearing', full_name='cnc.DroneStatus.bearing', index=2, + number=3, type=1, cpp_type=5, label=1, + has_default_value=False, default_value=float(0), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='rssi', full_name='cnc.DroneStatus.rssi', index=3, + number=4, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='mag', full_name='cnc.DroneStatus.mag', index=4, + number=5, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=337, + serialized_end=433, +) + + +_HSV = _descriptor.Descriptor( + name='HSV', + full_name='cnc.HSV', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='H', full_name='cnc.HSV.H', index=0, + number=1, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='S', full_name='cnc.HSV.S', index=1, + number=2, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='V', full_name='cnc.HSV.V', index=2, + number=3, type=3, cpp_type=2, label=1, + has_default_value=False, default_value=0, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + ], + serialized_start=435, + serialized_end=473, +) + + +_EXTRAS = _descriptor.Descriptor( + name='Extras', + full_name='cnc.Extras', + filename=None, + file=DESCRIPTOR, + containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[ + _descriptor.FieldDescriptor( + name='registering', full_name='cnc.Extras.registering', index=0, + number=1, type=8, cpp_type=7, label=1, + has_default_value=False, default_value=False, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='drone_id', full_name='cnc.Extras.drone_id', index=1, + number=2, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='commander_id', full_name='cnc.Extras.commander_id', index=2, + number=3, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='cmd', full_name='cnc.Extras.cmd', index=3, + number=4, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='location', full_name='cnc.Extras.location', index=4, + number=5, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='status', full_name='cnc.Extras.status', index=5, + number=6, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='detection_model', full_name='cnc.Extras.detection_model', index=6, + number=7, type=9, cpp_type=9, label=1, + has_default_value=False, default_value=b"".decode('utf-8'), + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='lower_bound', full_name='cnc.Extras.lower_bound', index=7, + number=8, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + _descriptor.FieldDescriptor( + name='upper_bound', full_name='cnc.Extras.upper_bound', index=8, + number=9, type=11, cpp_type=10, label=1, + has_default_value=False, default_value=None, + message_type=None, enum_type=None, containing_type=None, + is_extension=False, extension_scope=None, + serialized_options=None, file=DESCRIPTOR, create_key=_descriptor._internal_create_key), + ], + extensions=[ + ], + nested_types=[], + enum_types=[ + ], + serialized_options=None, + is_extendable=False, + syntax='proto3', + extension_ranges=[], + oneofs=[ + _descriptor.OneofDescriptor( + name='_lower_bound', full_name='cnc.Extras._lower_bound', + index=0, containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[]), + _descriptor.OneofDescriptor( + name='_upper_bound', full_name='cnc.Extras._upper_bound', + index=1, containing_type=None, + create_key=_descriptor._internal_create_key, + fields=[]), + ], + serialized_start=476, + serialized_end=768, +) + +_COMMAND.fields_by_name['pcmd'].message_type = _PCMD +_EXTRAS.fields_by_name['cmd'].message_type = _COMMAND +_EXTRAS.fields_by_name['location'].message_type = _LOCATION +_EXTRAS.fields_by_name['status'].message_type = _DRONESTATUS +_EXTRAS.fields_by_name['lower_bound'].message_type = _HSV +_EXTRAS.fields_by_name['upper_bound'].message_type = _HSV +_EXTRAS.oneofs_by_name['_lower_bound'].fields.append( + _EXTRAS.fields_by_name['lower_bound']) +_EXTRAS.fields_by_name['lower_bound'].containing_oneof = _EXTRAS.oneofs_by_name['_lower_bound'] +_EXTRAS.oneofs_by_name['_upper_bound'].fields.append( + _EXTRAS.fields_by_name['upper_bound']) +_EXTRAS.fields_by_name['upper_bound'].containing_oneof = _EXTRAS.oneofs_by_name['_upper_bound'] +DESCRIPTOR.message_types_by_name['Location'] = _LOCATION +DESCRIPTOR.message_types_by_name['PCMD'] = _PCMD +DESCRIPTOR.message_types_by_name['Command'] = _COMMAND +DESCRIPTOR.message_types_by_name['DroneStatus'] = _DRONESTATUS +DESCRIPTOR.message_types_by_name['HSV'] = _HSV +DESCRIPTOR.message_types_by_name['Extras'] = _EXTRAS +_sym_db.RegisterFileDescriptor(DESCRIPTOR) + +Location = _reflection.GeneratedProtocolMessageType('Location', (_message.Message,), { + 'DESCRIPTOR' : _LOCATION, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.Location) + }) +_sym_db.RegisterMessage(Location) + +PCMD = _reflection.GeneratedProtocolMessageType('PCMD', (_message.Message,), { + 'DESCRIPTOR' : _PCMD, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.PCMD) + }) +_sym_db.RegisterMessage(PCMD) + +Command = _reflection.GeneratedProtocolMessageType('Command', (_message.Message,), { + 'DESCRIPTOR' : _COMMAND, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.Command) + }) +_sym_db.RegisterMessage(Command) + +DroneStatus = _reflection.GeneratedProtocolMessageType('DroneStatus', (_message.Message,), { + 'DESCRIPTOR' : _DRONESTATUS, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.DroneStatus) + }) +_sym_db.RegisterMessage(DroneStatus) + +HSV = _reflection.GeneratedProtocolMessageType('HSV', (_message.Message,), { + 'DESCRIPTOR' : _HSV, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.HSV) + }) +_sym_db.RegisterMessage(HSV) + +Extras = _reflection.GeneratedProtocolMessageType('Extras', (_message.Message,), { + 'DESCRIPTOR' : _EXTRAS, + '__module__' : 'cnc_pb2' + # @@protoc_insertion_point(class_scope:cnc.Extras) + }) +_sym_db.RegisterMessage(Extras) + + +DESCRIPTOR._options = None +# @@protoc_insertion_point(module_scope) diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java index 1379105c..9de753e5 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java +++ b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/AvoidTask.java @@ -27,7 +27,7 @@ public String generateDefineTaskCode() { task_attr_%s = {} task_attr_%s["speed"] = "%s" task_attr_%s["model"] = "%s" - task_attr_%s["waypoints"] = "%s" + task_attr_%s["coords"] = "%s" """.formatted(taskID, taskID, taskID, speed, taskID, model, taskID, waypointsStr) + this.generateTaskTransCode() + """ diff --git a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/MissionPlan.java b/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/MissionPlan.java index 3f9db867..5b40f93c 100644 --- a/droneDSL/compile/src/main/java/org/droneDSL/compile/codeGen/concrete/MissionPlan.java +++ b/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(""" diff --git a/droneDSL/python/interfaces/Task.py b/droneDSL/python/interfaces/Task.py index d9eb058c..ba53445b 100644 --- a/droneDSL/python/interfaces/Task.py +++ b/droneDSL/python/interfaces/Task.py @@ -20,7 +20,7 @@ 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): diff --git a/droneDSL/python/task_defs/AvoidTask.py b/droneDSL/python/task_defs/AvoidTask.py new file mode 100644 index 00000000..6433b340 --- /dev/null +++ b/droneDSL/python/task_defs/AvoidTask.py @@ -0,0 +1,112 @@ +# SPDX-FileCopyrightText: 2023 Carnegie Mellon University - Satyalab +# +# SPDX-License-Identifier: GPL-2.0-only + +#from interfaces.Task import Task +import ast +import json +from json import JSONDecodeError +from gabriel_protocol import gabriel_pb2 +import time +import asyncio +import logging +from transition_defs.TimerTransition import TimerTransition +from interfaces.Task import Task + +logger = logging.getLogger(__name__) +logger.setLevel(logging.INFO) + +#class ObstacleTask(Task): +class AvoidTask(Task): + + def __init__(self, drone, cloudlet, task_id, trigger_event_queue, task_args): + super().__init__(drone, cloudlet, task_id, trigger_event_queue, task_args) + # PID controller parameters + self.time_prev = None + self.error_prev = 0 + self.pid_info = {"constants" : {"Kp": 0.1, "Ki": 0.001, "Kd": 0.2}, "saved" : {"I": 0.0}} + self.speed = self.task_attributes["speed"] + + + def create_transition(self): + logger.info(self.transitions_attributes) + args = { + 'task_id': self.task_id, + 'trans_active': self.trans_active, + 'trans_active_lock': self.trans_active_lock, + 'trigger_event_queue': self.trigger_event_queue + } + + # Triggered event + if ("timeout" in self.transitions_attributes): + timer = TimerTransition(args, self.transitions_attributes["timeout"]) + timer.daemon = True + timer.start() + + + def clamp(self, value, minimum, maximum): + return max(minimum, min(value, maximum)) + + async def moveForwardAndAvoid(self, error): + ts = round(time.time() * 1000) + # Reset pid loop if we haven't seen a target for a second or this is + # the first target we have seen. + if self.time_prev is None or (ts - self.time_prev) > 1000: + self.time_prev = ts - 1 # Do this to prevent a divide by zero error! + self.error_prev = error + + # Control loop + P = self.pid_info["constants"]["Kp"] * error + I = self.pid_info["constants"]["Ki"] * (ts - self.time_prev) + if error < 0: + I *= -1 + self.pid_info["saved"]["I"] += self.clamp(I, -100.0, 100.0) + D = self.pid_info["constants"]["Kd"] * (error - self.error_prev) / (ts - self.time_prev) + + roll = self.clamp(int(P + I + D), -100, 100) + self.time_prev = ts + self.error_prev = error + + await self.drone.PCMD(roll, self.speed, 0, 0) + + async def run(self): + # init + logger.info("[ObstacleTask] Started run") + self.cloudlet.switchModel(self.task_attributes["model"]) + self.create_transition() + + # logger.info(f"**************Avoid Task {self.task_id}: hi this is avoid task {self.task_id}**************\n") + # coords = ast.literal_eval(self.task_attributes["coords"]) + # await self.drone.setGimbalPose(0.0, 0.0, 0.0) + # for dest in coords: + # lng = dest["lng"] + # lat = dest["lat"] + # alt = dest["alt"] + # logger.info(f"**************Avoid Task {self.task_id}: Move **************\n") + # logger.info(f"**************Avoid Task {self.task_id}: move to {lat}, {lng}, {alt}**************\n") + # await self.drone.moveTo(lat, lng, alt) + # await asyncio.sleep(1) + # # await asyncio.sleep(hover_delay) + + # logger.info(f"**************Avoid Task {self.task_id}: Done**************\n") + # self._exit() + + # try: + while True: + result = self.cloudlet.getResults("obstacle-avoidance") + offset = 0 + try: + logger.info("[ObstacleTask] Getting results") + logger.info(f"[ObstacleTask] result: {result}") + if result is not None and result.payload_type == gabriel_pb2.TEXT: + json_string = result.payload.decode('utf-8') + json_data = json.loads(json_string) + logger.info("[ObstacleTask] Decoded results") + offset = json_data[0]['vector'] + await self.moveForwardAndAvoid(offset) + except JSONDecodeError as e: + logger.error(f"[ObstacleTask]: Error decoding JSON") + await asyncio.sleep(0.1) + # except Exception as e: + # logger.info(f"[ObstacleTask] Task failed with exception {e}") + # await self.drone.hover() \ No newline at end of file diff --git a/onboard/python/interfaces b/onboard/python/interfaces index 62a1de6a..e900f02c 120000 --- a/onboard/python/interfaces +++ b/onboard/python/interfaces @@ -1 +1 @@ -../../hermes/python/interfaces/ \ No newline at end of file +../../droneDSL/python/interfaces/ \ No newline at end of file