diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 5ede35a5..43883158 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -3,11 +3,10 @@ import rclpy from scr.node import Node from scr.states import DeviceStateEnum -# from scr_msgs.msg import SystemState, DeviceState, Log, ConfigurationInstruction from scr_msgs.msg import SystemState, DeviceState, ConfigUpdated -from scr_msgs.srv import SetSystemState, UpdateConfig +from scr_msgs.srv import SetSystemState, UpdateConfig, SetActivePreset, SaveActivePreset, GetPresets, DeletePreset from std_msgs.msg import Empty -from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, ObjectDetection, PathingDebug, GPSFeedback, IMUData, Conbus +from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, PathingDebug, GPSFeedback, IMUData, Conbus from sensor_msgs.msg import CompressedImage import asyncio import websockets @@ -16,438 +15,365 @@ import base64 import time -big_loop = asyncio.new_event_loop() +async_loop = asyncio.new_event_loop() + class Limiter: - def __init__(self) -> None: - self.limits = {} - self.nextAllowance = {} - - # Sets a limit for how many times per second a key can be used - def setLimit(self, key, limit): - self.limits[key] = limit - self.nextAllowance[key] = 0 - - # If it can be used, returns true and decrements the remaining uses - def use(self, key): - if key not in self.limits: - return True - - nextUsageAfter = self.nextAllowance[key] - if nextUsageAfter == 0: - self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) - return True - - if time.time() >= nextUsageAfter: - self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) - return True - - return False + def __init__(self) -> None: + self.limits = {} + self.nextAllowance = {} + + # Sets a limit for how many times per second a key can be used + def setLimit(self, key, limit): + self.limits[key] = limit + self.nextAllowance[key] = 0 + + # If it can be used, returns true and decrements the remaining uses + def use(self, key): + if key not in self.limits: + return True + + nextUsageAfter = self.nextAllowance[key] + if nextUsageAfter == 0: + self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) + return True + + if time.time() >= nextUsageAfter: + self.nextAllowance[key] = time.time() + (1.0 / self.limits[key]) + return True + + return False + class BroadcastNode(Node): - def __init__(self): - super().__init__("autonav_display_broadcast") - - self.port = 8023 - self.host = "0.0.0.0" - self.sendMap = {} - self.socketMap = {} - - self.limiter = Limiter() - self.limiter.setLimit("/autonav/MotorInput", 5) - self.limiter.setLimit("/autonav/MotorFeedback", 5) - self.limiter.setLimit("/autonav/MotorControllerDebug", 1) - self.limiter.setLimit("/autonav/imu", 5) - self.limiter.setLimit("/autonav/gps", 5) - self.limiter.setLimit("/autonav/position", 5) - self.limiter.setLimit("/autonav/camera/compressed/left", 2) - self.limiter.setLimit("/autonav/camera/compressed/right", 2) - self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) - self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) - self.limiter.setLimit("/autonav/debug/astar/image", 5) - - self.get_logger().info("Broadcasting on ws://{}:{}".format(self.host, self.port)) - - self.systemStateSubscriber = self.create_subscription(SystemState, "/scr/system_state", self.systemStateCallback, 20) - self.deviceStateSubscriber = self.create_subscription(DeviceState, "/scr/device_state", self.deviceStateCallback, 20) - self.broadcastPublisher = self.create_publisher(Empty, "/scr/state/broadcast", 20) - self.configurationInstructionSubscriber = self.create_subscription(ConfigUpdated, "/scr/config_updated", self.configurationInstructionCallback, 100) - # self.logSubscriber = self.create_subscription(Log, "/scr/logging", self.logCallback, 20) - # self.configurationInstructionPublisher = self.create_publisher(ConfigUpdated, "/scr/config_updated", 100) - self.configUpdateClient = self.create_client(UpdateConfig, "/scr/update_config_client") - - self.positionSubscriber = self.create_subscription(Position, "/autonav/position", self.positionCallback, 20) - self.motorFeedbackSubscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20) - self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) - self.motorControllerDebugSubscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) - self.objectDetectionSubscriber = self.create_subscription(ObjectDetection, "/autonav/ObjectDetection", self.objectDetectionCallback, 20) - self.pathingDebugSubscriber = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) - self.gpsFeedbackSubscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) - self.imuDataSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) - self.conbusSubscriber = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) - self.conbusPublisher = self.create_publisher(Conbus, "/autonav/conbus/instruction", 100) - - self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state") - - self.cameraSubscriberLeft = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.cameraCallbackLeft, 20) - self.cameraSubscriberRight = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.cameraCallbackRight, 20) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.filteredCallbackLeft, 20) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.filteredCallbackRight, 20) - self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, 20) - self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/expanded/image", self.debugAStarCallback, 20) - - self.get_logger().info("Starting event loop") - - self.loop_thread = threading.Thread(target=self.loopthread) - self.loop_thread.start() - - def loopthread(self): - asyncio.set_event_loop(big_loop) - big_loop.run_until_complete(self.start()) - - async def start(self): - async with websockets.serve(self.handler, self.host, self.port): - self.get_logger().info("Started websocket server") - await asyncio.Future() - - def getUserIdFromSocket(self, websocket): - try: - return websocket.path.split("?")[1].split("=")[1] - except: - return None - - def pushSendQueue(self, message, unique_id=None): - if len(self.sendMap) == 0: - return - - if unique_id is None: - for unique_id in self.sendMap: - self.sendMap[unique_id].append(message) - else: - self.sendMap[unique_id].append(message) - - async def producer(self, websocket): - unqiue_id = self.getUserIdFromSocket(websocket) - while True: - if len(self.sendMap[unqiue_id]) > 0: - await websocket.send(self.sendMap[unqiue_id].pop(0)) - else: - await asyncio.sleep(0.01) - - async def consumer(self, websocket): - self.get_logger().info("New websocket connection") - unique_id = self.getUserIdFromSocket(websocket) - async for message in websocket: - obj = json.loads(message) - if obj["op"] == "broadcast": - self.broadcastPublisher.publish(Empty()) - - if obj["op"] == "configuration" and "device" in obj and "json" in obj: - self.log("Received configuration instruction for " + obj["device"]) - config_packet = UpdateConfig.Request() - config_packet.device = obj["device"] - config_packet.json = json.dumps(obj["json"]) - self.configUpdateClient.call_async(config_packet) - - if obj["op"] == "get_nodes": - nodes = self.get_node_names() - for i in range(len(nodes)): - nodes[i] = nodes[i].replace("/", "") - node_states = {} - for identifier in nodes: - node_states[identifier] = self.device_states[identifier] if identifier in self.device_states else 0 - self.pushSendQueue(json.dumps({ - "op": "get_nodes_callback", - "nodes": nodes, - "states": node_states, - "configs": self.node_configs - }), unique_id) - - if obj["op"] == "set_system_state": - self.set_system_total_state(int(obj["state"]), self.system_mode, bool(obj["mobility"])) - - # if obj["op"] == "conbus": - # id = int(obj["id"]) - # data = list(map(lambda x: int(x), obj["data"])) - # msg = Conbus() - # msg.id = id - # msg.data = data - # msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 - # self.conbusPublisher.publish(msg) - - async def handler(self, websocket): - unique_id = self.getUserIdFromSocket(websocket) - if unique_id in self.socketMap or unique_id is None: - await websocket.close() - return - - self.socketMap[unique_id] = websocket - self.sendMap[unique_id] = [] - - consumer_task = asyncio.create_task(self.consumer(websocket)) - producer_task = asyncio.create_task(self.producer(websocket)) - pending = await asyncio.wait( - [consumer_task, producer_task], - return_when=asyncio.FIRST_COMPLETED - ) - for task in pending: - for t in task: - t.cancel() - - del self.socketMap[unique_id] - del self.sendMap[unique_id] - - def systemStateCallback(self, msg: SystemState): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/state/system", - "state": msg.state, - "estop": msg.estop, - "mobility": msg.mobility, - "mode": msg.mode - })) - - def deviceStateCallback(self, msg: DeviceState): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/state/device", - "state": msg.state, - "device": msg.device - })) - - # def logCallback(self, msg: Log): - # if msg.node == "autonav_display_broadcast": - # return - # self.pushSendQueue(json.dumps({ - # "op": "data", - # "topic": "/scr/logging", - # "data": msg.data, - # "node": msg.node - # })) - - def configurationInstructionCallback(self, msg: ConfigUpdated): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/scr/configuration", - "data": msg.json - })) - - def positionCallback(self, msg: Position): - if not self.limiter.use("/autonav/position"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/position", - "x": msg.x, - "y": msg.y, - "theta": msg.theta, - "latitude": msg.latitude, - "longitude": msg.longitude - })) - - def motorInputCallback(self, msg: MotorInput): - if not self.limiter.use("/autonav/MotorInput"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorInput", - "angular_velocity": msg.angular_velocity, - "forward_velocity": msg.forward_velocity - })) - - def motorFeedbackCallback(self, msg: MotorFeedback): - if not self.limiter.use("/autonav/MotorFeedback"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorFeedback", - "delta_x": msg.delta_x, - "delta_y": msg.delta_y, - "delta_theta": msg.delta_theta - })) - - def imuDataCallback(self, msg: IMUData): - if not self.limiter.use("/autonav/imu"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/imu", - "accel_x": msg.accel_x, - "accel_y": msg.accel_y, - "accel_z": msg.accel_z, - "angular_x": msg.angular_x, - "angular_y": msg.angular_y, - "angular_z": msg.angular_z, - "yaw": msg.yaw, - "pitch": msg.pitch, - "roll": msg.roll - })) - - def gpsFeedbackCallback(self, msg: GPSFeedback): - if not self.limiter.use("/autonav/gps"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/gps", - "latitude": msg.latitude, - "longitude": msg.longitude, - "altitude": msg.altitude, - "satellites": msg.satellites, - "is_locked": msg.is_locked, - "gps_fix": msg.gps_fix - })) - - def pathingDebugCallback(self, msg: PathingDebug): - if not self.limiter.use("/autonav/debug/astar"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/debug/astar", - "desired_heading": msg.desired_heading, - "desired_latitude": msg.desired_latitude, - "desired_longitude": msg.desired_longitude, - "distance_to_destination": msg.distance_to_destination, - "waypoints": msg.waypoints.tolist(), - "time_until_use_waypoints": msg.time_until_use_waypoints, - })) - - def objectDetectionCallback(self, msg: ObjectDetection): - if not self.limiter.use("/autonav/ObjectDetection"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/ObjectDetection", - "sensor_1": msg.sensor_1, - "sensor_2": msg.sensor_2, - "sensor_3": msg.sensor_3 - })) - - def motorControllerDebugCallback(self, msg: MotorControllerDebug): - if not self.limiter.use("/autonav/MotorControllerDebug"): - return - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/MotorControllerDebug", - "current_forward_velocity": msg.current_forward_velocity, - "forward_velocity_setpoint": msg.forward_velocity_setpoint, - "current_angular_velocity": msg.current_angular_velocity, - "angular_velocity_setpoint": msg.angular_velocity_setpoint, - "left_motor_output": msg.left_motor_output, - "right_motor_output": msg.right_motor_output - })) - - def cameraCallbackLeft(self, msg: CompressedImage): - if not self.limiter.use("/autonav/camera/compressed/left"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/camera/compressed/left", - "format": msg.format, - "data": base64_str - })) - - def cameraCallbackRight(self, msg: CompressedImage): - if not self.limiter.use("/autonav/camera/compressed/right"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/camera/compressed/right", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackLeft(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/left"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/left", - "format": msg.format, - "data": base64_str - })) - - def filteredCallbackRight(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image/right"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/image/right", - "format": msg.format, - "data": base64_str - })) - - def bigboiCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/debug"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/cfg_space/raw/debug", - "format": msg.format, - "data": base64_str - })) - - def debugAStarCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/debug/astar/image"): - return - - byts = msg.data.tobytes() - base64_str = base64.b64encode(byts).decode("utf-8") - - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/debug/astar/image", - "format": msg.format, - "data": base64_str - })) - - def conbusCallback(self, msg: Conbus): - self.pushSendQueue(json.dumps({ - "op": "data", - "topic": "/autonav/conbus", - "id": msg.id, - "data": msg.data.tolist(), - "iterator": msg.iterator - })) - - def init(self): - self.set_device_state(DeviceStateEnum.OPERATING) + def __init__(self): + super().__init__("autonav_display_broadcast") + + self.port = 8023 + self.host = "0.0.0.0" + self.send_map = {} + self.client_map = {} + + # Limiter + self.limiter = Limiter() + self.limiter.setLimit("/autonav/MotorInput", 5) + self.limiter.setLimit("/autonav/MotorFeedback", 5) + self.limiter.setLimit("/autonav/MotorControllerDebug", 1) + self.limiter.setLimit("/autonav/imu", 5) + self.limiter.setLimit("/autonav/gps", 5) + self.limiter.setLimit("/autonav/position", 5) + self.limiter.setLimit("/autonav/camera/compressed/left", 2) + self.limiter.setLimit("/autonav/camera/compressed/right", 2) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/left_small", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/image/right_small", 5) + self.limiter.setLimit("/autonav/cfg_space/combined/image", 5) + self.limiter.setLimit("/autonav/cfg_space/raw/debug", 5) + self.limiter.setLimit("/autonav/debug/astar/image", 5) + self.limiter.setLimit("/autonav/debug/astar", 5) + + # Clients + self.system_state_c = self.create_subscription(SystemState, "/scr/system_state", self.systemStateCallback, 20) + self.system_state_c = self.create_client(SetSystemState, "/scr/state/set_system_state") + self.config_c = self.create_client(UpdateConfig, "/scr/update_config_client") + self.get_presets_c = self.create_client(GetPresets, "/scr/get_presets") + self.set_active_preset_c = self.create_client(SetActivePreset, "/scr/set_active_preset") + self.save_active_preset_c = self.create_client(SaveActivePreset, "/scr/save_active_preset") + self.delete_preset_c = self.create_client(DeletePreset, "/scr/delete_preset") + + # Publishers + self.conbus_p = self.create_publisher(Conbus, "/autonav/conbus/instruction", 100) + self.broadcast_p = self.create_publisher(Empty, "/scr/state/broadcast", 20) + + # Subscriptions + self.device_state_s = self.create_subscription(DeviceState, "/scr/device_state", self.deviceStateCallback, 20) + self.config_s = self.create_subscription(ConfigUpdated, "/scr/config_updated", self.configurationInstructionCallback, 10) + self.position_s = self.create_subscription(Position, "/autonav/position", self.positionCallback, 20) + self.motor_feedback_s = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.motorFeedbackCallback, 20) + self.motor_input_s = self.create_subscription(MotorInput, "/autonav/MotorInput", self.motorInputCallback, 20) + self.motor_debug_s = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.motorControllerDebugCallback, 20) + self.pathing_s = self.create_subscription(PathingDebug, "/autonav/debug/astar", self.pathingDebugCallback, 20) + self.gps_s = self.create_subscription(GPSFeedback, "/autonav/gps", self.gpsFeedbackCallback, 20) + self.imu_s = self.create_subscription(IMUData, "/autonav/imu", self.imuDataCallback, 20) + self.conbus_s = self.create_subscription(Conbus, "/autonav/conbus/data", self.conbusCallback, 100) + self.camera_left_s = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left/cutout", self.cameraCallbackLeft, self.qos_profile) + self.camera_right_s = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right/cutout", self.cameraCallbackRight, self.qos_profile) + self.filtered_left_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filteredCallbackLeftSmall, self.qos_profile) + self.filtered_right_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filteredCallbackRightSmall, self.qos_profile) + self.combined_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/combined/image", self.filteredCallbackCombined, self.qos_profile) + self.inflated_s = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.inflated_callback, self.qos_profile) + self.astar_debug_s = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, self.qos_profile) + + self.loop_thread = threading.Thread(target=self.loopthread) + self.loop_thread.start() + + def loopthread(self): + asyncio.set_event_loop(async_loop) + async_loop.run_until_complete(self.start()) + + async def start(self): + async with websockets.serve(self.handler, self.host, self.port): + self.get_logger().info("Started websocket server") + await asyncio.Future() + + def get_id_from_socket(self, websocket): + try: + return websocket.path.split("?")[1].split("=")[1] + except: + return None + + def push_image(self, topic, msg): + if not self.limiter.use(topic): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.push_old(json.dumps({ + "op": "data", + "topic": topic, + "format": msg.format, + "data": base64_str + })) + + def push(self, topic, data, unique_id=None): + # Check limiter + if not self.limiter.use(topic): + return + + # Create packet + packet = { + "op": "data", + "topic": topic, + } + + # Copy properties from data to packet + for key in data.get_fields_and_field_types().keys(): + packet[key] = getattr(data, key) + + # Check if there are any clients + if len(self.send_map) == 0: + return + + # Convert to json + message_json = json.dumps(packet) + + # Send it out as needed + if unique_id is None: + for unique_id in self.send_map: + self.send_map[unique_id].append(message_json) + else: + self.send_map[unique_id].append(message_json) + + def push_old(self, message, unique_id=None): + if len(self.send_map) == 0: + return + + if unique_id is None: + for unique_id in self.send_map: + self.send_map[unique_id].append(message) + else: + self.send_map[unique_id].append(message) + + async def producer(self, websocket): + unqiue_id = self.get_id_from_socket(websocket) + while True: + if len(self.send_map[unqiue_id]) > 0: + await websocket.send(self.send_map[unqiue_id].pop(0)) + else: + await asyncio.sleep(0.01) + + async def consumer(self, websocket): + unique_id = self.get_id_from_socket(websocket) + async for message in websocket: + obj = json.loads(message) + if obj["op"] == "broadcast": + self.broadcast_p.publish(Empty()) + + if obj["op"] == "configuration" and "device" in obj and "json" in obj: + config_packet = UpdateConfig.Request() + config_packet.device = obj["device"] + config_packet.json = json.dumps(obj["json"]) + self.config_c.call_async(config_packet) + + if obj["op"] == "get_nodes": + nodes = self.get_node_names() + for i in range(len(nodes)): + nodes[i] = nodes[i].replace("/", "") + node_states = {} + for identifier in nodes: + node_states[identifier] = self.device_states[identifier] if identifier in self.device_states else 0 + self.push_old(json.dumps({ + "op": "get_nodes_callback", + "nodes": nodes, + "states": node_states, + "configs": self.node_configs, + "system": { + "state": self.system_state, + "mode": self.system_mode, + "mobility": self.mobility + } + }), unique_id) + + if obj["op"] == "set_system_state": + self.set_system_total_state(int(obj["state"]), self.system_mode, bool(obj["mobility"])) + + if obj["op"] == "conbus": + id = int(obj["id"]) + data = list(map(lambda x: int(x), obj["data"])) + msg = Conbus() + msg.id = id + msg.data = data + msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 + self.conbus_p.publish(msg) + + if obj["op"] == "get_presets": + req = GetPresets.Request() + req.empty = True + res = self.get_presets_c.call_async(req) + res.add_done_callback(self.get_presets_callback) + + if obj["op"] == "set_active_preset": + req = SetActivePreset.Request() + req.preset = obj["preset"] + self.set_active_preset_c.call_async(req) + + if obj["op"] == "save_preset_mode": + req = SaveActivePreset.Request() + req.write_mode = True + req.preset_name = "" + self.save_active_preset_c.call_async(req) + + if obj["op"] == "save_preset_as": + req = SaveActivePreset.Request() + req.preset_name = obj["preset"] + req.write_mode = False + self.save_active_preset_c.call_async(req) + + if obj["op"] == "delete_preset": + req = DeletePreset.Request() + req.preset = obj["preset"] + self.delete_preset_c.call_async(req) + + def get_presets_callback(self, future): + response = future.result() + self.push_old(json.dumps({ + "op": "get_presets_callback", + "presets": response.presets, + "active_preset": response.active_preset + })) + + async def handler(self, websocket): + unique_id = self.get_id_from_socket(websocket) + if unique_id in self.client_map or unique_id is None: + await websocket.close() + return + + self.client_map[unique_id] = websocket + self.send_map[unique_id] = [] + + consumer_task = asyncio.create_task(self.consumer(websocket)) + producer_task = asyncio.create_task(self.producer(websocket)) + pending = await asyncio.wait( + [consumer_task, producer_task], + return_when=asyncio.FIRST_COMPLETED + ) + for task in pending: + for t in task: + t.cancel() + + del self.client_map[unique_id] + del self.send_map[unique_id] + + def systemStateCallback(self, msg: SystemState): + self.push("/scr/state/system", msg) + + def deviceStateCallback(self, msg: DeviceState): + self.push("/scr/state/device", msg) + + def configurationInstructionCallback(self, msg: ConfigUpdated): + self.push("/scr/configuration", msg) + + def positionCallback(self, msg: Position): + self.push("/autonav/position", msg) + + def motorInputCallback(self, msg: MotorInput): + self.push("/autonav/MotorInput", msg) + + def motorFeedbackCallback(self, msg: MotorFeedback): + self.push("/autonav/MotorFeedback", msg) + + def imuDataCallback(self, msg: IMUData): + self.push("/autonav/imu", msg) + + def gpsFeedbackCallback(self, msg: GPSFeedback): + self.push("/autonav/gps", msg) + + def pathingDebugCallback(self, msg: PathingDebug): + if not self.limiter.use("/autonav/debug/astar"): + return + + self.push_old(json.dumps({ + "op": "data", + "topic": "/autonav/debug/astar", + "desired_heading": msg.desired_heading, + "desired_latitude": msg.desired_latitude, + "desired_longitude": msg.desired_longitude, + "distance_to_destination": msg.distance_to_destination, + "waypoints": msg.waypoints.tolist(), + "time_until_use_waypoints": msg.time_until_use_waypoints, + })) + + def motorControllerDebugCallback(self, msg: MotorControllerDebug): + self.push("/autonav/MotorControllerDebug", msg) + + def cameraCallbackLeft(self, msg: CompressedImage): + self.push_image("/autonav/camera/compressed/left", msg) + + def cameraCallbackRight(self, msg: CompressedImage): + self.push_image("/autonav/camera/compressed/right", msg) + def filteredCallbackLeft(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/left", msg) + + def filteredCallbackRight(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/right", msg) + + def filteredCallbackLeftSmall(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/left_small", msg) + + def filteredCallbackRightSmall(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/image/right_small", msg) + + def filteredCallbackCombined(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/combined/image", msg) + + def inflated_callback(self, msg: CompressedImage): + self.push_image("/autonav/cfg_space/raw/debug", msg) + + def debugAStarCallback(self, msg: CompressedImage): + self.push_image("/autonav/debug/astar/image", msg) + + def conbusCallback(self, msg: Conbus): + self.push_old(json.dumps({ + "op": "data", + "topic": "/autonav/conbus", + "id": msg.id, + "data": msg.data.tolist(), + "iterator": msg.iterator + })) + + def init(self): + self.set_device_state(DeviceStateEnum.OPERATING) def main(): - rclpy.init() - node = BroadcastNode() - Node.run_node(node) - rclpy.shutdown() + rclpy.init() + node = BroadcastNode() + Node.run_node(node) + rclpy.shutdown() + if __name__ == "__main__": - main() \ No newline at end of file + main() diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/autonav_ws/src/autonav_filters/src/filters.py index 27af58bb..ca1c0fd9 100644 --- a/autonav_ws/src/autonav_filters/src/filters.py +++ b/autonav_ws/src/autonav_filters/src/filters.py @@ -12,6 +12,7 @@ import rclpy import math + class FilterType(IntEnum): DEAD_RECKONING = 0, PARTICLE_FILTER = 1 @@ -20,25 +21,22 @@ class FilterType(IntEnum): class FiltersNodeConfig: def __init__(self): self.filter_type = FilterType.PARTICLE_FILTER - self.degree_offset = 107.0 - self.seed_heading = False class FiltersNode(Node): def __init__(self): super().__init__("autonav_filters") - self.lastIMUReceived = None - self.firstGps = None - self.lastGps = None - - self.latitudeLength = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value - self.longitudeLength = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value - - self.pf = ParticleFilter(self.latitudeLength, self.longitudeLength) - self.reckoning = DeadReckoningFilter() + self.first_gps = None + self.last_gps = None + + self.latitude_length = self.declare_parameter("latitude_length", 111086.2).get_parameter_value().double_value + self.longitude_length = self.declare_parameter("longitude_length", 81978.2).get_parameter_value().double_value + + self.pf = ParticleFilter(self.latitude_length, self.longitude_length) + self.dr = DeadReckoningFilter() self.config = self.get_default_config() - + self.onReset() def config_updated(self, jsonObject): @@ -49,29 +47,14 @@ def get_default_config(self): def init(self): self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20) - self.create_subscription(IMUData, "/autonav/imu", self.onIMUReceived, 20); self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.onMotorFeedbackReceived, 20) - self.positionPublisher = self.create_publisher(Position, "/autonav/position", 20) + self.position_publisher = self.create_publisher(Position, "/autonav/position", 20) self.set_device_state(DeviceStateEnum.OPERATING) - - def onIMUReceived(self, msg: IMUData): - self.lastIMUReceived = msg - - def getRealHeading(self, heading: float): - if heading < 0: - heading = 360 + -heading - - heading += self.config.degree_offset - return heading def onReset(self): - if self.lastIMUReceived is not None and self.config.seed_heading: - self.reckoning.reset(self.getRealHeading(self.lastIMUReceived.heading)) - self.pf.init_particles(self.getRealHeading(self.lastIMUReceived.heading), True) - else: - self.reckoning.reset() - self.pf.init_particles() + self.dr.reset() + self.pf.init_particles() def system_state_transition(self, old: SystemState, updated: SystemState): if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS: @@ -79,50 +62,46 @@ def system_state_transition(self, old: SystemState, updated: SystemState): if old.mobility == False and updated.mobility == True: self.onReset() - + def onGPSReceived(self, msg: GPSFeedback): if msg.gps_fix == 0 and msg.is_locked == False: return - - if self.firstGps is None: - self.firstGps = msg - self.lastGps = msg + if self.first_gps is None: + self.first_gps = msg - filterType = self.config.filter_type - if filterType == FilterType.PARTICLE_FILTER: + self.last_gps = msg + if self.config.filter_type == FilterType.PARTICLE_FILTER: self.pf.gps(msg) - elif filterType == FilterType.DEAD_RECKONING: - self.reckoning.gps(msg) + else: + self.dr.gps(msg) def onMotorFeedbackReceived(self, msg: MotorFeedback): - filterType = self.config.filter_type averages = None - if filterType == FilterType.PARTICLE_FILTER: + if self.config.filter_type == FilterType.PARTICLE_FILTER: averages = self.pf.feedback(msg) - if filterType == FilterType.DEAD_RECKONING: - averages = self.reckoning.feedback(msg) - + else: + averages = self.dr.feedback(msg) + if averages is None: return - + position = Position() position.x = averages[0] position.y = averages[1] position.theta = (-1 * math.pi * 2 + averages[2]) * 1 - - if self.firstGps is not None: - gps_x = self.firstGps.latitude + position.x / self.latitudeLength - gps_y = self.firstGps.longitude - position.y / self.longitudeLength + if self.first_gps is not None: + gps_x = self.first_gps.latitude + position.x / self.latitude_length + gps_y = self.first_gps.longitude - position.y / self.longitude_length position.latitude = gps_x position.longitude = gps_y - if self.system_mode == SystemModeEnum.SIMULATION and self.lastGps is not None: - position.latitude = self.lastGps.latitude - position.longitude = self.lastGps.longitude - - self.positionPublisher.publish(position) + if self.system_mode == SystemModeEnum.SIMULATION and self.last_gps is not None: + position.latitude = self.last_gps.latitude + position.longitude = self.last_gps.longitude + + self.position_publisher.publish(position) def main(): diff --git a/autonav_ws/src/autonav_filters/src/particlefilter.py b/autonav_ws/src/autonav_filters/src/particlefilter.py index 35f0b7c8..b88a6d8c 100644 --- a/autonav_ws/src/autonav_filters/src/particlefilter.py +++ b/autonav_ws/src/autonav_filters/src/particlefilter.py @@ -3,13 +3,15 @@ import math import random + class Particle: - def __init__(self, x = 0, y = 0, theta = 0, weight = 1) -> None: + def __init__(self, x=0, y=0, theta=0, weight=1) -> None: self.x = x self.y = y self.theta = theta self.weight = weight + class ParticleFilter: def __init__(self, latitudeLength, longitudeLength) -> None: self.num_particles = 750 @@ -17,9 +19,9 @@ def __init__(self, latitudeLength, longitudeLength) -> None: self.odom_noise = [0.05, 0.05, 0.1] self.init_particles() self.first_gps = None - - self.latitudeLength = latitudeLength - self.longitudeLength = longitudeLength + + self.latitude_length = latitudeLength + self.longitude_length = longitudeLength def init_particles(self, seedHeading: float = 0.0, useSeedHeading: bool = False): if useSeedHeading: @@ -33,7 +35,7 @@ def feedback(self, feedback: MotorFeedback) -> list[float]: sum_theta_x = 0 sum_theta_y = 0 sum_weight = 0 - + for particle in self.particles: particle.x += feedback.delta_x * 1.2 * math.cos(particle.theta) + feedback.delta_y * math.sin(particle.theta) particle.y += feedback.delta_x * 1.2 * math.sin(particle.theta) + feedback.delta_y * math.cos(particle.theta) @@ -45,44 +47,44 @@ def feedback(self, feedback: MotorFeedback) -> list[float]: sum_theta_x += math.cos(particle.theta) * weight sum_theta_y += math.sin(particle.theta) * weight sum_weight += weight - + if sum_weight < 0.000001: sum_weight = 0.000001 - + avg_x = sum_x / sum_weight avg_y = sum_y / sum_weight avg_theta = math.atan2(sum_theta_y / sum_weight, sum_theta_x / sum_weight) % (2 * math.pi) - + return [avg_x, avg_y, avg_theta] - + def gps(self, gps: GPSFeedback) -> list[float]: if self.first_gps is None: self.first_gps = gps - - gps_x = (gps.latitude - self.first_gps.latitude) * self.latitudeLength - gps_y = (self.first_gps.longitude - gps.longitude) * self.longitudeLength - + + gps_x = (gps.latitude - self.first_gps.latitude) * self.latitude_length + gps_y = (self.first_gps.longitude - gps.longitude) * self.longitude_length + for particle in self.particles: dist_sqrt = np.sqrt((particle.x - gps_x) ** 2 + (particle.y - gps_y) ** 2) particle.weight = math.exp(-dist_sqrt / (2 * self.gps_noise[0] ** 2)) - + self.resample() return [gps_x, gps_y] - + def resample(self) -> None: weights = [particle.weight for particle in self.particles] weights_sum = sum(weights) if weights_sum <= 0.00001: weights_sum = 0.00001 weights = [weight / weights_sum for weight in weights] - - new_particles = random.choices(self.particles, weights, k = self.num_particles) + + new_particles = random.choices(self.particles, weights, k=self.num_particles) self.particles = [] - + for particle in new_particles: rand_x = np.random.normal(0, self.odom_noise[0]) rand_y = np.random.normal(0, self.odom_noise[1]) x = particle.x + rand_x * math.cos(particle.theta) + rand_y * math.sin(particle.theta) y = particle.y + rand_x * math.sin(particle.theta) + rand_y * math.cos(particle.theta) theta = np.random.normal(particle.theta, self.odom_noise[2]) % (2 * math.pi) - self.particles.append(Particle(x, y, theta, particle.weight)) \ No newline at end of file + self.particles.append(Particle(x, y, theta, particle.weight)) diff --git a/autonav_ws/src/autonav_launch/launch/competition.xml b/autonav_ws/src/autonav_launch/launch/competition.xml index 662946c6..33e17eb1 100644 --- a/autonav_ws/src/autonav_launch/launch/competition.xml +++ b/autonav_ws/src/autonav_launch/launch/competition.xml @@ -1,22 +1,18 @@ - - - - - + - + @@ -25,7 +21,12 @@ + + + + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 80476b3f..cec156a8 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,17 +1,12 @@ - - - - - + - - - + + @@ -25,6 +20,8 @@ + + diff --git a/autonav_ws/src/autonav_launch/launch/practice.xml b/autonav_ws/src/autonav_launch/launch/practice.xml index 80476b3f..8eee093c 100644 --- a/autonav_ws/src/autonav_launch/launch/practice.xml +++ b/autonav_ws/src/autonav_launch/launch/practice.xml @@ -1,17 +1,12 @@ - - - - - + - @@ -25,6 +20,7 @@ + diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index 1de7695f..2252c411 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -3,22 +3,27 @@ - - + + - - - + + + + + + + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_manual/src/steam.py b/autonav_ws/src/autonav_manual/src/steam.py index 38a7e5e5..47e7cc52 100644 --- a/autonav_ws/src/autonav_manual/src/steam.py +++ b/autonav_ws/src/autonav_manual/src/steam.py @@ -90,6 +90,7 @@ def onButtonReleased(self, button: SteamControllerButton, msTime: float): self.safetyLightsPublisher.publish(toSafetyLights(False, False, 2, 100, "#FF6F00")) if button == SteamControllerButton.STEAM and self.system_state != SystemStateEnum.AUTONOMOUS: + self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 100, "#FF0000")) self.set_system_state(SystemStateEnum.AUTONOMOUS) if button == SteamControllerButton.BACK and self.system_state != SystemStateEnum.DISABLED: diff --git a/autonav_ws/src/autonav_msgs/CMakeLists.txt b/autonav_ws/src/autonav_msgs/CMakeLists.txt index ef949a6d..2babe12a 100644 --- a/autonav_ws/src/autonav_msgs/CMakeLists.txt +++ b/autonav_ws/src/autonav_msgs/CMakeLists.txt @@ -20,7 +20,6 @@ set(msg_files "msg/Position.msg" "msg/GoalPoint.msg" "msg/Waypoint.msg" - "msg/ObjectDetection.msg" "msg/Path.msg" "msg/Obstacle.msg" "msg/Obstacles.msg" diff --git a/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg b/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg deleted file mode 100644 index a25a0f67..00000000 --- a/autonav_ws/src/autonav_msgs/msg/ObjectDetection.msg +++ /dev/null @@ -1,3 +0,0 @@ -byte sensor_1 -byte sensor_2 -byte sensor_3 \ No newline at end of file diff --git a/autonav_ws/src/autonav_nav/CMakeLists.txt b/autonav_ws/src/autonav_nav/CMakeLists.txt index ae34a5e5..3d3ac60f 100644 --- a/autonav_ws/src/autonav_nav/CMakeLists.txt +++ b/autonav_ws/src/autonav_nav/CMakeLists.txt @@ -14,9 +14,9 @@ ament_python_install_package(${PROJECT_NAME}) # Install Python executables install(PROGRAMS + src/pure_pursuit.py src/path_resolver.py src/astar.py - src/pure_pursuit.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index f64da18e..63042404 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -20,30 +20,21 @@ GRID_SIZE = 0.1 -VERTICAL_CAMERA_DIST = 2.75 -HORIZONTAL_CAMERA_DIST = 3 CV_BRIDGE = cv_bridge.CvBridge() -# STARTING_PT = (42.6681268, -83.218887) - competition_waypoints = [ - # Start Nomans, Frist Ramp, Middle Ramp, End Ramp, End Nomans - [(42.6682837222, -83.2193403028), (42.6681206444, -83.2193606083), (42.66809863885, -83.2193606083), (42.6680766333, -83.2193606083), - (42.6679277056, -83.2193276417), (42.6679817056, -83.2193316417), (42.66794, -83.2189), (42.6681268, -83.218887)], # Start Facing North - # [(42.6679277056,-83.2193276417),(42.6680766333,-83.2193591583),(42.6681206444,-83.2193606083),(42.6682837222 ,-83.2193403028), (42.668290020, -83.2189080), (42.6681268, -83.218887)], # Start Facing South - # [(42.668222,-83.218472),(42.6680859611,-83.2184456444),(42.6679600583,-83.2184326556)], # Start Facing North - # [(42.6679600583,-83.2184326556), (42.6680859611,-83.2184456444),(42.668222,-83.218472)], # Start Facing South - [], + [], # NORTH + [] # SOUTH ] practice_waypoints = [ - [(35.2104852, -97.44193), (35.210483, -97.44207), (35.2104841, -97.4421986), (35.2104819, -97.4423302), (35.2105455, -97.4423329), - (35.2106096, -97.4423347), (35.2106107, -97.4422153), (35.2106078, -97.4421059), (35.2105575, -97.4420365), (35.2104852, -97.44193)] + [], # NORTH + [] # SOUTH ] simulation_waypoints = [ - [(35.19505, -97.43823),(35.19492, -97.43824),(35.19485, -97.43824),(35.19471, -97.43823)], # Facing North - [(35.19471, -97.43823),(35.19492, -97.43824),(35.19485, -97.43824),(35.19505, -97.43823)], # Facing South + [(35.19505, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19471, -97.43823)], # NORTH + [(35.19471, -97.43823), (35.19492, -97.43824), (35.19485, -97.43824), (35.19505, -97.43823)] # SOUTH ] @@ -70,8 +61,13 @@ class AStarNodeConfig: def __init__(self): self.waypointPopDistance = 1.1 self.waypointDirection = 0 + self.calculateWaypointDirection = False self.useOnlyWaypoints = False - self.waypointDelay = 10 + self.waypointDelay = 18 + self.waypointWeight = 1.0 + self.waypointMaxWeight = 10.0 + self.horizontal_fov = 3.4 + self.vertical_fov = 2.75 class AStarNode(Node): @@ -90,17 +86,16 @@ def get_default_config(self): return AStarNodeConfig() def init(self): - self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.onConfigSpaceReceived, 20) - self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.onPoseReceived, 20) - self.imuSubscriber = self.create_subscription(IMUData, "/autonav/imu", self.onImuReceived, 20) + self.configSpaceSubscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/expanded", self.cfg_space_Received, 20) + self.poseSubscriber = self.create_subscription(Position, "/autonav/position", self.pose_received, 20) self.debugPublisher = self.create_publisher(PathingDebug, "/autonav/debug/astar", 20) self.pathPublisher = self.create_publisher(Path, "/autonav/path", 20) self.safetyLightsPublisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) - self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", 20) - self.mapTimer = self.create_timer(0.1, self.createPath) + self.pathDebugImagePublisher = self.create_publisher(CompressedImage, "/autonav/debug/astar/image", self.qos_profile) + self.map_timer = self.create_timer(0.1, self.create_path) - self.resetWhen = -1.0 - self.waypointTime = time.time() + self.config.waypointDelay + self.reset_at = -1.0 + self.waypoint_start_time = time.time() + self.config.waypointDelay self.set_device_state(DeviceStateEnum.OPERATING) def getAngleDifference(self, to_angle, from_angle): @@ -108,20 +103,19 @@ def getAngleDifference(self, to_angle, from_angle): delta = (delta + math.pi) % (2 * math.pi) - math.pi return delta - def onImuReceived(self, msg: IMUData): - self.imu = msg - def onReset(self): - self.imu = None - self.lastPath = None + self.last_path = None self.position = None - self.configSpace = None - self.costMap = None - self.bestPosition = (0, 0) + self.cfg_spce = None + self.cost_map = None + self.best_pos = (0, 0) self.waypoints = [] - self.waypointTime = self.config.waypointDelay + time.time() + self.waypoint_start_time = self.config.waypointDelay + time.time() - def getWaypointsForDirection(self): + def get_waypoints_for_dir(self): + if not self.config.calculateWaypointDirection: + return simulation_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[self.config.waypointDirection] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[self.config.waypointDirection] + # Get out current heading and estimate within 180 degrees which direction we are facing (north or south, 0 and 1 respectively) heading = self.position.theta direction_index = 0 @@ -135,43 +129,45 @@ def getWaypointsForDirection(self): return simulation_waypoints[direction_index] if self.system_mode == SystemModeEnum.SIMULATION else competition_waypoints[direction_index] if self.system_mode == SystemModeEnum.COMPETITION else practice_waypoints[direction_index] - def transition(self, old: SystemState, updated: SystemState): + def system_state_transition(self, old: SystemState, updated: SystemState): if updated.state == SystemStateEnum.AUTONOMOUS and updated.mobility and len(self.waypoints) == 0: - self.waypointTime = time.time() + self.config.waypointDelay + self.waypoint_start_time = time.time() + self.config.waypointDelay if updated.state != SystemStateEnum.AUTONOMOUS and self.device_state == DeviceStateEnum.OPERATING: self.onReset() - def onPoseReceived(self, msg: Position): + def pose_received(self, msg: Position): self.position = msg - def createPath(self): - if self.position is None or self.costMap is None: + def create_path(self): + if self.position is None or self.cost_map is None: return robot_pos = (40, 78) - path = self.findPathToPoint(robot_pos, self.bestPosition, self.costMap, 80, 80) + path = self.find_path_to_point(robot_pos, self.best_pos, self.cost_map, 80, 80) if path is not None: global_path = Path() - global_path.poses = [self.pathToGlobalPose(pp[0], pp[1]) for pp in path] - self.lastPath = path - self.pathPublisher.publish(global_path) + global_path.poses = [self.path_to_global(pp[0], pp[1]) for pp in path] + self.last_path = path + + if self.system_state == SystemStateEnum.AUTONOMOUS and self.mobility: + self.pathPublisher.publish(global_path) # Draw the cost map onto a debug iamge cvimg = np.zeros((80, 80), dtype=np.uint8) for i in range(80): for j in range(80): - cvimg[i, j] = self.costMap[i * 80 + j] * 255 + cvimg[i, j] = self.cost_map[i * 80 + j] * 255 cvimg = cv2.cvtColor(cvimg, cv2.COLOR_GRAY2RGB) for pp in path: cv2.circle(cvimg, (pp[0], pp[1]), 1, (0, 255, 0), 1) - cv2.circle(cvimg, (self.bestPosition[0], self.bestPosition[1]), 1, (255, 0, 0), 1) - cvimg = cv2.resize(cvimg, (800, 800),interpolation=cv2.INTER_NEAREST) + cv2.circle(cvimg, (self.best_pos[0], self.best_pos[1]), 1, (255, 0, 0), 1) + cvimg = cv2.resize(cvimg, (320, 320), interpolation=cv2.INTER_NEAREST) self.pathDebugImagePublisher.publish(CV_BRIDGE.cv2_to_compressed_imgmsg(cvimg)) - def reconstructPath(self, path, current): + def reconstruct_path(self, path, current): total_path = [current] while current in path: @@ -180,7 +176,7 @@ def reconstructPath(self, path, current): return total_path[::-1] - def findPathToPoint(self, start, goal, map, width, height): + def find_path_to_point(self, start, goal, map, width, height): looked_at = np.zeros((80, 80)) open_set = [start] path = {} @@ -217,7 +213,7 @@ def getG(pt): looked_at[current[0], current[1]] = 1 if current == goal: - return self.reconstructPath(path, current) + return self.reconstruct_path(path, current) open_set.remove(current) for delta_x, delta_y, dist in search_dirs: @@ -235,8 +231,8 @@ def getG(pt): open_set.append(neighbor) heappush(next_current, (fScore[neighbor], neighbor)) - def onConfigSpaceReceived(self, msg: OccupancyGrid): - if self.position is None or self.system_state != SystemStateEnum.AUTONOMOUS: + def cfg_space_Received(self, msg: OccupancyGrid): + if self.position is None: return grid_data = msg.data @@ -250,31 +246,31 @@ def onConfigSpaceReceived(self, msg: OccupancyGrid): if self.config.useOnlyWaypoints == True: grid_data = [0] * len(msg.data) - if len(self.waypoints) == 0 and time.time() > self.waypointTime and self.waypointTime != 0: - self.waypoints = [wp for wp in self.getWaypointsForDirection()] - self.waypointTime = 0 + if len(self.waypoints) == 0 and time.time() > self.waypoint_start_time and self.waypoint_start_time != 0: + self.waypoints = [wp for wp in self.get_waypoints_for_dir()] + self.waypoint_start_time = 0 - if time.time() < self.waypointTime and len(self.waypoints) == 0: - time_remaining = self.waypointTime - time.time() + if time.time() < self.waypoint_start_time and len(self.waypoints) == 0: + time_remaining = self.waypoint_start_time - time.time() pathingDebug = PathingDebug() pathingDebug.waypoints = [] pathingDebug.time_until_use_waypoints = time_remaining self.debugPublisher.publish(pathingDebug) - if time.time() > self.resetWhen and self.resetWhen != -1 and self.mobility: + if time.time() > self.reset_at and self.reset_at != -1 and self.mobility: self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 255, "#FFFFFF")) - self.resetWhen = -1 + self.reset_at = -1 if len(self.waypoints) > 0: next_waypoint = self.waypoints[0] north_to_gps = (next_waypoint[0] - self.position.latitude) * self.latitudeLength - west_to_gps = (self.position.longitude -next_waypoint[1]) * self.longitudeLength + west_to_gps = (self.position.longitude - next_waypoint[1]) * self.longitudeLength heading_to_gps = math.atan2(west_to_gps, north_to_gps) % (2 * math.pi) if north_to_gps ** 2 + west_to_gps ** 2 <= self.config.waypointPopDistance: self.waypoints.pop(0) self.safetyLightsPublisher.publish(toSafetyLights(True, False, 2, 255, "#00FF00")) - self.resetWhen = time.time() + 1.5 + self.reset_at = time.time() + 1.5 pathingDebug = PathingDebug() pathingDebug.desired_heading = heading_to_gps @@ -298,7 +294,7 @@ def onConfigSpaceReceived(self, msg: OccupancyGrid): if len(self.waypoints) > 0: heading_err_to_gps = abs(self.getAngleDifference(self.position.theta + math.atan2(40 - x, 80 - y), heading_to_gps)) * 180 / math.pi - cost -= max(heading_err_to_gps, 10) + cost -= max(heading_err_to_gps, self.config.waypointMaxWeight) * self.config.waypointWeight if cost > best_pos_cost: best_pos_cost = cost @@ -318,12 +314,12 @@ def onConfigSpaceReceived(self, msg: OccupancyGrid): depth += 1 - self.costMap = grid_data - self.bestPosition = temp_best_pos + self.cost_map = grid_data + self.best_pos = temp_best_pos - def pathToGlobalPose(self, pp0, pp1): - x = (80 - pp1) * VERTICAL_CAMERA_DIST / 80 - y = (40 - pp0) * HORIZONTAL_CAMERA_DIST / 80 + def path_to_global(self, pp0, pp1): + x = (80 - pp1) * self.config.vertical_fov / 80 + y = (40 - pp0) * self.config.horizontal_fov / 80 new_x = x * math.cos(0) + y * math.sin(0) new_y = x * math.sin(0) + y * math.cos(0) diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 828fc4dd..09de5c35 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -3,6 +3,7 @@ import json from types import SimpleNamespace from autonav_msgs.msg import MotorInput, Position, SafetyLights +import rclpy.qos from scr_msgs.msg import SystemState from scr.node import Node from scr.states import DeviceStateEnum, SystemStateEnum @@ -38,13 +39,13 @@ def toSafetyLights(autonomous: bool, eco: bool, mode: int, brightness: int, colo class PathResolverNodeConfig: def __init__(self): - self.forward_speed = 2.1 + self.forward_speed = 1.5 self.reverse_speed = -0.4 self.radius_multiplier = 1.2 self.radius_max = 4.0 self.radius_start = 0.7 - self.angular_aggression = 2.2 - self.max_angular_speed = 0.5 + self.angular_aggression = 1.8 + self.max_angular_speed = 0.8 class PathResolverNode(Node): @@ -56,13 +57,13 @@ def init(self): self.pure_pursuit = PurePursuit() self.backCount = -1 self.status = -1 - self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, 20) - self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, 20) - self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 20) - self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 20) + self.path_subscriber = self.create_subscription(Path, "/autonav/path", self.on_path_received, 1) + self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.on_position_received, 1) + self.motor_publisher = self.create_publisher(MotorInput, "/autonav/MotorInput", 1) + self.safety_lights_publisher = self.create_publisher(SafetyLights, "/autonav/SafetyLights", 1) self.config = self.get_default_config() - self.create_timer(0.05, self.onResolve) + self.create_timer(0.05, self.resolve) self.set_device_state(DeviceStateEnum.READY) def config_updated(self, jsonObject): @@ -107,7 +108,7 @@ def on_path_received(self, msg: Path): self.points = [x.pose.position for x in msg.poses] self.pure_pursuit.set_points([(point.x, point.y)for point in self.points]) - def onResolve(self): + def resolve(self): if self.position is None or self.device_state != DeviceStateEnum.OPERATING or self.system_state != SystemStateEnum.AUTONOMOUS or not self.mobility: return diff --git a/autonav_ws/src/autonav_playback/CMakeLists.txt b/autonav_ws/src/autonav_playback/CMakeLists.txt new file mode 100644 index 00000000..1c654a44 --- /dev/null +++ b/autonav_ws/src/autonav_playback/CMakeLists.txt @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 3.8) +project(autonav_playback) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_python REQUIRED) + +# Install Python modules +ament_python_install_package(${PROJECT_NAME}) + +# Install Python executables +install(PROGRAMS + src/playback.py + DESTINATION lib/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/autonav_playback/__init__.py b/autonav_ws/src/autonav_playback/autonav_playback/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/autonav_ws/src/autonav_playback/package.xml b/autonav_ws/src/autonav_playback/package.xml new file mode 100644 index 00000000..d69062ba --- /dev/null +++ b/autonav_ws/src/autonav_playback/package.xml @@ -0,0 +1,17 @@ + + + + autonav_playback + 1.0.0 + Contains nodes relevant to playing back previously recorded runs + Dylan Zemlin + MIT + + ament_cmake_python + ros2launch + + rclpy + + ament_cmake + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_playback/src/playback.py b/autonav_ws/src/autonav_playback/src/playback.py new file mode 100644 index 00000000..7ab8dd21 --- /dev/null +++ b/autonav_ws/src/autonav_playback/src/playback.py @@ -0,0 +1,272 @@ +#!/usr/bin/env python3 + +from types import SimpleNamespace +from autonav_msgs.msg import IMUData, GPSFeedback, MotorFeedback, MotorInput, Position, MotorControllerDebug +from scr.states import DeviceStateEnum, SystemStateEnum +from sensor_msgs.msg import CompressedImage +from scr_msgs.msg import SystemState, DeviceState +from cv_bridge import CvBridge +from scr.node import Node +from datetime import datetime +import shutil +import rclpy +import cv2 +import os +import json +import subprocess + + +class ImageTransformerConfig: + def __init__(self): + self.record_imu = True + self.record_gps = True + self.record_position = True + self.record_feedback = True + self.record_motor_debug = True + self.record_raw_cameras = True + self.record_filtered_cameras = True + self.record_astar = True + self.record_autonomous = True + self.record_manual = True + self.frame_rate = 8 + + +class PlaybackNode(Node): + def __init__(self): + super().__init__("autonav_playback") + + self.startTime = datetime.now().timestamp() + self.file = None + self.file_name = None + self.home_dir = os.path.expanduser("~") + self.camera_index = 0 + self.filtered_index = 0 + self.astar_index = 0 + self.bridge = CvBridge() + self.config = self.get_default_config() + self.image_raw_left = None + self.image_raw_right = None + self.image_filtered_left = None + self.image_filtered_right = None + + def init(self): + self.imu_subscriber = self.create_subscription(IMUData, "/autonav/imu", self.imu_callback, 20) + self.gps_subscriber = self.create_subscription(GPSFeedback, "/autonav/gps", self.gps_callback, 20) + self.feedback_subscriber = self.create_subscription(MotorFeedback, "/autonav/MotorFeedback", self.feedback_callback, 20) + self.position_subscriber = self.create_subscription(Position, "/autonav/position", self.position_callback, 20) + self.controller_debug_subscriber = self.create_subscription(MotorControllerDebug, "/autonav/MotorControllerDebug", self.controller_debug_callback, 20) + self.thresholded_subscriber_left = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left_small", self.filtered_callback_left, self.qos_profile) + self.thresholded_subscriber_right = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right_small", self.filtered_callback_right, self.qos_profile) + self.camera_subscriber_left = self.create_subscription(CompressedImage, "/autonav/camera/compressed/left", self.camera_callback_left, self.qos_profile) + self.camera_subscriber_right = self.create_subscription(CompressedImage, "/autonav/camera/compressed/right", self.camera_callback_right, self.qos_profile) + self.astar_subscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.astar_callback, self.qos_profile) + + self.set_device_state(DeviceStateEnum.OPERATING) + + def config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + def get_default_config(self): + return ImageTransformerConfig() + + def timestamp(self): + return datetime.now().timestamp() + + def create_file(self): + time = datetime.now() + timeFrmt = time.strftime("%Y-%m-%d_%H-%M-%S") + stateFrmt = "autonomous" if self.system_state == SystemStateEnum.AUTONOMOUS else "manual" + return f"{stateFrmt}_{timeFrmt}" + + def create_video(self, folder, name): + IMAGES_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name, "images", folder) + SAVE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + with open(os.devnull, "wb") as devnull: + subprocess.call(["ffmpeg", "-r", f"{self.config.frame_rate}", "-i", f"{IMAGES_PATH}/%d.jpg", "-vcodec", "libx264", "-crf", "18", "-pix_fmt", "yuv420p", "-y", f"{SAVE_PATH}/{name}.mp4"], stdout=devnull, stderr=devnull) + + def create_entry(self): + self.file_name = self.create_file() + self.startTime = datetime.now().timestamp() + + BASE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + os.makedirs(BASE_PATH, exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "thresholded"), exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "astar"), exist_ok=True) + os.makedirs(os.path.join(BASE_PATH, "images", "camera"), exist_ok=True) + + self.file = open(os.path.join(BASE_PATH, "log.csv"), "w") + self.file.write("timestamp, type\n") + + self.filtered_index = 0 + self.astar_index = 0 + self.camera_index = 0 + + self.get_logger().info(f"Recording playback data at {BASE_PATH}") + + def close_entry(self): + if self.file is None: + return + + self.file.close() + self.file = None + + # Zip up the folder at $HOME/.scr/playback/{fileName} and then delete it + BASE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name) + self.create_video("thresholded", "thresholded") + self.create_video("astar", "astar") + self.create_video("camera", "camera") + + # For every type of log in the log file, create a seperate csv file for it + with open(os.path.join(BASE_PATH, "log.csv"), "r") as logFile: + for line in logFile.readlines()[1:]: + logEntry = line.split(", ") + logType = logEntry[1].strip() + logPath = os.path.join(BASE_PATH, f"{logType}.csv") + with open(logPath, "a") as logTypeFile: + logTypeFile.write(line) + + # Delete the images folder + shutil.rmtree(os.path.join(BASE_PATH, "images"), ignore_errors=True) + + shutil.make_archive(BASE_PATH, "zip", BASE_PATH) + SIZE_OF_ZIP = os.path.getsize(BASE_PATH + ".zip") / 1024 / 1024 + TIME_ELAPSED = datetime.now().timestamp() - self.startTime + shutil.rmtree(BASE_PATH, ignore_errors=True) + + self.get_logger().info(f"Finished recording playback data at {BASE_PATH}. Size of zip: {SIZE_OF_ZIP:.2f} MB. Time elapsed: {TIME_ELAPSED:.2f} seconds") + + def write_file(self, msg: str): + if self.file is None: + return + + self.file.write(msg + "\n") + + def write_image(self, img: CompressedImage, relative_path: str, idx: int): + if self.file_name is None: + return + + IMAGE_PATH = os.path.join(self.home_dir, ".scr", "playback", self.file_name, "images", relative_path) + cv2Image = self.bridge.compressed_imgmsg_to_cv2(img, "bgr8") + cv2.imwrite(os.path.join(IMAGE_PATH, f"{idx}.jpg"), cv2Image) + + def system_state_transition(self, old: SystemState, updated: SystemState): + if old.state == SystemStateEnum.AUTONOMOUS and updated.state != SystemStateEnum.AUTONOMOUS: + self.close_entry() + + if old.state == SystemStateEnum.MANUAL and updated.state != SystemStateEnum.MANUAL: + self.close_entry() + + if old.state != SystemStateEnum.AUTONOMOUS and updated.state == SystemStateEnum.AUTONOMOUS and self.config.record_autonomous: + self.create_entry() + + if old.state != SystemStateEnum.MANUAL and updated.state == SystemStateEnum.MANUAL and self.config.record_manual: + self.create_entry() + + def imu_callback(self, msg: IMUData): + if not self.config.record_imu: + return + + self.write_file(f"{self.timestamp()}, ENTRY_IMU, {msg.accel_x}, {msg.accel_y}, {msg.accel_z}, {msg.angular_x}, {msg.angular_y}, {msg.angular_z}, {msg.roll}, {msg.pitch}, {msg.yaw}") + + def gps_callback(self, msg: GPSFeedback): + if not self.config.record_gps: + return + + self.write_file(f"{self.timestamp()}, ENTRY_GPS, {msg.latitude}, {msg.longitude}, {msg.altitude}, {msg.gps_fix}, {msg.is_locked}, {msg.satellites}") + + def feedback_callback(self, msg: MotorFeedback): + if not self.config.record_feedback: + return + + self.write_file(f"{self.timestamp()}, ENTRY_FEEDBACK, {msg.delta_x}, {msg.delta_y}, {msg.delta_theta}") + + def position_callback(self, msg: Position): + if not self.config.record_position: + return + + self.write_file(f"{self.timestamp()}, ENTRY_POSITION, {msg.x}, {msg.y}, {msg.theta}, {msg.latitude}, {msg.longitude}") + + def controller_debug_callback(self, msg: MotorControllerDebug): + if not self.config.record_motor_debug: + return + + self.write_file(f"{self.timestamp()}, ENTRY_MOTORDEBUG, {msg.current_forward_velocity}, {msg.forward_velocity_setpoint}, {msg.current_angular_velocity}, {msg.angular_velocity_setpoint}, {msg.left_motor_output}, {msg.right_motor_output}") + + def filtered_callback_left(self, msg: CompressedImage): + if not self.config.record_filtered_cameras: + return + + self.image_filtered_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + + def filtered_callback_right(self, msg: CompressedImage): + if not self.config.record_filtered_cameras: + return + + self.image_filtered_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + + def filtered_callback_combined(self): + if not self.config.record_filtered_cameras or self.image_filtered_left is None or self.image_filtered_right is None: + return + + # Scale the images to the 800 x 800 + self.image_filtered_left = cv2.resize(self.image_filtered_left, (800, 800)) + self.image_filtered_right = cv2.resize(self.image_filtered_right, (800, 800)) + + img = cv2.hconcat([self.image_filtered_left, self.image_filtered_right]) + msg = self.bridge.cv2_to_compressed_imgmsg(img) + self.write_file(f"{self.timestamp()}, ENTRY_FILTERED_IMAGE, /images/thresholded/{self.filtered_index}.jpg") + self.write_image(msg, "thresholded", self.filtered_index) + self.filtered_index += 1 + self.image_filtered_left = None + self.image_filtered_right = None + + def astar_callback(self, msg: CompressedImage): + if not self.config.record_astar or self.image_filtered_left is None or self.image_filtered_right is None or self.image_raw_left is None or self.image_raw_right is None: + return + + # Scale the images to the 800 x 800 + img = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + img = cv2.resize(img, (800, 800)) + msg = self.bridge.cv2_to_compressed_imgmsg(img) + + self.write_file(f"{self.timestamp()}, ENTRY_ASTAR_IMAGE, /images/expandified/{self.astar_index}.jpg") + self.write_image(msg, "astar", self.astar_index) + self.astar_index += 1 + + self.camera_callback_combined() + self.filtered_callback_combined() + + def camera_callback_left(self, msg: CompressedImage): + if not self.config.record_raw_cameras: + return + + self.image_raw_left = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + + def camera_callback_right(self, msg: CompressedImage): + if not self.config.record_raw_cameras: + return + + self.image_raw_right = self.bridge.compressed_imgmsg_to_cv2(msg, "bgr8") + + def camera_callback_combined(self): + if not self.config.record_raw_cameras or self.image_raw_left is None or self.image_raw_right is None: + return + + img = cv2.hconcat([self.image_raw_left, self.image_raw_right]) + msg = self.bridge.cv2_to_compressed_imgmsg(img) # Bad but works + self.write_file(f"{self.timestamp()}, ENTRY_CAMERA_IMAGE, /images/camera/{self.camera_index}.jpg") + self.write_image(msg, "camera", self.camera_index) + self.camera_index += 1 + self.image_raw_left = None + self.image_raw_right = None + + +def main(): + rclpy.init() + node = PlaybackNode() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 7c2573a3..cacfd10c 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -18,38 +18,52 @@ class CameraNodeConfig: def __init__(self): self.refresh_rate = 8 - self.output_width = 640 - self.output_height = 480 - self.camera_index = 0 + self.output_width = 480 + self.output_height = 640 self.scan_rate = 1.0 + self.flip_horizontal = False + self.flip_vertical = False + self.rotate_clockwise = False class CameraNode(Node): - def __init__(self): - super().__init__("autonav_serial_camera") - self.camera_publisher = self.create_publisher(CompressedImage, "/autonav/camera/compressed", 20) - self.camera_thread = threading.Thread(target=self.camera_worker) - self.camera_thread.daemon = True + def __init__(self, side, udev_path): + super().__init__("autonav_serial_camera_" + side) + self.camera_publisher = self.create_publisher(CompressedImage, "/autonav/camera/compressed/" + side, self.qos_profile) + self.camera_thread = None + self.camera_kill = False + self.camera_side = side + self.camera_path = udev_path def init(self): - self.get_logger().info("Initializing camera node...") + self.create_thread() + + def create_thread(self): + if self.camera_thread is not None: + self.camera_kill = True + self.camera_thread.join() + self.camera_thread = None + + self.camera_kill = False + self.camera_thread = threading.Thread(target=self.camera_worker) self.camera_thread.start() def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + self.create_thread() def get_default_config(self): return CameraNodeConfig() def camera_worker(self): capture = None - while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN: + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and not self.camera_kill: try: - if not os.path.exists("/dev/video" + str(self.config.camera_index)): + if not os.path.exists(self.camera_path): time.sleep(self.config.scan_rate) continue - capture = cv2.VideoCapture(0) + capture = cv2.VideoCapture(self.camera_path) if capture is None or not capture.isOpened(): time.sleep(self.config.scan_rate) continue @@ -62,12 +76,25 @@ def camera_worker(self): time.sleep(self.config.scan_rate) continue - while rclpy.ok() and self.getSystemState().state != SystemStateEnum.SHUTDOWN: + while rclpy.ok() and self.system_state != SystemStateEnum.SHUTDOWN and not self.camera_kill: if self.device_state != DeviceStateEnum.OPERATING: continue try: ret, frame = capture.read() + + if self.config.rotate_clockwise: + frame = cv2.rotate(frame, cv2.ROTATE_90_CLOCKWISE) + else: + frame = cv2.rotate(frame, cv2.ROTATE_90_COUNTERCLOCKWISE) + + frame = cv2.resize(frame, (self.config.output_width, self.config.output_height)) + + if self.config.flip_horizontal: + frame = cv2.flip(frame, 1) + + if self.config.flip_vertical: + frame = cv2.flip(frame, 0) except: if capture is not None: capture.release() @@ -78,17 +105,18 @@ def camera_worker(self): if not ret or frame is None: continue - + self.camera_publisher.publish(bridge.cv2_to_compressed_imgmsg(frame)) time.sleep(1.0 / self.config.refresh_rate) def main(): rclpy.init() - node = CameraNode() - Node.run_node(node) + node_left = CameraNode("left", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_8174526F-video-index0") + node_right = CameraNode("right", "/dev/v4l/by-id/usb-046d_HD_Pro_Webcam_C920_3F47331F-video-index0") + Node.run_nodes([node_left, node_right]) rclpy.shutdown() if __name__ == "__main__": - main() + main() \ No newline at end of file diff --git a/autonav_ws/src/autonav_serial/src/safety_lights.py b/autonav_ws/src/autonav_serial/src/safety_lights.py index 4bde4d25..a7ec2717 100644 --- a/autonav_ws/src/autonav_serial/src/safety_lights.py +++ b/autonav_ws/src/autonav_serial/src/safety_lights.py @@ -20,7 +20,8 @@ class SafetyLightsPacket(Structure): ("brightness", c_uint8, 8), ("red", c_uint8, 8), ("green", c_uint8, 8), - ("blue", c_uint8, 8) + ("blue", c_uint8, 8), + ("blink_period", c_uint8, 8) ] @@ -70,6 +71,8 @@ def onSafetyLightsReceived(self, lights: SafetyLights): data["red"] = lights.red data["green"] = lights.green data["blue"] = lights.blue + data["blink_period"] = 500 / 10 + self.writeQueueLock.acquire() self.writeQueue.append(data) self.writeQueueLock.release() diff --git a/autonav_ws/src/autonav_serial/src/serial_node.py b/autonav_ws/src/autonav_serial/src/serial_node.py index 56a4325d..59d03808 100644 --- a/autonav_ws/src/autonav_serial/src/serial_node.py +++ b/autonav_ws/src/autonav_serial/src/serial_node.py @@ -2,10 +2,11 @@ from ctypes import Structure, c_bool, c_uint8 import rclpy +import time import can import threading import struct -from autonav_msgs.msg import MotorInput, MotorFeedback, ObjectDetection, MotorControllerDebug, SafetyLights, Conbus +from autonav_msgs.msg import MotorInput, MotorFeedback, MotorControllerDebug, SafetyLights, Conbus from scr.node import Node from scr.states import DeviceStateEnum @@ -15,7 +16,6 @@ MOBILITY_STOP_ID = 1 MOBILITY_START_ID = 9 MOTOR_FEEDBACK_ID = 14 -OBJECT_DETECTION = 20 SAFETY_LIGHTS_ID = 13 CAN_50 = 50 @@ -30,7 +30,8 @@ class SafetyLightsPacket(Structure): ("brightness", c_uint8, 8), ("red", c_uint8, 8), ("green", c_uint8, 8), - ("blue", c_uint8, 8) + ("blue", c_uint8, 8), + ("blink_period", c_uint8, 8) ] @@ -49,7 +50,6 @@ def __init__(self): self.safetyLightsSubscriber = self.create_subscription(SafetyLights, "/autonav/SafetyLights", self.onSafetyLightsReceived, 20) self.motorInputSubscriber = self.create_subscription(MotorInput, "/autonav/MotorInput", self.onMotorInputReceived, 20) self.motorDebugPublisher = self.create_publisher(MotorControllerDebug, "/autonav/MotorControllerDebug", 20) - self.objectDetectionPublisher = self.create_publisher(ObjectDetection, "/autonav/ObjectDetection", 20) self.motorFeedbackPublisher = self.create_publisher(MotorFeedback, "/autonav/MotorFeedback", 20) self.conbuSubscriber = self.create_subscription(Conbus, "/autonav/conbus/instruction", self.onConbusReceived, 20) self.conbusPublisher = self.create_publisher(Conbus, "/autonav/conbus/data", 20) @@ -72,6 +72,9 @@ def canThreadWorker(self): except can.CanError: pass + def getClockMs(self): + return time.time() * 1000.0 + def onCanMessageReceived(self, msg): arb_id = msg.arbitration_id if arb_id == MOTOR_FEEDBACK_ID: @@ -115,15 +118,6 @@ def onCanMessageReceived(self, msg): pkg.timestamp = self.getClockMs() * 1.0 self.motorDebugPublisher.publish(pkg) - if arb_id == OBJECT_DETECTION: - # Load in 4 bytes - zero, left, middle, right = struct.unpack("BBBB", msg.data) - pkg = ObjectDetection() - pkg.sensor_1 = left - pkg.sensor_2 = middle - pkg.sensor_3 = right - self.objectDetectionPublisher.publish(pkg) - if arb_id >= 1000 and arb_id < 1400: # self.log(f"[CAN -> {arb_id}] Received ConBus message") pkg = Conbus() @@ -156,11 +150,12 @@ def onSafetyLightsReceived(self, lights: SafetyLights): packed_data = SafetyLightsPacket() packed_data.autonomous = lights.autonomous packed_data.eco = False - packed_data.mode = lights.mode + packed_data.mode = 0 packed_data.brightness = lights.brightness packed_data.red = lights.red packed_data.green = lights.green packed_data.blue = lights.blue + packed_data.blink_period = 500 can_msg = can.Message( arbitration_id=SAFETY_LIGHTS_ID, data=bytes(packed_data)) try: diff --git a/autonav_ws/src/autonav_vision/CMakeLists.txt b/autonav_ws/src/autonav_vision/CMakeLists.txt index 205352b9..1bf2cd6d 100644 --- a/autonav_ws/src/autonav_vision/CMakeLists.txt +++ b/autonav_ws/src/autonav_vision/CMakeLists.txt @@ -21,7 +21,6 @@ find_package(image_transport REQUIRED) # Install Python programs install(PROGRAMS - src/circumscriber.py src/transformations.py src/combination.py DESTINATION lib/${PROJECT_NAME} diff --git a/autonav_ws/src/autonav_vision/src/circumscriber.py b/autonav_ws/src/autonav_vision/src/circumscriber.py deleted file mode 100644 index 4cb6ba10..00000000 --- a/autonav_ws/src/autonav_vision/src/circumscriber.py +++ /dev/null @@ -1,202 +0,0 @@ -#!/usr/bin/env python3 - -import numpy as np -import cv2 as cv -from enum import IntEnum -import rclpy -from rclpy.node import Node -from cv_bridge import CvBridge -import time -import math - -from sensor_msgs.msg import CompressedImage -from autonav_msgs.msg import Obstacle -from autonav_msgs.msg import Obstacles - -bridge = CvBridge() - -class Register(IntEnum): - LOWER_HUE = 0 - LOWER_SATURATION = 1 - LOWER_VALUE = 2 - UPPER_HUE = 3 - UPPER_SATURATION = 4 - UPPER_VALUE = 5 - BLUR = 6 - BLUR_ITERATIONS = 7 - APPLY_FLATTENING = 8 - APPLY_REGION_OF_INTEREST = 9 - -class Circumscriber(Node): - - def __init__(self): - super().__init__("circumscriber") - self.subscriber = self.create_subscription(CompressedImage, "/igvc/camera/compressed", self.on_image_received, 10) - self.publisher = self.create_publisher(CompressedImage, "/autonav/camera/filtered", 1) - self.obstacle_publisher = self.create_publisher(Obstacles, "/autonav/obstacles", 1) - self.h = None - self.w = None - - - def publish_obstacles(self, local_obstacle_list): - msg = Obstacles() - for local_obstacles in local_obstacle_list: - obstacle = Obstacle() - if local_obstacles[1] >= 240: - obstacle.center_x, obstacle.center_y, obstacle.radius = local_obstacles[0], local_obstacles[1] - 2 * (local_obstacles[1] - 240), local_obstacles[2] - else: - obstacle.center_x, obstacle.center_y, obstacle.radius = local_obstacles[0], local_obstacles[1] + 2 * (240 - local_obstacles[1]), local_obstacles[2] - - msg.obstacles_data.append(obstacle) - - self.obstacle_publisher.publish(msg) - self.get_logger().info(f"publishing {msg} as Obstacles to /autonav/obstacles") - - def on_image_received(self, image: CompressedImage): - - self.get_logger().info("Image received") - # Decompress - cv_image = bridge.compressed_imgmsg_to_cv2(image) - - # Blur it up - for _ in range(1): # Register.BLUR_ITERATIONS - cv_image = cv.blur(cv_image, self.get_blur()) - - # Apply filter and return a mask - img = cv.cvtColor(cv_image, cv.COLOR_BGR2HSV) - lower = ( - 0, # LOWER_HUE - 0, # LOWER_SATURATION - 35, #LOWER_VALUE - ) - upper = ( - 255, # UPPER_HUE - 100, # UPPER_SATURATION - 170, # UPPER_VALUE - ) - mask = cv.inRange(img, lower, upper) - mask = 255 - mask - mask[mask < 250] = 0 - - # Apply region of interest and flattening - height = img.shape[0] - width = img.shape[1] - region_of_interest_vertices = [ - (0, height), - (width / 2, height / 2 + 120), - (width, height), - ] - - map_image = mask - if True: # Register.APPLY_FLATTENING - map_image = self.region_of_interest(mask, np.array([region_of_interest_vertices], np.int32)) - - if True: # Register.APPLY_REGION_OF_INTEREST - map_image = self.flatten_image(mask) - - # Convert mask to RGB for preview - preview_image = map_image - cv.imshow("preview_image", preview_image) - cv.waitKey(1000) - - - start = time.time() - ret, thresh = cv.threshold(preview_image, 150, 255, 0) - # split the image into parts - h, w = thresh.shape - self.h, self.w = h, w - print(f"h = {h} w = {w}") - - # define how many sections of the image you want to search for objects in - # grid sizes need to be square numbers - - sections = 4 - grid_sections = sections ** 2 - fractional_w = int(w // sections) - fractional_h = int(h // sections) - - # for each grid section, find the contours and find the circles - # solve the grid problem - contour_collections = [] - for idx in range(grid_sections): - right_displacement = int(idx % sections) - left_displacement = int(sections - right_displacement - 1) - bottom_displacement = int(idx // sections) - top_displacement = int(sections - bottom_displacement - 1) - contours, _ = cv.findContours(thresh[fractional_h * bottom_displacement: h - (fractional_h * top_displacement), fractional_w * right_displacement : w - (fractional_w * left_displacement)], cv.RETR_TREE, cv.CHAIN_APPROX_SIMPLE) - contour_collections.append(contours) - - - obstacles = [] - preview_image = cv.cvtColor(preview_image, cv.COLOR_GRAY2RGB) - # for the contours found, add them to the image - for collections in range(len(contour_collections)): - right_displacement = collections % sections - left_displacement = sections - right_displacement -1 - bottom_displacement = collections // sections - top_displacement = sections - bottom_displacement - 1 - - for cnt in contour_collections[collections]: - [x,y], radius = cv.minEnclosingCircle(cnt) - center = (int(x) + int(fractional_w * right_displacement), int(y) + int(fractional_h * bottom_displacement)) - #center = (int(x), int(y)) - radius = int(radius) - obstacles.append([center[0], center[1], radius]) - preview_image = cv.circle(preview_image, center, radius, (0,0,255), 2) - obstacles.append((center[0], center[1], radius)) - - # for testing whole or partial images - end = time.time() - self.get_logger().info(f"Time to draw circles: {end - start}") - - # display the image - cv.imshow("preview_image after circles", preview_image) - cv.waitKey(10000) - cv.destroyAllWindows() - - # send the obstacles to the path planner - self.publish_obstacles(obstacles) - preview_msg = bridge.cv2_to_compressed_imgmsg(preview_image) - preview_msg.header = image.header - preview_msg.format = "jpeg" - self.publisher.publish(preview_msg) - self.get_logger().info("Publishing an image") - - - def get_blur(self): - blur = 5 # Register.BLUR - blur = max(1, blur) - return (blur, blur) - - def region_of_interest(self, img, vertices): - mask = np.zeros_like(img) * 255 - match_mask_color = 0 - cv.fillPoly(mask, vertices, match_mask_color) - return cv.bitwise_and(img, mask) - - def flatten_image(self, img): - top_left = (int)(img.shape[1] * 0), (int)(img.shape[0] * 1) - top_right = (int)(img.shape[1] * 1), (int)(img.shape[0] * 1) - bottom_left = (int)(img.shape[1] * 0), (int)(img.shape[0] * 0) - bottom_right = (int)(img.shape[1] * 1), (int)(img.shape[0] * 0) - - src = np.float32([top_left, top_right, bottom_left, bottom_right]) - dst = np.float32([[0, img.shape[0]], [img.shape[1], img.shape[0]], [0, 0], [img.shape[1], 0]]) - - M = cv.getPerspectiveTransform(src, dst) - return cv.warpPerspective(img, M, (img.shape[1], img.shape[0])) - - -def main(args=None): - rclpy.init(args=args) - - circumscriber = Circumscriber() - - rclpy.spin(circumscriber) - - circumscriber.destroy_node() - - rclpy.shutdown() - -if __name__ == "__main__": - main() diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 527f256a..22f7db8b 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -7,11 +7,11 @@ import numpy as np from cv_bridge import CvBridge from nav_msgs.msg import MapMetaData, OccupancyGrid +from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import Pose from scr.node import Node from scr.states import DeviceStateEnum -from sensor_msgs.msg import CompressedImage from nav_msgs.msg import OccupancyGrid @@ -29,113 +29,112 @@ IMAGE_HEIGHT = 480 -class ImageCombinerConfig: - def __init__(self): - self.overlap = 0 - self.map_res = 80 - - class ImageCombiner(Node): def __init__(self): - super().__init__("autonav_image_combiner") + super().__init__("autonav_vision_combiner") def init(self): - self.image_left = None - self.image_right = None - self.image_left_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/left", self.image_received_left, 10) - self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image/right", self.image_received_right, 10) - self.combined_image_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/raw", 10) - self.combined_image_publisher_debug = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw/debug", 10) + self.grid_left = None + self.grid_right = None + self.grid_left_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/left", self.grid_received_left, 1) + self.grid_right_subscriber = self.create_subscription(OccupancyGrid, "/autonav/cfg_space/raw/right", self.grid_received_right, 1) + self.combined_grid_publisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/combined", 1) + self.combined_grid_image_publisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/combined/image", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) - def image_received_left(self, msg): - self.image_left = msg - self.try_combine_images() - - def image_received_right(self, msg): - self.image_right = msg - self.try_combine_images() - - def order_points(self, pts): - # initialzie a list of coordinates that will be ordered - # such that the first entry in the list is the top-left, - # the second entry is the top-right, the third is the - # bottom-right, and the fourth is the bottom-left - rect = np.zeros((4, 2), dtype = "float32") - # the top-left point will have the smallest sum, whereas - # the bottom-right point will have the largest sum - s = pts.sum(axis = 1) - rect[0] = pts[np.argmin(s)] - rect[2] = pts[np.argmax(s)] - # now, compute the difference between the points, the - # top-right point will have the smallest difference, - # whereas the bottom-left will have the largest difference - diff = np.diff(pts, axis = 1) - rect[1] = pts[np.argmin(diff)] - rect[3] = pts[np.argmax(diff)] - # return the ordered coordinates - return rect - - - def four_point_transform(self, image, pts): - # obtain a consistent order of the points and unpack them - # individually - rect = self.order_points(pts) - (tl, tr, br, bl) = rect - # compute the width of the new image, which will be the - # maximum distance between bottom-right and bottom-left - # x-coordiates or the top-right and top-left x-coordinates - widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) - widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) - maxWidth = max(int(widthA), int(widthB)) - # compute the height of the new image, which will be the - # maximum distance between the top-right and bottom-right - # y-coordinates or the top-left and bottom-left y-coordinates - heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) - heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) - maxHeight = max(int(heightA), int(heightB)) - # now that we have the dimensions of the new image, construct - # the set of destination points to obtain a "birds eye view", - # (i.e. top-down view) of the image, again specifying points - # in the top-left, top-right, bottom-right, and bottom-left - # order - dst = np.array([ - [0, 0], - [maxWidth - 1, 0], - [maxWidth - 1, maxHeight - 1], - [0, maxHeight - 1]], dtype="float32") - # compute the perspective transform matrix and then apply it - M = cv2.getPerspectiveTransform(rect, dst) - warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) - # return the warped image - return warped - - def try_combine_images(self): - if self.image_left is None or self.image_right is None: + def grid_received_left(self, msg): + self.grid_left = msg + self.try_combine_grids() + + def grid_received_right(self, msg): + self.grid_right = msg + self.try_combine_grids() + + def scale_grid(self, grid): + # Initialize a new grid of the same height but half the width + scaled_grid = OccupancyGrid() + scaled_grid.info = grid.info + scaled_grid.info.width = 40 + scaled_grid.data = [0] * (80 * 80) + + # Copy every second column from the original grid to the new grid + for y in range(80): + for x in range(40): + # Take every second element from the row + scaled_grid.data[y * 40 + x] = grid.data[y * 80 + (2 * x)] + + return scaled_grid + + def try_combine_grids(self): + if self.grid_left is None or self.grid_right is None: return - # Combine the images to form (IMAGE_WIDTH * 2) x IMAGE_HEIGHT - # The left image will be on the left, and the right image will be on the right - cv2img_left = g_bridge.compressed_imgmsg_to_cv2(self.image_left) - cv2img_right = g_bridge.compressed_imgmsg_to_cv2(self.image_right) - combined = cv2.hconcat([cv2img_left, cv2img_right]) - pts = [(147, 287), (851, 232), (955, 678), (0, 678)] - combined = self.four_point_transform(combined, np.array(pts)) - datamap = cv2.resize(combined, dsize=(self.config.map_res, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 - flat = list(datamap.flatten().astype(int)) - msg = OccupancyGrid(info=g_mapData, data=flat) - - self.image_left = None - self.image_right = None - self.combined_image_publisher.publish(msg) - compressed_msg = g_bridge.cv2_to_compressed_imgmsg(combined) - self.combined_image_publisher_debug.publish(compressed_msg) + combined_grid = OccupancyGrid() + combined_grid.header = self.grid_left.header + combined_grid.info = self.grid_left.info + combined_grid.data = [0] * len(self.grid_left.data) + for i in range(len(self.grid_left.data)): + if self.grid_left.data[i] > 0 or self.grid_right.data[i] > 0: + # Need to figure out what the value actually is: 255, 100, 1? + combined_grid.data[i] = self.grid_left.data[i] if self.grid_left.data[i] > self.grid_right.data[i] else self.grid_right.data[i] + else: + combined_grid.data[i] = 0 + + self.grid_left = None + self.grid_right = None + self.combined_grid_publisher.publish(combined_grid) + + # Publish the combined grid as an image + preview_image = np.zeros((80, 80), dtype=np.uint8) + for i in range(80): + for j in range(80): + preview_image[i, j] = 0 if combined_grid.data[i * 80 + j] <= 10 else 255 + preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) + preview_image = cv2.resize(preview_image, (320, 320)) + compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) + self.combined_grid_image_publisher.publish(compressed_image) + + def try_combine_grids_old(self): + if self.grid_left is None or self.grid_right is None: + return + + # Scale down grids + scaled_left = self.scale_grid(self.grid_left) + scaled_right = self.scale_grid(self.grid_right) + + # Create a new 80x80 grid for the combined result + combined_grid = OccupancyGrid() + combined_grid.info = g_mapData + combined_grid.info.width = 80 + combined_grid.info.height = 80 + combined_grid.data = [0] * (80 * 80) + + # Place each grid in the combined grid + for y in range(80): + for x in range(40): # Fill from left scaled grid + combined_grid.data[y * 80 + x] = scaled_left.data[y * 40 + x] + for x in range(40): # Fill from right scaled grid + combined_grid.data[y * 80 + (40 + x)] = scaled_right.data[y * 40 + x] + + # Publish the combined grid + self.grid_left = None + self.grid_right = None + self.combined_grid_publisher.publish(combined_grid) + + # Publish the combined grid as an image + preview_image = np.zeros((80, 80), dtype=np.uint8) + for i in range(80): + for j in range(80): + preview_image[i, j] = 255 - combined_grid.data[i * 80 + j] * 255 / 100 + preview_image = cv2.cvtColor(preview_image, cv2.COLOR_GRAY2RGB) + compressed_image = g_bridge.cv2_to_compressed_imgmsg(preview_image) + self.combined_grid_image_publisher.publish(compressed_image) def config_updated(self, jsonObject): self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) def get_default_config(self): - return ImageCombinerConfig() + return {} def main(): diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 89456876..fec74c39 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -11,8 +11,11 @@ struct ExpandifyConfig float vertical_fov; float horizontal_fov; float map_res; + float max_range; + float no_go_percent; + float no_go_range; - NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExpandifyConfig, vertical_fov, horizontal_fov, map_res) + NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExpandifyConfig, vertical_fov, horizontal_fov, map_res, max_range, no_go_percent, no_go_range) }; struct Circle @@ -38,40 +41,61 @@ class ExpandifyNode : public SCR::Node map.origin.position.x = -10.0; map.origin.position.y = -10.0; + build_circles(); + + raw_map_subscriber = create_subscription("/autonav/cfg_space/combined", 1, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); + expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 1); + debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", qos_profile); + + set_device_state(SCR::DeviceState::OPERATING); + } + + void build_circles() + { + // Reset variables + maxRange = config.max_range; + noGoPercent = config.no_go_percent; + noGoRange = config.no_go_range; + + // Calculate the range of the circles auto tempRange = maxRange * noGoPercent; maxRange = (int)(maxRange / (config.horizontal_fov / config.map_res)); noGoRange = (int)(tempRange / (config.horizontal_fov / config.map_res)); - circles.push_back(Circle{0, 0, 0}); + // Build the circles + std::vector newCircles; + newCircles.push_back(Circle{0, 0, 0}); for (int x = -maxRange; x <= maxRange; x++) { for (int y = -maxRange; y <= maxRange; y++) { + // Check if the point is within the circle if (maxRange * noGoPercent <= sqrt(x * x + y * y) && sqrt(x * x + y * y) < maxRange) { - circles.push_back(Circle{x, y, sqrt(x * x + y * y)}); + // Add the point to the circle + newCircles.push_back(Circle{x, y, sqrt(x * x + y * y)}); } } } - raw_map_subscriber = create_subscription("/autonav/cfg_space/raw", 20, std::bind(&ExpandifyNode::onConfigSpaceReceived, this, std::placeholders::_1)); - expanded_map_publisher = create_publisher("/autonav/cfg_space/expanded", 20); - debug_publisher = create_publisher("/autonav/cfg_space/expanded/image", 20); - - set_device_state(SCR::DeviceState::OPERATING); + circles = newCircles; } void config_updated(json newConfig) override { config = newConfig.template get(); + build_circles(); } json get_default_config() override { ExpandifyConfig newConfig; newConfig.vertical_fov = 2.75; - newConfig.horizontal_fov = 3; + newConfig.horizontal_fov = 3.4; newConfig.map_res = 80.0f; + newConfig.max_range = 1.0; + newConfig.no_go_percent = 0.70; + newConfig.no_go_range = 0; return newConfig; } @@ -151,8 +175,8 @@ class ExpandifyNode : public SCR::Node nav_msgs::msg::MapMetaData map; - float maxRange = 0.10; - float noGoPercent = 0.38; + float maxRange = 0.55; + float noGoPercent = 0.60; int noGoRange = 0; std::vector circles; ExpandifyConfig config; diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 19cf8d77..04c3d250 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -6,9 +6,11 @@ import numpy as np from nav_msgs.msg import MapMetaData, OccupancyGrid +import rclpy.qos from sensor_msgs.msg import CompressedImage from geometry_msgs.msg import Pose from cv_bridge import CvBridge +from scr.states import SystemModeEnum import json from scr.node import Node @@ -27,32 +29,59 @@ class ImageTransformerConfig: def __init__(self): + # HSV self.lower_hue = 0 self.lower_sat = 0 self.lower_val = 0 self.upper_hue = 255 self.upper_sat = 95 self.upper_val = 210 + + # Blur self.blur_weight = 5 self.blur_iterations = 3 - self.rod_offset = 130 self.map_res = 80 - self.image_warp_tl = 0.26 - self.image_warp_tr = 0.26 + + # Perspective transform + self.left_topleft = [80, 220] + self.left_topright = [400, 220] + self.left_bottomright = [480, 640] + self.left_bottomleft = [0, 640] + self.right_topleft = [80, 220] + self.right_topright = [400, 220] + self.right_bottomright = [480, 640] + self.right_bottomleft = [0, 640] + + # Region of disinterest + # Order: top-left, top-right, bottom-right, bottom-left + self.parallelogram_left = [[500, 405], [260, 400], [210, 640], [640, 640]] + self.parallelogram_right = [[0, 415], [195, 390], [260, 640], [0, 640]] + + self.top_width = 320 + self.bottom_width = 240 + self.offset = 20 + + # Disabling + self.disable_blur = False + self.disable_hsv = False + self.disable_region_of_disinterest = False + self.disable_perspective_transform = False class ImageTransformer(Node): def __init__(self, dir = "left"): super().__init__("autonav_vision_transformer") + self.config = self.get_default_config() self.dir = dir def directionify(self, topic): return topic + "/" + self.dir def init(self): - self.cameraSubscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, 1) - self.rawMapPublisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/preraw"), 1) - self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), 1) + self.camera_subscriber = self.create_subscription(CompressedImage, self.directionify("/autonav/camera/compressed") , self.onImageReceived, self.qos_profile) + self.camera_debug_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/camera/compressed") + "/cutout", self.qos_profile) + self.grid_publisher = self.create_publisher(OccupancyGrid, self.directionify("/autonav/cfg_space/raw"), 1) + self.grid_image_publisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image") + "_small", self.qos_profile) self.set_device_state(DeviceStateEnum.OPERATING) @@ -62,70 +91,192 @@ def config_updated(self, jsonObject): def get_default_config(self): return ImageTransformerConfig() - def getBlur(self): + def get_blur_level(self): blur = self.config.blur_weight blur = max(1, blur) return (blur, blur) + def order_points(self, pts): + # initialzie a list of coordinates that will be ordered + # such that the first entry in the list is the top-left, + # the second entry is the top-right, the third is the + # bottom-right, and the fourth is the bottom-left + rect = np.zeros((4, 2), dtype = "float32") + # the top-left point will have the smallest sum, whereas + # the bottom-right point will have the largest sum + s = pts.sum(axis = 1) + rect[0] = pts[np.argmin(s)] + rect[2] = pts[np.argmax(s)] + # now, compute the difference between the points, the + # top-right point will have the smallest difference, + # whereas the bottom-left will have the largest difference + diff = np.diff(pts, axis = 1) + rect[1] = pts[np.argmin(diff)] + rect[3] = pts[np.argmax(diff)] + # return the ordered coordinates + return rect + + def epic_noah_transform(self, image, pts, top_width, bottom_width, height, offset): + + rect = self.order_points(pts) + (tl, tr, br, bl) = rect + # compute the width of the new image, which will be the + # maximum distance between bottom-right and bottom-left + # x-coordiates or the top-right and top-left x-coordinates + widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) + widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) + maxWidth = max(int(widthA), int(widthB)) + # compute the height of the new image, which will be the + # maximum distance between the top-right and bottom-right + # y-coordinates or the top-left and bottom-left y-coordinates + heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) + heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) + maxHeight = max(int(heightA), int(heightB)) + + if self.dir == "left": + dst = np.array([ + [240 - bottom_width - offset, 0], + [240 - offset, 0], + [240 - offset, height - 1], + [240 - top_width - offset, height - 1]], dtype="float32") + else: + dst = np.array([ + [240 + offset, 0], + [240 + bottom_width + offset, 0], + [240 + top_width + offset, height - 1], + [240 + offset, height - 1]], dtype="float32") + # compute the perspective transform matrix and then apply it + M = cv2.getPerspectiveTransform(rect, dst) + warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) + # return the warped image + return warped + + + def four_point_transform(self, image, pts): + # obtain a consistent order of the points and unpack them + # individually + rect = self.order_points(pts) + (tl, tr, br, bl) = rect + # compute the width of the new image, which will be the + # maximum distance between bottom-right and bottom-left + # x-coordiates or the top-right and top-left x-coordinates + widthA = np.sqrt(((br[0] - bl[0]) ** 2) + ((br[1] - bl[1]) ** 2)) + widthB = np.sqrt(((tr[0] - tl[0]) ** 2) + ((tr[1] - tl[1]) ** 2)) + maxWidth = max(int(widthA), int(widthB)) + # compute the height of the new image, which will be the + # maximum distance between the top-right and bottom-right + # y-coordinates or the top-left and bottom-left y-coordinates + heightA = np.sqrt(((tr[0] - br[0]) ** 2) + ((tr[1] - br[1]) ** 2)) + heightB = np.sqrt(((tl[0] - bl[0]) ** 2) + ((tl[1] - bl[1]) ** 2)) + maxHeight = max(int(heightA), int(heightB)) + # now that we have the dimensions of the new image, construct + # the set of destination points to obtain a "birds eye view", + # (i.e. top-down view) of the image, again specifying points + # in the top-left, top-right, bottom-right, and bottom-left + # order + dst = np.array([ + [0, 0], + [maxWidth - 1, 0], + [maxWidth - 1, maxHeight - 1], + [0, maxHeight - 1]], dtype="float32") + # compute the perspective transform matrix and then apply it + M = cv2.getPerspectiveTransform(rect, dst) + warped = cv2.warpPerspective(image, M, (maxWidth, maxHeight)) + # return the warped image + return warped + def regionOfDisinterest(self, img, vertices): mask = np.ones_like(img) * 255 cv2.fillPoly(mask, vertices, 0) masked_image = cv2.bitwise_and(img, mask) return masked_image - def publishOccupancyMap(self, img): + def publish_occupancy_grid(self, img): datamap = cv2.resize(img, dsize=(self.config.map_res, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 flat = list(datamap.flatten().astype(int)) msg = OccupancyGrid(info=g_mapData, data=flat) - self.rawMapPublisher.publish(msg) + self.grid_publisher.publish(msg) - def onImageReceived(self, image = CompressedImage): - # Decompressify - cv_image = g_bridge.compressed_imgmsg_to_cv2(image) + # Publish msg as a 80x80 image + preview_image = cv2.resize(img, dsize=(80, 80), interpolation=cv2.INTER_LINEAR) + preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) + preview_msg.format = "jpeg" + self.grid_image_publisher.publish(preview_msg) + + def publish_debug_image(self, img): + img_copy = img.copy() + + # Draw parallelogram for region of disinterest + parallelogram = self.config.parallelogram_left if self.dir == "left" else self.config.parallelogram_right + cv2.polylines(img_copy, [np.array(parallelogram)], True, (0, 255, 0), 2) + + # Draw perspective transform points + pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] + cv2.polylines(img_copy, [np.array(pts)], True, (0, 0, 255), 2) + + self.camera_debug_publisher.publish(g_bridge.cv2_to_compressed_imgmsg(img_copy)) + + def apply_blur(self, img): + if self.config.disable_blur: + return img - # Blur it up for _ in range(self.config.blur_iterations): - cv_image = cv2.blur(cv_image, self.getBlur()) + img = cv2.blur(img, self.get_blur_level()) - # Apply filter and return a mask - img = cv2.cvtColor(cv_image, cv2.COLOR_BGR2HSV) + return img + + def apply_hsv(self, img): + if self.config.disable_hsv: + return img + + img = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) lower = (self.config.lower_hue, self.config.lower_sat, self.config.lower_val) upper = (self.config.upper_hue, self.config.upper_sat, self.config.upper_val) mask = cv2.inRange(img, lower, upper) mask = 255 - mask + return mask - # Apply region of disinterest and flattening - height = img.shape[0] - width = img.shape[1] - if self.dir == "left": - region_of_disinterest_vertices = [ - (width, height), - (width, height / 1.8), - (0, height) - ] - else: - region_of_disinterest_vertices = [ - (0, height), - (0, height / 1.8), - (width, height) - ] + def apply_region_of_disinterest(self, img): + if self.config.disable_region_of_disinterest: + return img - # Apply region of disinterest and flattening - mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) + mask = np.ones_like(img) * 255 + parallelogram = self.config.parallelogram_left if self.dir == "left" else self.config.parallelogram_right + cv2.fillPoly(mask, [np.array(parallelogram)], 0) + mask = cv2.bitwise_and(img, mask) mask[mask < 250] = 0 + return mask - # Actually generate the map - self.publishOccupancyMap(mask) + def apply_perspective_transform(self, img): + if self.config.disable_perspective_transform: + return img - # Preview the image - preview_image = cv2.cvtColor(mask, cv2.COLOR_GRAY2RGB) - # cv2.polylines(preview_image, np.array([region_of_disinterest_vertices], np.int32), True, (0, 255, 0), 2) - preview_msg = g_bridge.cv2_to_compressed_imgmsg(preview_image) - preview_msg.header = image.header - preview_msg.format = "jpeg" + pts = [self.config.left_topleft, self.config.left_topright, self.config.left_bottomright, self.config.left_bottomleft] if self.dir == "left" else [self.config.right_topleft, self.config.right_topright, self.config.right_bottomright, self.config.right_bottomleft] + # mask = self.four_point_transform(img, np.array(pts)) + mask = self.epic_noah_transform(img, np.array(pts), self.config.top_width, self.config.bottom_width, 640, self.config.offset) + return mask - # Publish the preview - self.filteredImagePublisher.publish(preview_msg) + def onImageReceived(self, image: CompressedImage): + # Decompress image + img = g_bridge.compressed_imgmsg_to_cv2(image) + + # Publish debug image + self.publish_debug_image(img) + + # Blur it up + img = self.apply_blur(img) + + # Apply filter and return a mask + img = self.apply_hsv(img) + + # Apply region of disinterest and flattening + img = self.apply_region_of_disinterest(img) + + # Apply perspective transform + img = self.apply_perspective_transform(img) + + # Actually generate the map + self.publish_occupancy_grid(img) def main(): diff --git a/autonav_ws/src/scr/include/scr/constants.hpp b/autonav_ws/src/scr/include/scr/constants.hpp index 34f17769..28cf15ec 100644 --- a/autonav_ws/src/scr/include/scr/constants.hpp +++ b/autonav_ws/src/scr/include/scr/constants.hpp @@ -19,6 +19,10 @@ namespace SCR const std::string SYSTEM_STATE = "/scr/system_state_client"; const std::string DEVICE_STATE = "/scr/device_state_client"; const std::string CONFIG_UPDATE = "/scr/update_config_client"; + const std::string SET_ACTIVE_PRESET = "/scr/set_active_preset"; + const std::string SAVE_ACTIVE_PRESET = "/scr/save_active_preset"; + const std::string GET_PRESETS = "/scr/get_presets"; + const std::string DELETE_PRESET = "/scr/delete_preset"; } } diff --git a/autonav_ws/src/scr/include/scr/node.hpp b/autonav_ws/src/scr/include/scr/node.hpp index a041e2d9..c85bb9ea 100644 --- a/autonav_ws/src/scr/include/scr/node.hpp +++ b/autonav_ws/src/scr/include/scr/node.hpp @@ -105,6 +105,10 @@ namespace SCR /// @param data The message to log void log(std::string data); + /// @brief Logs a message to the console + /// @param message The message to log + void log_debug(std::string message); + private: void system_state_callback(const scr_msgs::msg::SystemState msg); void device_state_callback(const scr_msgs::msg::DeviceState msg); @@ -133,6 +137,9 @@ namespace SCR /// @brief The current map of device configurations std::map device_states; + /// @brief The current qos profile + rclcpp::QoS qos_profile = rclcpp::QoS(rclcpp::KeepLast(10)); + private: NodeSubscriptions subscriptions; NodePublishers publishers; diff --git a/autonav_ws/src/scr/scr/constants.py b/autonav_ws/src/scr/scr/constants.py index ab66edaf..a20d3a2a 100644 --- a/autonav_ws/src/scr/scr/constants.py +++ b/autonav_ws/src/scr/scr/constants.py @@ -8,4 +8,8 @@ class Topics: class Services: SYSTEM_STATE = "/scr/system_state_client" DEVICE_STATE = "/scr/device_state_client" - CONFIG_UPDATE = "/scr/update_config_client" \ No newline at end of file + CONFIG_UPDATE = "/scr/update_config_client" + SET_ACTIVE_PRESET = "/scr/set_active_preset" + SAVE_ACTIVE_PRESET = "/scr/save_active_preset" + GET_PRESETS = "/scr/get_presets" + DELETE_PRESET = "/scr/delete_preset" \ No newline at end of file diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 71879696..c12c870c 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -5,11 +5,14 @@ from std_msgs.msg import Float64 from rclpy.node import Node as ROSNode from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSHistoryPolicy import scr.constants import time import rclpy from rclpy.executors import MultiThreadedExecutor import json +import os +import signal class Node(ROSNode): @@ -20,6 +23,11 @@ class Node(ROSNode): def __init__(self, node_name): super().__init__(node_name) self.identifier = node_name + self.qos_profile = QoSProfile( + reliability=QoSReliabilityPolicy.BEST_EFFORT, + history=QoSHistoryPolicy.KEEP_LAST, + depth=1 + ) # Create the callback groups self.device_state_callback_group = MutuallyExclusiveCallbackGroup() @@ -30,11 +38,11 @@ def __init__(self, node_name): self.device_state_subscriber = self.create_subscription(DeviceState, scr.constants.Topics.DEVICE_STATE, self.on_device_state, 10) self.system_state_subscriber = self.create_subscription(SystemState, scr.constants.Topics.SYSTEM_STATE, self.on_system_state, 10) self.config_updated_subscriber = self.create_subscription(ConfigUpdated, scr.constants.Topics.CONFIG_UPDATE, self.on_config_updated, 10) - + self.device_state_client = self.create_client(UpdateDeviceState, scr.constants.Services.DEVICE_STATE, callback_group=self.device_state_callback_group) self.system_state_client = self.create_client(UpdateSystemState, scr.constants.Services.SYSTEM_STATE, callback_group=self.system_state_callback_group) self.config_updated_client = self.create_client(UpdateConfig, scr.constants.Services.CONFIG_UPDATE, callback_group=self.config_updated_callback_group) - + self.performance_publisher = self.create_publisher(Float64, scr.constants.Topics.PERFORMANCE_TRACK, 10) self.logging_publisher = self.create_publisher(Log, scr.constants.Topics.LOGGING, 10) @@ -61,7 +69,7 @@ def jdump(self, obj): return json.dumps(obj.__dict__) else: return json.dumps(obj) - + def log(self, data): """ Logs a message to the logging topic. @@ -74,6 +82,9 @@ def log(self, data): log_packet.node = self.identifier self.logging_publisher.publish(log_packet) + def log_debug(self, message): + self.get_logger().log(message) + def on_device_state(self, msg: DeviceState): """ Called when a device state message is received. @@ -97,7 +108,7 @@ def on_device_state(self, msg: DeviceState): if not rclpy.ok(): self.get_logger().error("Interrupted while waiting for service") return - + try: result = self.config_updated_client.call(request) if not result.success: @@ -117,22 +128,27 @@ def on_system_state(self, msg: SystemState): :param msg: The system state message. """ + # If the system state is shutdown, kill this node killing the proces + if msg.state == SystemStateEnum.SHUTDOWN: + self.get_logger().info("Received shutdown signal, shutting down") + os.kill(os.getpid(), signal.SIGKILL) + return + oldState = SystemState() oldState.state = self.system_state oldState.mode = self.system_mode oldState.mobility = self.mobility - - self.system_state_transition(oldState, msg) - self.system_state = msg.state self.system_mode = msg.mode self.mobility = msg.mobility + self.system_state_transition(oldState, msg) + def on_config_updated(self, msg: ConfigUpdated): self.node_configs[msg.device] = json.loads(msg.json) if msg.device is None or msg.device != self.identifier: return - + try: parsed_json = json.loads(msg.json) self.config_updated(parsed_json) @@ -246,7 +262,7 @@ def set_system_mobility(self, mobility: bool): self.set_system_total_state( self.system_state, self.system_mode, mobility) - + def perf_start(self, name: str): """ Starts a performance measurement. @@ -295,4 +311,4 @@ def run_nodes(nodes): executor.add_node(node) executor.spin() for node in nodes: - executor.remove_node(node) \ No newline at end of file + executor.remove_node(node) diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index cd033467..73304b56 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -26,6 +26,9 @@ namespace SCR clients.device_state = this->create_client(Constants::Services::DEVICE_STATE, rmw_qos_profile_services_default, callback_groups.device_state); clients.config_update = this->create_client(Constants::Services::CONFIG_UPDATE, rmw_qos_profile_services_default, callback_groups.config_updated); + rmw_qos_profile_t q = rmw_qos_profile_sensor_data; + qos_profile = rclcpp::QoS(rclcpp::QoSInitialization(q.history, 1), q); + // Create a thread to wait a sec for the node to boot without blocking the main thread std::thread booting_thread([this]() { @@ -53,23 +56,23 @@ namespace SCR oldState.mobility = mobility; oldState.mode = system_mode; + // If the new system state is shutdown, just exit the process + if (msg.state == static_cast(SCR::SystemState::SHUTDOWN)) + { + kill(getpid(), SIGKILL); + } + SCR::SystemState newStateEnum = static_cast(msg.state); SCR::SystemState oldStateEnum = static_cast(oldState.state); SCR::SystemMode newMode = static_cast(msg.mode); SCR::SystemMode oldMode = static_cast(oldState.mode); - system_state_transition(oldState, msg); - system_mode = static_cast(msg.mode); mobility = msg.mobility; system_state = static_cast(msg.state); - if (system_state == SCR::SystemState::SHUTDOWN) - { - // Just exit this process and die - exit(0); - } + system_state_transition(oldState, msg); } void Node::device_state_callback(const scr_msgs::msg::DeviceState msg) @@ -276,4 +279,8 @@ namespace SCR rclcpp::shutdown(); } + void Node::log_debug(std::string message) + { + RCLCPP_INFO(this->get_logger(), "%s", message.c_str()); + } } \ No newline at end of file diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index dbc343d7..260e9e9d 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -8,7 +8,15 @@ #include "scr/structs.hpp" #include "scr_msgs/srv/update_device_state.hpp" #include "scr_msgs/srv/update_config.hpp" +#include "scr_msgs/srv/set_active_preset.hpp" +#include "scr_msgs/srv/save_active_preset.hpp" +#include "scr_msgs/srv/get_presets.hpp" +#include "scr_msgs/srv/delete_preset.hpp" #include +#include +#include +#include +#include #include "scr/json.hpp" using json = nlohmann::json; @@ -25,6 +33,10 @@ struct services rclcpp::Service::SharedPtr system_state; rclcpp::Service::SharedPtr device_state; rclcpp::Service::SharedPtr config_update; + rclcpp::Service::SharedPtr set_active_preset; + rclcpp::Service::SharedPtr save_active_preset; + rclcpp::Service::SharedPtr get_presets; + rclcpp::Service::SharedPtr delete_preset; }; class CoreNode : public rclcpp::Node @@ -39,17 +51,45 @@ class CoreNode : public rclcpp::Node services.system_state = this->create_service(SCR::Constants::Services::SYSTEM_STATE, std::bind(&CoreNode::on_system_state_called, this, std::placeholders::_1, std::placeholders::_2)); services.device_state = this->create_service(SCR::Constants::Services::DEVICE_STATE, std::bind(&CoreNode::on_device_state_called, this, std::placeholders::_1, std::placeholders::_2)); services.config_update = this->create_service(SCR::Constants::Services::CONFIG_UPDATE, std::bind(&CoreNode::on_config_update_called, this, std::placeholders::_1, std::placeholders::_2)); + services.set_active_preset = this->create_service(SCR::Constants::Services::SET_ACTIVE_PRESET, std::bind(&CoreNode::on_set_active_preset_called, this, std::placeholders::_1, std::placeholders::_2)); + services.save_active_preset = this->create_service(SCR::Constants::Services::SAVE_ACTIVE_PRESET, std::bind(&CoreNode::on_save_active_preset_called, this, std::placeholders::_1, std::placeholders::_2)); + services.get_presets = this->create_service(SCR::Constants::Services::GET_PRESETS, std::bind(&CoreNode::on_get_presets_called, this, std::placeholders::_1, std::placeholders::_2)); + services.delete_preset = this->create_service(SCR::Constants::Services::DELETE_PRESET, std::bind(&CoreNode::on_delete_preset_called, this, std::placeholders::_1, std::placeholders::_2)); // Load the initial system state from the parameters mode = static_cast(this->declare_parameter("mode", static_cast(SCR::SystemMode::COMPETITION))); state = static_cast(this->declare_parameter("state", static_cast(SCR::SystemState::DISABLED))); mobility = this->declare_parameter("mobility", false); + + // Set mode timer + mode_timer = this->create_wall_timer(std::chrono::seconds(2), std::bind(&CoreNode::mode_timer_callback, this)); } private: + std::string get_preset_by_name(std::string name) + { + // Look at $HOME/.config/autonav/{name}.preset + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + if (!std::filesystem::exists(path)) + { + return ""; + } + + std::ifstream file(path); + std::string preset((std::istreambuf_iterator(file)), std::istreambuf_iterator()); + return preset; + } + + std::string get_preset_for_mode() + { + std::string modeStr = SCR::systemModeToString(mode); + return get_preset_by_name(modeStr); + } + /// @brief Callback for when the system state service is called. This is used to set the system state. - /// @param request - /// @param response + /// @param request + /// @param response void on_system_state_called(const std::shared_ptr request, std::shared_ptr response) { if (request->state < 0 || request->state > (uint8_t)SCR::SystemState::SHUTDOWN) @@ -71,6 +111,12 @@ class CoreNode : public rclcpp::Node mode = (SCR::SystemMode)request->mode; mobility = request->mobility; + if (state == SCR::SystemState::SHUTDOWN) + { + // Start a 5 second timer to kill the node + mode_timer = this->create_wall_timer(std::chrono::seconds(5), std::bind(&CoreNode::kill_self, this)); + } + // Send the response response->success = true; @@ -83,8 +129,8 @@ class CoreNode : public rclcpp::Node } /// @brief Callback for when the device state service is called. This is used to set a specific devices state. - /// @param request - /// @param response + /// @param request + /// @param response void on_device_state_called(const std::shared_ptr request, std::shared_ptr response) { if (request->state < 0 || request->state > (uint8_t)SCR::DeviceState::ERRORED) @@ -121,6 +167,12 @@ class CoreNode : public rclcpp::Node config_updated_message.json = config.second.dump(); publishers.config_updated->publish(config_updated_message); } + + // Reset the mode timer + if (mode_timer_running) + { + mode_timer->reset(); + } } device_states[request->device] = (SCR::DeviceState)request->state; @@ -134,6 +186,16 @@ class CoreNode : public rclcpp::Node publishers.device_state->publish(device_state_message); } + void ensure_directories() + { + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/"; + if (!std::filesystem::exists(path)) + { + std::filesystem::create_directories(path); + } + } + void on_config_update_called(const std::shared_ptr request, std::shared_ptr response) { json config; @@ -151,16 +213,223 @@ class CoreNode : public rclcpp::Node // Store the config configs[request->device] = config; + // Publish the new config + publish_config(request->device, config); + // Send the response response->success = true; + } - // Publish the new config + void on_set_active_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + std::string preset = get_preset_by_name(request->preset); + if (preset.empty()) + { + RCLCPP_ERROR(this->get_logger(), "Failed to load preset %s", request->preset.c_str()); + response->ok = false; + return; + } + + active_preset = json::parse(preset); + active_preset_name = request->preset; + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + RCLCPP_ERROR(this->get_logger(), "Warning when loading preset %s: missing config for device %s", request->preset.c_str(), device.c_str()); + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + publish_config(device, cfg); + } + + response->ok = true; + } + + void on_save_active_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + active_preset = configs; + active_preset_name = request->write_mode ? SCR::systemModeToString(mode) : request->preset_name; + + std::string name = request->write_mode ? SCR::systemModeToString(mode) : request->preset_name; + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + + write_preset(name, active_preset); + response->ok = true; + } + + void on_get_presets_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/"; + + // Get all files that end with .preset + presets.clear(); + for (const auto &entry : std::filesystem::directory_iterator(path)) + { + std::string filename = entry.path().filename().string(); + if (filename.find(".preset") != std::string::npos) + { + presets.push_back(filename.substr(0, filename.find(".preset"))); + } + } + + response->presets = presets; + response->active_preset = active_preset_name; + } + + void mode_timer_callback() + { + ensure_directories(); + + // Stop the mode timer + mode_timer_running = false; + mode_timer->cancel(); + mode_timer = nullptr; + + // Check if there is a preset for the current mode + std::string preset = get_preset_for_mode(); + if (!preset.empty()) + { + auto upcoming_preset = json::parse(preset); + + // Checking for missing devices and keys + for (auto const &[device, cfg] : configs) + { + // Check for missing devices + if (upcoming_preset.find(device) == upcoming_preset.end()) + { + // The upcoming preset is missing a config, so we need to add it + upcoming_preset[device] = cfg; + RCLCPP_WARN(this->get_logger(), "Warning: upcoming preset is missing config for device %s", device.c_str()); + continue; + } + + // Check for missing keys + for (auto const &[key, value] : cfg.items()) + { + if (upcoming_preset[device].find(key) == upcoming_preset[device].end()) + { + // The upcoming preset is missing a key, so we need to add it + upcoming_preset[device][key] = value; + RCLCPP_WARN(this->get_logger(), "Warning: upcoming preset is missing key %s for device %s", key.c_str(), device.c_str()); + } + } + } + + active_preset = upcoming_preset; + active_preset_name = SCR::systemModeToString(mode); + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + publish_config(device, cfg); + } + + write_preset(SCR::systemModeToString(mode), active_preset); + return; + } + + // If there is no preset for the current mode, then we need to create one + active_preset = configs; + active_preset_name = SCR::systemModeToString(mode); + write_preset(active_preset_name, active_preset); + RCLCPP_INFO(this->get_logger(), "Created preset for mode %d", (int)mode); + } + + void on_delete_preset_called(const std::shared_ptr request, std::shared_ptr response) + { + ensure_directories(); + + // Check that the preset exists + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + request->preset + ".preset"; + if (!std::filesystem::exists(path)) + { + response->ok = false; + return; + } + + // Ensure its not the active mode preset + if (request->preset == SCR::systemModeToString(mode)) + { + response->ok = false; + return; + } + + // Delete the preset + std::filesystem::remove(path); + + // Remove it from the list of presets + auto it = std::find(presets.begin(), presets.end(), request->preset); + if (it != presets.end()) + { + presets.erase(it); + } + + // If it was the active preset, change the active preset to the current system mode + if (active_preset_name == request->preset) + { + active_preset = json::parse(get_preset_for_mode()); + active_preset_name = SCR::systemModeToString(mode); + for (auto const &[device, cfg] : active_preset) + { + if (configs.find(device) == configs.end()) + { + continue; + } + + // Update the config + configs[device] = cfg; + + // Publish the new config + publish_config(device, cfg); + } + } + + response->ok = true; + } + + void write_preset(std::string name, json preset) + { + std::string home = std::getenv("HOME"); + std::string path = home + "/.config/autonav/" + name + ".preset"; + + // Write the preset to disk, if it exists then overwrite it + std::string jsonStr = nlohmann::json(preset).dump(); + std::ofstream file(path); + file << jsonStr; + file.close(); + } + + void publish_config(std::string device, nlohmann::json cfg) + { scr_msgs::msg::ConfigUpdated config_updated_message; - config_updated_message.device = request->device; - config_updated_message.json = request->json; + config_updated_message.device = device; + config_updated_message.json = cfg.dump(); publishers.config_updated->publish(config_updated_message); } + void kill_self() + { + kill(getpid(), SIGKILL); + } + private: struct publishers publishers; struct services services; @@ -170,6 +439,11 @@ class CoreNode : public rclcpp::Node bool mobility; std::map device_states; std::map configs; + std::map active_preset; + std::vector presets; + rclcpp::TimerBase::SharedPtr mode_timer; + bool mode_timer_running = true; + std::string active_preset_name; }; int main(int argc, char **argv) diff --git a/autonav_ws/src/scr_msgs/CMakeLists.txt b/autonav_ws/src/scr_msgs/CMakeLists.txt index 2c4a14de..5983d199 100644 --- a/autonav_ws/src/scr_msgs/CMakeLists.txt +++ b/autonav_ws/src/scr_msgs/CMakeLists.txt @@ -27,6 +27,10 @@ set(srv_files "srv/UpdateDeviceState.srv" "srv/UpdateConfig.srv" "srv/UpdateSystemState.srv" + "srv/SaveActivePreset.srv" + "srv/SetActivePreset.srv" + "srv/GetPresets.srv" + "srv/DeletePreset.srv" ) rosidl_generate_interfaces(${PROJECT_NAME} diff --git a/autonav_ws/src/scr_msgs/srv/DeletePreset.srv b/autonav_ws/src/scr_msgs/srv/DeletePreset.srv new file mode 100644 index 00000000..b72a0a4b --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/DeletePreset.srv @@ -0,0 +1,3 @@ +string preset +--- +bool ok \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/GetPresets.srv b/autonav_ws/src/scr_msgs/srv/GetPresets.srv new file mode 100644 index 00000000..a435d7aa --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/GetPresets.srv @@ -0,0 +1,4 @@ +bool empty +--- +string[] presets +string active_preset \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv b/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv new file mode 100644 index 00000000..e55a076a --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/SaveActivePreset.srv @@ -0,0 +1,4 @@ +string preset_name +bool write_mode +--- +bool ok \ No newline at end of file diff --git a/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv b/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv new file mode 100644 index 00000000..b72a0a4b --- /dev/null +++ b/autonav_ws/src/scr_msgs/srv/SetActivePreset.srv @@ -0,0 +1,3 @@ +string preset +--- +bool ok \ No newline at end of file diff --git a/display/index.html b/display/index.html index dc9a5ceb..54ba0f87 100644 --- a/display/index.html +++ b/display/index.html @@ -21,21 +21,21 @@ - - @@ -56,7 +56,6 @@

General

State:
Mode:
Mobility:
-
ESTOP:
@@ -105,6 +104,25 @@
Distance to Waypoint:
Waypoints:
+
+
+

Vision

+
+
+
+ + +
+
+ + +
+
+ + +
+
+

Device States

@@ -117,21 +135,37 @@

Device States

-
-
-

Camera / Filtered

+
+
+

+ Active Preset: None +

+
+ + +
+
+ + +
+
-
- - - - - - +
-
+
-
+
@@ -215,7 +249,7 @@

System

-
diff --git a/display/scripts/globals.js b/display/scripts/globals.js index a5d5cdae..dffd8daa 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -16,7 +16,8 @@ var deviceStates = {}; var logs = []; var iterator = 0; var iterators = []; -var debug = false; +var development_mode = false; +var current_preset = "ERROR_NO_PRESET_AUTODETECTED"; var addressKeys = { "autonav_serial_imu": { @@ -24,12 +25,26 @@ var addressKeys = { "imu_read_rate": "float" }, - "autonav_serial_camera": { - "internal_title": "[Serial] Camera", + "autonav_serial_camera_left": { + "internal_title": "[Serial] Left Camera", "refresh_rate": "int", "output_width": "int", "output_height": "int", - "camera_index": "int" + "scan_rate": "int", + "flip_horizontal": "bool", + "flip_vertical": "bool", + "rotate_clockwise": "bool" + }, + + "autonav_serial_camera_right": { + "internal_title": "[Serial] Right Camera", + "refresh_rate": "int", + "output_width": "int", + "output_height": "int", + "scan_rate": "int", + "flip_horizontal": "bool", + "flip_vertical": "bool", + "rotate_clockwise": "bool" }, "autonav_vision_transformer": { @@ -42,15 +57,34 @@ var addressKeys = { "upper_val": "int", "blur_weight": "int", "blur_iterations": "int", - "rod_offset": "int", - "map_res": "int" + "map_res": "int", + "left_bottomleft": "point.int", + "left_bottomright": "point.int", + "left_topleft": "point.int", + "left_topright": "point.int", + "right_bottomleft": "point.int", + "right_bottomright": "point.int", + "right_topleft": "point.int", + "right_topright": "point.int", + "disable_blur": "bool", + "disable_hsv": "bool", + "disable_perspective_transform": "bool", + "disable_region_of_disinterest": "bool", + "parallelogram_left": "parallelogram.int", + "parallelogram_right": "parallelogram.int", + "top_width": "float", + "bottom_width": "float", + "offset": "float" }, "autonav_vision_expandifier": { "internal_title": "[Vision] Expandifier", - "horizontal_fov": "int", + "horizontal_fov": "float", "map_res": "int", - "vertical_fov": "float" + "vertical_fov": "float", + "max_range": "float", + "no_go_percent": "float", + "no_go_range": "float", }, "autonav_filters": { @@ -58,9 +92,7 @@ var addressKeys = { "filter_type": { 0: "Deadreckoning", 1: "Particle Filter" - }, - "degree_offset": "float", - "seed_heading": "bool", + } }, "autonav_manual_steamcontroller": { @@ -89,8 +121,13 @@ var addressKeys = { 5: "Misc 4", 6: "Misc 5", }, + "calculateWaypointDirection": "bool", "useOnlyWaypoints": "bool", "waypointDelay": "float", + "vertical_fov": "float", + "horizontal_fov": "float", + "waypointMaxWeight": "float", + "waypointWeight": "float", }, "autonav_nav_resolver": { @@ -106,9 +143,23 @@ var addressKeys = { "autonav_image_combiner": { "internal_title": "[Image Combiner]", - "overlap": "int", "map_res": "int" - } + }, + + "autonav_playback": { + "internal_title": "[Playback]", + "record_imu": "bool", + "record_gps": "bool", + "record_position": "bool", + "record_feedback": "bool", + "record_motor_debug": "bool", + "record_raw_cameras": "bool", + "record_filtered_cameras": "bool", + "record_astar": "bool", + "record_autonomous": "bool", + "record_manual": "bool", + "frame_rate": "int" + } } var conbusDevices = { diff --git a/display/scripts/main.js b/display/scripts/main.js index d16495e7..90572df4 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -24,6 +24,7 @@ $(document).ready(function () { send({ op: "broadcast" }); send({ op: "get_nodes" }); + send({ op: "get_presets" }); const waitInterval = setInterval(() => { if (deviceStates["autonav_serial_can"] != 3) { @@ -48,19 +49,47 @@ $(document).ready(function () { $(".connecting").hide(); $("#main").show(); }, 1000); + + setTimeout(() => { + if(websocket.readyState == 1) + { + send({ op: "get_presets" }); + } + }, 3000); }; websocket.onmessage = function (event) { const messages = event.data.split("\n"); - for (const message of messages) - { + for (const message of messages) { const obj = JSON.parse(message); const { op, topic } = obj; - + if (op == "data") { onTopicData(topic, obj); } - + + if (op == "get_presets_callback") { + const presets = obj.presets; + const presetElement = $("#dropdown_elements"); + presetElement.empty(); + for (const preset of presets) { + const dropdownItem = $(`
  • ${preset}
  • `); + presetElement.append(dropdownItem); + + dropdownItem.on("click", function () { + const preset_name = $(this).children().attr("data-value"); + send({ + op: "set_active_preset", + preset: preset_name + }); + send({ op: "get_presets" }); + }); + } + + current_preset = obj.active_preset; + $("#active_preset_value").text(current_preset); + } + if (op == "get_nodes_callback") { console.log(obj); for (let i = 0; i < obj.nodes.length; i++) { @@ -74,8 +103,7 @@ $(document).ready(function () { const statemap = obj.states; if (node in statemap) { - if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") - { + if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") { continue; } @@ -89,11 +117,20 @@ $(document).ready(function () { } } - for (const key in obj.configs) - { + for (const key in obj.configs) { config[key] = obj.configs[key]; } regenerateConfig(); + + // Update system state + let system = obj["system"]; + $("#var_system_state").text(system["state"] == 0 ? "Diabled" : system["state"] == 1 ? "Autonomous" : system["state"] == 2 ? "Manual" : "Shutdown"); + $("#var_system_mode").text(system["mode"] == 0 ? "Competition" : system["mode"] == 1 ? "Simulation" : "Practice"); + $("#var_system_mobility").text(system["mobility"] ? "Enabled" : "Disabled"); + + // Update some buttons + $("#checkbox_system_mobility").prop("checked", system["mobility"]); + $("#input_system_state").val(system["state"]); } } }; @@ -116,7 +153,14 @@ $(document).ready(function () { }; } - createWebsocket(); + if (!development_mode) { + createWebsocket(); + } else { + $("#connecting-state").text("Updating Data"); + $(".connecting-input").hide(); + $(".connecting").hide(); + $("#main").show(); + } var sendQueue = []; @@ -124,7 +168,6 @@ $(document).ready(function () { send({ op: "set_system_state", state: systemState.state, - estop: systemState.estop, mobility: systemState.mobility, }); } @@ -285,22 +328,19 @@ $(document).ready(function () { } if (topic == "/scr/state/system") { - const { state, mode, mobility, estop } = msg; + const { state, mode, mobility } = msg; $("#var_system_state").text(state == 0 ? "Diabled" : state == 1 ? "Autonomous" : state == 2 ? "Manual" : "Shutdown"); $("#var_system_mode").text(mode == 0 ? "Competition" : mode == 1 ? "Simulation" : "Practice"); $("#var_system_mobility").text(mobility ? "Enabled" : "Disabled"); - $("#var_system_estop").text(estop ? "Yes" : "No"); systemState.state = state; systemState.mode = mode; systemState.mobility = mobility; - systemState.estop = estop; $("#input_system_state").val(state); $("#input_system_mode").val(mode); $("#input_system_mobility").prop("checked", mobility); - $("#input_system_estop").prop("checked", estop); return; } @@ -318,7 +358,9 @@ $(document).ready(function () { } if (topic == "/scr/configuration") { - console.log(msg); + const { device, json } = msg; + config[device] = JSON.parse(json); + regenerateConfig(); return; } @@ -416,6 +458,24 @@ $(document).ready(function () { return; } + if (topic == "/autonav/cfg_space/raw/image/left_small") { + const imgElement = document.getElementById("target_filtered_left"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/raw/image/right_small") { + const imgElement = document.getElementById("target_filtered_right"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/combined/image") { + const imgElement = document.getElementById("target_combined"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + if (topic == "/autonav/cfg_space/raw/debug") { const imgElement = document.getElementById("target_camera_raw"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; @@ -423,7 +483,7 @@ $(document).ready(function () { } if (topic == "/autonav/debug/astar/image") { - const imgElement = document.getElementById("target_astar_path"); + const imgElement = document.getElementById("target_expandified"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; return; } @@ -500,6 +560,7 @@ $(document).ready(function () { $(".dropdown-menu a").on("click", function () { const parentDataTarget = $(this).parents(".dropdown").attr("data-target"); + console.log(parentDataTarget); if (parentDataTarget == "system_state") { const id = $(this).attr("data-value"); systemState.state = parseInt(id); @@ -519,9 +580,29 @@ $(document).ready(function () { } }); - $("#checkbox_system_estop").on("change", function () { - systemState.estop = $(this).is(":checked"); - setSystemState(); + $("#save_preset_mode").on("click", function () { + send({ + op: "save_preset_mode" + }); + send({ op: "get_presets" }); + }); + + $("#save_preset_as").on("click", function () { + const preset_name = $("#preset_save_name").val(); + send({ + op: "save_preset_as", + preset: preset_name + }); + send({ op: "get_presets" }); + $("#preset_save_name").val(""); + }); + + $("#delete_preset").on("click", function () { + send({ + op: "delete_preset", + preset: current_preset + }); + send({ op: "get_presets" }); }); $("#checkbox_system_mobility").on("change", function () { @@ -554,40 +635,40 @@ $(document).ready(function () { function generateElementForConfiguration(data, type, device, text) { if (type == "bool") { const checked = data == 1; - + // Create a dropdown const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const select = document.createElement("select"); select.classList.add("form-select"); select.onchange = function () { - config[device][text] = select.value; + config[device][text] = select.value == 1 ? true : false; send({ op: "configuration", device: device, json: config[device], }); } - + const optionTrue = document.createElement("option"); optionTrue.value = 1; optionTrue.innerText = "True"; optionTrue.selected = checked; - + const optionFalse = document.createElement("option"); optionFalse.value = 0; optionFalse.innerText = "False"; optionFalse.selected = !checked; - + select.appendChild(optionTrue); select.appendChild(optionFalse); - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(select); return div; @@ -596,7 +677,7 @@ $(document).ready(function () { const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const input = document.createElement("input"); input.type = "number"; input.classList.add("form-control"); @@ -609,11 +690,11 @@ $(document).ready(function () { json: config[device], }); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(input); return div; @@ -622,7 +703,7 @@ $(document).ready(function () { const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const input = document.createElement("input"); input.type = "number"; input.classList.add("form-control"); @@ -635,26 +716,167 @@ $(document).ready(function () { json: config[device], }); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(input); return div; } + else if (type == "point.int") { + // x, y point for two integers + const div = document.createElement("div"); + div.classList.add("input-group"); + div.classList.add("mb-3"); + + const inputX = document.createElement("input"); + inputX.type = "number"; + inputX.classList.add("form-control"); + inputX.value = data[0]; + inputX.onchange = function () { + config[device][text][0] = parseInt(inputX.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const inputY = document.createElement("input"); + inputY.type = "number"; + inputY.classList.add("form-control"); + inputY.value = data[1]; + inputY.onchange = function () { + config[device][text][1] = parseInt(inputY.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + const span = document.createElement("span"); + span.classList.add("input-group-text"); + span.innerText = text; + + div.appendChild(span); + div.appendChild(inputX); + div.appendChild(inputY); + return div; + } + else if (type == "parallelogram.int") { + const div = document.createElement("div"); + div.classList.add("input-group", "mb-3"); + + function createCoordinateInput(value, onChangeHandler) { + const input = document.createElement("input"); + input.type = "number"; + input.classList.add("form-control", "coordinate-input"); + input.value = value; + input.onchange = onChangeHandler; + return input; + } + + const inputX1 = createCoordinateInput(data[0][0], function () { + config[device][text][0][0] = parseInt(inputX1.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputY1 = createCoordinateInput(data[0][1], function () { + config[device][text][0][1] = parseInt(inputY1.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputX2 = createCoordinateInput(data[1][0], function () { + config[device][text][1][0] = parseInt(inputX2.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputY2 = createCoordinateInput(data[1][1], function () { + config[device][text][1][1] = parseInt(inputY2.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputX3 = createCoordinateInput(data[2][0], function () { + config[device][text][2][0] = parseInt(inputX3.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputY3 = createCoordinateInput(data[2][1], function () { + config[device][text][2][1] = parseInt(inputY3.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputX4 = createCoordinateInput(data[3][0], function () { + config[device][text][3][0] = parseInt(inputX4.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const inputY4 = createCoordinateInput(data[3][1], function () { + config[device][text][3][1] = parseInt(inputY4.value); + send({ + op: "configuration", + device: device, + json: config[device], + }); + }); + + const span = document.createElement("span"); + span.classList.add("input-group-text"); + span.innerText = text; + + div.appendChild(span); + div.appendChild(inputX1); + div.appendChild(inputY1); + div.appendChild(inputX2); + div.appendChild(inputY2); + div.appendChild(inputX3); + div.appendChild(inputY3); + div.appendChild(inputX4); + div.appendChild(inputY4); + return div; + } else { const options = addressKeys[device][text]; - + if (typeof options == "object") { const index = data; - + // Create a dropdown const div = document.createElement("div"); div.classList.add("input-group"); div.classList.add("mb-3"); - + const select = document.createElement("select"); select.classList.add("form-select"); select.onchange = function () { @@ -665,7 +887,7 @@ $(document).ready(function () { json: config[device], }); } - + for (let i = 0; i < Object.keys(options).length; i++) { const option = document.createElement("option"); option.value = i; @@ -673,63 +895,98 @@ $(document).ready(function () { option.innerText = options[i]; select.appendChild(option); } - + const span = document.createElement("span"); span.classList.add("input-group-text"); span.innerText = text; - + div.appendChild(span); div.appendChild(select); return div; } } } - + const regenerateConfig = () => { - const configElement = $("#configuration"); + const configElement = $("#options"); configElement.empty(); - - for (const deviceId in config) { - const deviceConfig = config[deviceId]; - if (addressKeys[deviceId] == undefined) { - console.log(`Unknown Device Config: ${deviceId}`); - // const alert = $(``); - // configElement.append(alert); + + // Sort the keys in each config by their addressKeys + for(const deviceId in addressKeys) + { + if (!(deviceId in config)) { continue; } - + const title = addressKeys[deviceId]["internal_title"]; const deviceElement = $(`
    `); deviceElement.append(`
    ${title}
    `); const deviceBody = $(`
    `); deviceElement.append(deviceBody); - - for (const address of Object.keys(deviceConfig).sort()) { - const data = deviceConfig[address]; - const type = addressKeys[deviceId][address]; - if (type == undefined) { - const alert = $(``); + + const deviceConfig = config[deviceId]; + for (const address in addressKeys[deviceId]) { + if (address == "internal_title") { + continue; + } + + if (!(address in deviceConfig)) { + const alert = $(``); deviceBody.append(alert); continue; } - + + const data = deviceConfig[address]; + const type = addressKeys[deviceId][address]; const inputElement = generateElementForConfiguration(data, type, deviceId, address); deviceBody.append(inputElement); } - - for (const address in addressKeys[deviceId]) { - if (address in deviceConfig || address == "internal_title") { - continue; - } - - const alert = $(``); - deviceBody.append(alert); - } - + configElement.append(deviceElement); } + + // config = outputConfig; + // for (const deviceId in config) { + // const deviceConfig = config[deviceId]; + // if (addressKeys[deviceId] == undefined) { + // console.log(`Unknown Device Config: ${deviceId}`); + // // const alert = $(``); + // // configElement.append(alert); + // continue; + // } + + // const title = addressKeys[deviceId]["internal_title"]; + // const deviceElement = $(`
    `); + // deviceElement.append(`
    ${title}
    `); + // const deviceBody = $(`
    `); + // deviceElement.append(deviceBody); + + // for (const address of Object.keys(deviceConfig).sort()) { + // const data = deviceConfig[address]; + // const type = addressKeys[deviceId][address]; + // if (type == undefined) { + // const alert = $(``); + // deviceBody.append(alert); + // continue; + // } + + // const inputElement = generateElementForConfiguration(data, type, deviceId, address); + // deviceBody.append(inputElement); + // } + + // for (const address in addressKeys[deviceId]) { + // if (address in deviceConfig || address == "internal_title") { + // continue; + // } + + // const alert = $(``); + // deviceBody.append(alert); + // } + + // configElement.append(deviceElement); + // } } - + function send(obj) { sendQueue.push(obj); } diff --git a/display/scripts/utilities.js b/display/scripts/utilities.js index c529122d..ad9fef1c 100644 --- a/display/scripts/utilities.js +++ b/display/scripts/utilities.js @@ -28,6 +28,15 @@ const transferImageToElement = (id, data) => { img.src = "data:image/jpeg;base64," + data; } +const trasferImageBytesToElement = (id, data) => { + const img = document.getElementById(id); + const msgarray = new Uint8Array(data); + const blob = new Blob([msgarray], { type: "image/jpeg" }); + const urlCreator = window.URL || window.webkitURL; + const imageUrl = urlCreator.createObjectURL(blob); + img.src = imageUrl; +} + const fromFloatToBytes = (value) => { var buffer = new ArrayBuffer(4); var view = new DataView(buffer); diff --git a/display/styles/index.css b/display/styles/index.css index 2db3d8cd..db90d939 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -141,6 +141,30 @@ span[data-state="5"] { margin: 1rem 0; } +.controls-row { + display: flex; + flex-direction: row; + gap: 1rem; +} + +.controls-row > button { + text-wrap: nowrap; +} + +#controls { + display: flex; + flex-direction: column; + gap: 1rem; + width: fit-content; +} + +#options { + display: flex; + flex-direction: column; + gap: 1rem; + margin: 1rem 0; +} + #preferences { display: flex; flex-direction: column; @@ -153,21 +177,55 @@ span[data-state="5"] { #images { display: flex; - flex-direction: row; + flex-direction: column; gap: 1rem; - flex-wrap: wrap; } -#images > canvas[data-type="regular"] { - width: 640px !important; - height: 480px !important; +img[data-type="regular"] { + width: 480px !important; + height: 640px !important; } -#images > canas[data-type="bigboy"] { +img[data-type="bigboy"] { width: 800px !important; height: 800px !important; } +img[data-type="small"] { + width: 320px !important; + height: 320px !important; +} + #logging { margin: 1rem; +} + +.coordinate-input { + width: 60px; + margin-right: 5px; + text-align: center; + border-radius: 5px; + border: 1px solid #ced4da; +} + +.roww { + display: flex; + flex-direction: row; + flex-wrap: wrap; + width: fit-content; + height: min-content; +} + +#dashboard { + display: flex; + flex-wrap: wrap; + gap: 1rem; +} + +#vtest { + min-width: 1000px; +} + +#active_preset_value { + color: #ff7bff; } \ No newline at end of file diff --git a/setup/autonav.rules b/setup/etc/autonav.rules similarity index 95% rename from setup/autonav.rules rename to setup/etc/autonav.rules index cde1f625..f6cd8301 100644 --- a/setup/autonav.rules +++ b/setup/etc/autonav.rules @@ -11,4 +11,4 @@ KERNEL=="hidraw*", KERNELS=="*28DE:*", MODE="0666", TAG+="uaccess" SUBSYSTEM=="usb", ATTRS{idVendor}=="28de", MODE="0666" # Safety Lights PICO -KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" +KERNEL=="ttyACM*", ATTRS{idVendor}=="2e8a", ATTRS{idProduct}=="000a", ATTRS{serial}=="E660D051138A8531", MODE:="0666", GROUP:="dialout", SYMLINK+="autonav-mc-safetylights" \ No newline at end of file diff --git a/setup/autonav.service b/setup/etc/autonav.service similarity index 87% rename from setup/autonav.service rename to setup/etc/autonav.service index 87d6432d..2bb7edc3 100644 --- a/setup/autonav.service +++ b/setup/etc/autonav.service @@ -1,5 +1,5 @@ [Unit] -Description=Autonav 2023 Service +Description=Autonav 2024 Service [Service] Type=simple diff --git a/setup/autonav_service.sh b/setup/etc/autonav_service.sh similarity index 80% rename from setup/autonav_service.sh rename to setup/etc/autonav_service.sh index d123c582..66f7c22d 100644 --- a/setup/autonav_service.sh +++ b/setup/etc/autonav_service.sh @@ -6,7 +6,7 @@ export DISPLAY=:0 source /opt/ros/humble/setup.bash # Check if build is needed, if so, build -cd /home/$LOCAL_USER/autonav_software_2023/autonav_ws +cd /home/$LOCAL_USER/autonav_software_2024/autonav_ws UPSTREAM=${1:-'@{u}'} LOCAL=$(git rev-parse @) REMOTE=$(git rev-parse "$UPSTREAM") @@ -21,5 +21,5 @@ else fi # Launch -source /home/$LOCAL_USER/autonav_software_2023/autonav_ws/install/setup.bash +source /home/$LOCAL_USER/autonav_software_2024/autonav_ws/install/setup.bash ros2 launch autonav_launch competition.xml \ No newline at end of file diff --git a/setup/steam.sh b/setup/etc/steam.sh similarity index 100% rename from setup/steam.sh rename to setup/etc/steam.sh diff --git a/setup/vnav.sh b/setup/etc/vnav.sh similarity index 100% rename from setup/vnav.sh rename to setup/etc/vnav.sh diff --git a/setup/jams.sh b/setup/jams.sh deleted file mode 100755 index 36a4a2d2..00000000 --- a/setup/jams.sh +++ /dev/null @@ -1,5 +0,0 @@ -# Download and unzip the best jams around -curl --output jams.zip --netrc-file vectorsecrets.txt https://files.dylanzeml.in/protected/jams.zip -mkdir -p $HOME/autonav_software_2023/deps/songs -unzip -o jams.zip -d $HOME/autonav_software_2023/deps/songs -rm jams.zip \ No newline at end of file diff --git a/setup/setup.sh b/setup/setup.sh index 4290c79b..50a1da4d 100755 --- a/setup/setup.sh +++ b/setup/setup.sh @@ -19,19 +19,13 @@ bash vnav.sh # Steam Controller Dependencies bash steam.sh -# Music Dependencies -bash jams.sh - -# ImGUI Dependencies -sudo apt install libglfw3-dev libglew-dev -y - # Python deps sudo apt install python3-pip -y pip3 install python-can[serial] pip3 install websockets # Copy the udev rules to the correct location -sudo cp autonav.rules /etc/udev/rules.d/autonav.rules +sudo cp etc/autonav.rules /etc/udev/rules.d/autonav.rules # Reload udev sudo service udev reload @@ -39,8 +33,8 @@ sleep 2 sudo service udev restart # Copy services -sudo cp autonav.service /etc/systemd/system/autonav.service -sudo cp autonav_service.sh /usr/bin/autonav_service.sh +sudo cp etc/autonav.service /etc/systemd/system/autonav.service +sudo cp etc/autonav_service.sh /usr/bin/autonav_service.sh # chmod time :D sudo chmod +x /usr/bin/autonav_service.sh