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 ...@@ -32,10 +32,11 @@ from infdist.robot.ros2_network import ROS2Network, ROS2NativeMessage
from infdist.optimization.simplesim import ( from infdist.optimization.simplesim import (
create_msgnum_constraint_violations, create_msgnum_constraint_violations,
) )
import serial
from .sniffing import sniffer from sniffing import sniffer
from .mission import generate_mission_context from mission import generate_mission_context
from .detect import detect_red from detect import detect_red
CUSTOM_QOS = QoSProfile( CUSTOM_QOS = QoSProfile(
history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST, history=QoSHistoryPolicy.RMW_QOS_POLICY_HISTORY_KEEP_LAST,
...@@ -79,6 +80,7 @@ class CameraStream(Node, MissionExecutor): ...@@ -79,6 +80,7 @@ class CameraStream(Node, MissionExecutor):
self.offset = self.timer_period/8 * int(self.hosts[self.hostname]) self.offset = self.timer_period/8 * int(self.hosts[self.hostname])
self.t_start = datetime.now().timestamp() self.t_start = datetime.now().timestamp()
self.t_end = 30 self.t_end = 30
# self.ser = serial.Serial('/dev/ttyACM0', 115200, timeout=1)
self.sniffer = threading.Thread( self.sniffer = threading.Thread(
target=sniffer.sniff, target=sniffer.sniff,
args=(self.camera_received_callback,) args=(self.camera_received_callback,)
...@@ -138,6 +140,10 @@ class CameraStream(Node, MissionExecutor): ...@@ -138,6 +140,10 @@ class CameraStream(Node, MissionExecutor):
self.get_logger().info('Mission started') self.get_logger().info('Mission started')
time.sleep(self.offset) time.sleep(self.offset)
self.timer = self.create_timer(self.timer_period, self.timer_callback) 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): def end_mission(self, timestamp):
self.get_logger().info('Mission ended') self.get_logger().info('Mission ended')
...@@ -205,6 +211,41 @@ class CameraStream(Node, MissionExecutor): ...@@ -205,6 +211,41 @@ class CameraStream(Node, MissionExecutor):
end = datetime.now().timestamp() end = datetime.now().timestamp()
print(f'generated: {msg_generated-all_start}, agent: {end-msg_generated}') 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(): def main():
rclpy.init() rclpy.init()
......
...@@ -3,7 +3,7 @@ ...@@ -3,7 +3,7 @@
from infdist.optimization.models import MissionContext, InformationType from infdist.optimization.models import MissionContext, InformationType
from infdist.optimization.aggregations import AggregationMostRecent from infdist.optimization.aggregations import AggregationMostRecent
from .utility import PeriodicTypeForecast, UtilityVideo from utility import PeriodicTypeForecast, UtilityVideo
# from infdist.optimization.message_forecast import PeriodicTypeForecast # 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