Commit 7ba0d5e0 authored by Barciś, Agata's avatar Barciś, Agata
Browse files

movement of balboas (commented)

parent cab08193
......@@ -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()
......
......@@ -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
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment