movement of balboas (commented)

camera_streaming/camera_stream.py

@@ -32,10 +32,11 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
 from infdist.optimization.simplesim import (
     create_msgnum_constraint_violations,
 )
+import serial
 
-from .sniffing import sniffer
-from .mission import generate_mission_context
-from .detect import detect_red
+from sniffing import sniffer
+from mission import generate_mission_context
+from detect import detect_red
 
 CUSTOM_QOS = QoSProfile(
     history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
@@ -79,6 +80,7 @@ class CameraStream(Node, MissionExecutor):
         self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
         self.t_start = datetime.now().timestamp()
         self.t_end = 30
+        # self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
         self.sniffer = threading.Thread(
             target=sniffer.sniff,
             args=(self.camera_received_callback,)
@@ -138,6 +140,10 @@ class CameraStream(Node, MissionExecutor):
         self.get_logger().info('Mission started')
         time.sleep(self.offset)
         self.timer = self.create_timer(self.timer_period, self.timer_callback)
+        # self.movement = threading.Thread(
+        #     target=self.move
+        # )
+        # self.movement.start()
 
     def end_mission(self, timestamp):
         self.get_logger().info('Mission ended')
@@ -205,6 +211,41 @@ class CameraStream(Node, MissionExecutor):
         end = datetime.now().timestamp()
         print(f'generated: {msg_generated-all_start}, agent: {end-msg_generated}')
 
+    def move(self):
+        mission = {
+            'fdrones_b01': 'wwwlaaawwlssldddl',
+            'fdrones_b02': 'ssswwwwssswwwwwww',
+            'fdrones_b03': 'wllllaldlalllaaal',
+            'fdrones_b05': 'wwllllwwllllddlld',
+            'fdrones_b06': 'wwwwllllsssssllll',
+            'fdrones_b07': 'aalddlaalwwlaaldd',
+            'fdrones_b08': 'wwwwwsssswwwsssww',
+            'fdrones_b09': 'wwellallallallall',
+        }
+
+        KEYS = {
+            "w": (1., 0., 'front'),
+            "s": (-1., 0., 'back'),
+            "a": (0., 1., 'left'),
+            "d": (0., -1., 'right'),
+            "e": (1., -1., 'front right'),
+            "q": (1., 1., 'front left'),
+            "l": (0, 0, 'sleeping'),
+            "n": (0, 0, 'noop'),
+        }
+
+        repeats = 10
+
+        for c in mission[self.hostname]:
+            for i in range(repeats):
+                linear, angular, text = KEYS[c]
+
+                self.ser.write("V {} {}\n".format(
+                    int(angular * 0.5 * 100),
+                    int(linear * 0.2 * 100)
+                ).encode('ascii'))
+                time.sleep(0.2)
+
 
 def main():
     rclpy.init()

camera_streaming/mission.py

@@ -3,7 +3,7 @@
 from infdist.optimization.models import MissionContext, InformationType
 from infdist.optimization.aggregations import AggregationMostRecent
 
-from .utility import PeriodicTypeForecast, UtilityVideo
+from utility import PeriodicTypeForecast, UtilityVideo
 # from infdist.optimization.message_forecast import PeriodicTypeForecast