diff --git a/.vscode/settings.json b/.vscode/settings.json index f2a6e860..1de9fc4b 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -92,5 +92,6 @@ "coroutine": "cpp", "source_location": "cpp" }, - "cmake.configureOnOpen": false + "cmake.configureOnOpen": false, + "ros.distro": "humble" } \ No newline at end of file diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 80f2307e..5ede35a5 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -5,7 +5,7 @@ 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 +from scr_msgs.srv import SetSystemState, UpdateConfig from std_msgs.msg import Empty from autonav_msgs.msg import Position, MotorFeedback, MotorInput, MotorControllerDebug, ObjectDetection, PathingDebug, GPSFeedback, IMUData, Conbus from sensor_msgs.msg import CompressedImage @@ -62,7 +62,9 @@ def __init__(self): 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", 5) + 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)) @@ -72,7 +74,8 @@ def __init__(self): 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(ConfigurationInstruction, "/scr/configuration", 100) + # 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) @@ -87,9 +90,12 @@ def __init__(self): self.systemStateService = self.create_client(SetSystemState, "/scr/state/set_system_state") - self.cameraSubscriber = self.create_subscription(CompressedImage, "/autonav/camera/compressed", self.cameraCallback, 20) - self.filteredSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/image", self.filteredCallback, 20) - self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, 20) + 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") @@ -137,22 +143,25 @@ async def consumer(self, websocket): if obj["op"] == "broadcast": self.broadcastPublisher.publish(Empty()) - # if obj["op"] == "configuration" and "device" in obj and "opcode" in obj: - # msg = ConfigurationInstruction() - # msg.device = str(obj["device"]) - # msg.opcode = int(obj["opcode"]) - # msg.data = obj["data"] if "data" in obj else [] - # msg.address = str(obj["address"]) if "address" in obj else "" - # msg.iterator = int(obj["iterator"]) if "iterator" in obj else 0 - # self.configurationInstructionPublisher.publish(msg) + 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 + "nodes": nodes, + "states": node_states, + "configs": self.node_configs }), unique_id) if obj["op"] == "set_system_state": @@ -336,8 +345,50 @@ def motorControllerDebugCallback(self, msg: MotorControllerDebug): "right_motor_output": msg.right_motor_output })) - def cameraCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/camera/compressed"): + 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() @@ -345,13 +396,13 @@ def cameraCallback(self, msg: CompressedImage): self.pushSendQueue(json.dumps({ "op": "data", - "topic": "/autonav/camera/compressed", + "topic": "/autonav/cfg_space/raw/image/right", "format": msg.format, "data": base64_str })) - def filteredCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image"): + def bigboiCallback(self, msg: CompressedImage): + if not self.limiter.use("/autonav/cfg_space/raw/debug"): return byts = msg.data.tobytes() @@ -359,7 +410,7 @@ def filteredCallback(self, msg: CompressedImage): self.pushSendQueue(json.dumps({ "op": "data", - "topic": "/autonav/cfg_space/raw/image", + "topic": "/autonav/cfg_space/raw/debug", "format": msg.format, "data": base64_str })) diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/autonav_ws/src/autonav_filters/src/filters.py index ad12ee83..2c98e345 100644 --- a/autonav_ws/src/autonav_filters/src/filters.py +++ b/autonav_ws/src/autonav_filters/src/filters.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +import json +from types import SimpleNamespace from autonav_msgs.msg import MotorFeedback, GPSFeedback, Position, IMUData from scr.states import DeviceStateEnum, SystemStateEnum, SystemModeEnum from particlefilter import ParticleFilter @@ -15,16 +17,11 @@ class FilterType(IntEnum): PARTICLE_FILTER = 1 -CONFIG_FILTER_TYPE = "filter_type" -CONFIG_DEGREE_OFFSET = "degree_offset" -CONFIG_SEED_HEADING = "seed_heading" - - class FiltersNodeConfig: def __init__(self): - self.filterType = FilterType.PARTICLE_FILTER - self.degreeOffset = 107.0 - self.seedHeading = False + self.filter_type = FilterType.PARTICLE_FILTER + self.degree_offset = 107.0 + self.seed_heading = False class FiltersNode(Node): @@ -40,10 +37,16 @@ def __init__(self): self.pf = ParticleFilter(self.latitudeLength, self.longitudeLength) self.reckoning = DeadReckoningFilter() - self.config = FiltersNodeConfig() + self.config = self.get_default_config() self.onReset() + def config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + def get_default_config(self): + return FiltersNodeConfig() + def init(self): self.create_subscription(GPSFeedback, "/autonav/gps", self.onGPSReceived, 20) self.create_subscription(IMUData, "/autonav/imu", self.onIMUReceived, 20) @@ -59,11 +62,11 @@ def getRealHeading(self, heading: float): if heading < 0: heading = 360 + -heading - heading += self.config.degreeOffset + heading += self.config.degree_offset return heading def onReset(self): - if self.lastIMUReceived is not None and self.config.seedHeading: + 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: @@ -90,15 +93,14 @@ def onGPSReceived(self, msg: GPSFeedback): self.lastGps = msg - filterType = self.config.filterType + filterType = self.config.filter_type if filterType == FilterType.PARTICLE_FILTER: self.pf.gps(msg) '''elif filterType == FilterType.DEAD_RECKONING: self.reckoning.gps(msg) ''' def onMotorFeedbackReceived(self, msg: MotorFeedback): - #self.get_logger().info("received motorfeedback") - filterType = self.config.filterType + filterType = self.config.filter_type averages = None if filterType == FilterType.PARTICLE_FILTER: averages = self.pf.feedback(msg) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 8e17dd93..5b1cfed5 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,23 +1,17 @@ - - + + + - - - - - - - - - - - - + + + + + \ No newline at end of file diff --git a/autonav_ws/src/autonav_launch/launch/simulation.xml b/autonav_ws/src/autonav_launch/launch/simulation.xml index ae50a81c..c38c55da 100644 --- a/autonav_ws/src/autonav_launch/launch/simulation.xml +++ b/autonav_ws/src/autonav_launch/launch/simulation.xml @@ -15,4 +15,5 @@ + \ No newline at end of file diff --git a/autonav_ws/src/autonav_launch/launch/test.xml b/autonav_ws/src/autonav_launch/launch/test.xml index e13e926a..8c237d0e 100644 --- a/autonav_ws/src/autonav_launch/launch/test.xml +++ b/autonav_ws/src/autonav_launch/launch/test.xml @@ -10,6 +10,7 @@ + \ No newline at end of file diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index b2d9594c..e3c42369 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -45,8 +45,8 @@ class SteamJoyNode : public SCR::Node json get_default_config() override { SteamJoyNodeConfig defaultConfig; - defaultConfig.throttle_deadzone = 0.03f; - defaultConfig.steering_deadzone = 0.03f; + defaultConfig.throttle_deadzone = 0.15f; + defaultConfig.steering_deadzone = 0.15f; defaultConfig.forward_speed = 1.8f; defaultConfig.turn_speed = 1.0f; defaultConfig.max_turn_speed = 3.14159265f; @@ -66,6 +66,11 @@ class SteamJoyNode : public SCR::Node set_device_state(SCR::DeviceState::READY); } } + + float lerp(float a, float b, float t) + { + return a + (b - a) * t; + } void onSteamDataReceived(const autonav_msgs::msg::SteamInput &msg) { @@ -82,18 +87,39 @@ class SteamJoyNode : public SCR::Node if (abs(msg.ltrig) > throttleDeadzone || abs(msg.rtrig) > throttleDeadzone) { - throttle = msg.rtrig; - throttle = throttle - msg.ltrig; + target_throttle = msg.rtrig - msg.ltrig; + is_working = true; } if (abs(msg.lpad_x) > steeringDeadzone) { - steering = msg.lpad_x; + target_steering = msg.lpad_x; + is_working = true; + } + + if (abs(msg.ltrig) < throttleDeadzone && abs(msg.rtrig) < throttleDeadzone) + { + target_throttle = 0; + is_working = false; } + if (abs(msg.lpad_x) < steeringDeadzone) + { + target_steering = 0; + is_working = false; + } + + // Generate a forward/angular velocity command that ramps up/down smoothly + const float throttleRate = 0.03; + const float steeringRate = 0.01; + current_throttle = lerp(current_throttle, target_throttle * config.forward_speed, throttleRate * (is_working ? 1 : 1.8)); + current_steering = lerp(current_steering, target_steering * config.turn_speed, steeringRate * (is_working ? 1 : 1.8)); + autonav_msgs::msg::MotorInput input; - input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed); - input.angular_velocity = -1 * SCR::Utilities::clamp(steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); + // input.forward_velocity = SCR::Utilities::clamp(throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_speed); + // input.angular_velocity = -1 * SCR::Utilities::clamp(steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); + input.forward_velocity = SCR::Utilities::clamp(current_throttle, -config.max_forward_speed, config.max_forward_speed); + input.angular_velocity = -1 * SCR::Utilities::clamp(current_steering * config.turn_speed, -config.max_turn_speed, config.max_turn_speed); motor_publisher->publish(input); } @@ -104,6 +130,11 @@ class SteamJoyNode : public SCR::Node private: SteamJoyNodeConfig config; + float target_throttle = 0; + float target_steering = 0; + float current_throttle = 0; + float current_steering = 0; + bool is_working = false; }; int main(int argc, char *argv[]) diff --git a/autonav_ws/src/autonav_manual/src/steam.py b/autonav_ws/src/autonav_manual/src/steam.py index 5c0ece1e..38a7e5e5 100644 --- a/autonav_ws/src/autonav_manual/src/steam.py +++ b/autonav_ws/src/autonav_manual/src/steam.py @@ -96,6 +96,9 @@ def onButtonReleased(self, button: SteamControllerButton, msTime: float): self.set_system_state(SystemStateEnum.DISABLED) self.safetyLightsPublisher.publish(toSafetyLights(False, False, 2, 100, "#A020F0")) + def getClockMs(self): + return time.time() * 1000 + def onSteamControllerInput(self, _, sci: SteamControllerInput): if self.device_state != DeviceStateEnum.OPERATING: return diff --git a/autonav_ws/src/autonav_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 191ccb0b..d681161a 100644 --- a/autonav_ws/src/autonav_nav/src/astar.py +++ b/autonav_ws/src/autonav_nav/src/astar.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +import json +from types import SimpleNamespace from autonav_msgs.msg import Position, IMUData, PathingDebug, SafetyLights from scr_msgs.msg import SystemState from scr.node import Node @@ -78,9 +80,15 @@ def __init__(self): 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.config = AStarNodeConfig() + self.config = self.get_default_config() self.onReset() + def config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + 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) diff --git a/autonav_ws/src/autonav_nav/src/path_resolver.py b/autonav_ws/src/autonav_nav/src/path_resolver.py index 3add6c73..828fc4dd 100644 --- a/autonav_ws/src/autonav_nav/src/path_resolver.py +++ b/autonav_ws/src/autonav_nav/src/path_resolver.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +import json +from types import SimpleNamespace from autonav_msgs.msg import MotorInput, Position, SafetyLights from scr_msgs.msg import SystemState from scr.node import Node @@ -36,13 +38,13 @@ def toSafetyLights(autonomous: bool, eco: bool, mode: int, brightness: int, colo class PathResolverNodeConfig: def __init__(self): - self.FORWARD_SPEED = 2.1 - self.REVERSE_SPEED = -0.4 - self.RADIUS_MULTIPLIER = 1.2 - self.RADIUS_MAX = 4.0 - self.RADIUS_START = 0.7 - self.ANGULAR_AGGRESSINON = 2.2 - self.MAX_ANGULAR_SPEED = 0.5 + self.forward_speed = 2.1 + 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 class PathResolverNode(Node): @@ -58,11 +60,17 @@ def init(self): 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.config = PathResolverNodeConfig() + self.config = self.get_default_config() self.create_timer(0.05, self.onResolve) self.set_device_state(DeviceStateEnum.READY) + def config_updated(self, jsonObject): + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) + + def get_default_config(self): + return PathResolverNodeConfig() + def reset(self): self.position = None self.backCount = -1 @@ -105,10 +113,10 @@ def onResolve(self): cur_pos = (self.position.x, self.position.y) lookahead = None - radius = self.config.RADIUS_START - while lookahead is None and radius <= self.config.RADIUS_MAX: + radius = self.config.radius_start + while lookahead is None and radius <= self.config.radius_max: lookahead = self.pure_pursuit.get_lookahead_point(cur_pos[0], cur_pos[1], radius) - radius *= self.config.RADIUS_MULTIPLIER + radius *= self.config.radius_multiplier motor_packet = MotorInput() motor_packet.forward_velocity = 0.0 @@ -117,9 +125,9 @@ def onResolve(self): if self.backCount == -1 and (lookahead is not None and ((lookahead[1] - cur_pos[1]) ** 2 + (lookahead[0] - cur_pos[0]) ** 2) > 0.25): angle_diff = math.atan2(lookahead[1] - cur_pos[1], lookahead[0] - cur_pos[0]) error = self.get_angle_difference(angle_diff, self.position.theta) / math.pi - forward_speed = self.config.FORWARD_SPEED * (1 - abs(error)) ** 5 + forward_speed = self.config.forward_speed * (1 - abs(error)) ** 5 motor_packet.forward_velocity = forward_speed - motor_packet.angular_velocity = clamp(error * self.config.ANGULAR_AGGRESSINON, -self.config.MAX_ANGULAR_SPEED, self.config.MAX_ANGULAR_SPEED) + motor_packet.angular_velocity = clamp(error * self.config.angular_aggression, -self.config.max_angular_speed, self.config.max_angular_speed) if self.status == 0: self.safety_lights_publisher.publish(toSafetyLights(True, False, 2, 255, "#FFFFFF")) @@ -132,7 +140,7 @@ def onResolve(self): self.status = 0 self.backCount -= 1 - motor_packet.forward_velocity = self.config.REVERSE_SPEED + motor_packet.forward_velocity = self.config.reverse_speed motor_packet.angular_velocity = BACK_SPEED if IS_SOUTH else (-1 * BACK_SPEED) if not self.mobility: diff --git a/autonav_ws/src/autonav_serial/src/camera.py b/autonav_ws/src/autonav_serial/src/camera.py index 79b08eec..7c2573a3 100644 --- a/autonav_ws/src/autonav_serial/src/camera.py +++ b/autonav_ws/src/autonav_serial/src/camera.py @@ -36,8 +36,7 @@ def init(self): self.camera_thread.start() def config_updated(self, jsonObject): - self.config = json.loads(json.dumps(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) - self.log(json.dumps(self.config.__dict__)) + self.config = json.loads(self.jdump(jsonObject), object_hook=lambda d: SimpleNamespace(**d)) def get_default_config(self): return CameraNodeConfig() diff --git a/autonav_ws/src/autonav_vision/CMakeLists.txt b/autonav_ws/src/autonav_vision/CMakeLists.txt index f432c62d..205352b9 100644 --- a/autonav_ws/src/autonav_vision/CMakeLists.txt +++ b/autonav_ws/src/autonav_vision/CMakeLists.txt @@ -18,17 +18,12 @@ find_package(sensor_msgs REQUIRED) find_package(scr REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) - -# Install Python executables -install(PROGRAMS - src/transformations.py - DESTINATION lib/${PROJECT_NAME} -) # 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/combination.py b/autonav_ws/src/autonav_vision/src/combination.py new file mode 100644 index 00000000..527f256a --- /dev/null +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -0,0 +1,149 @@ +#!/usr/bin/env python3 + +from types import SimpleNamespace +import rclpy +import json +import cv2 +import numpy as np +from cv_bridge import CvBridge +from nav_msgs.msg import MapMetaData, OccupancyGrid +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 + + +g_bridge = CvBridge() +g_mapData = MapMetaData() +g_mapData.width = 100 +g_mapData.height = 100 +g_mapData.resolution = 0.1 +g_mapData.origin = Pose() +g_mapData.origin.position.x = -10.0 +g_mapData.origin.position.y = -10.0 + + +IMAGE_WIDTH = 640 +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") + + 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.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: + 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) + + 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() + + +def main(): + rclpy.init() + node = ImageCombiner() + Node.run_node(node) + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 08f01e06..353644a8 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -97,7 +97,7 @@ class ExpandifyNode : public SCR::Node return; } - std::vector cfg_space = std::vector(config.map_res * config.map_res); + std::vector cfg_space = std::vector((config.map_res) * config.map_res); std::fill(cfg_space.begin(), cfg_space.end(), 0); for (int x = 0; x < config.map_res; x++) @@ -153,8 +153,8 @@ class ExpandifyNode : public SCR::Node nav_msgs::msg::MapMetaData map; - float maxRange = 0.65; - float noGoPercent = 0.70; + float maxRange = 0.10; + float noGoPercent = 0.38; 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 cf14069d..19cf8d77 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -17,7 +17,7 @@ g_bridge = CvBridge() g_mapData = MapMetaData() -g_mapData.width = 200 +g_mapData.width = 100 g_mapData.height = 100 g_mapData.resolution = 0.1 g_mapData.origin = Pose() @@ -37,16 +37,22 @@ def __init__(self): self.blur_iterations = 3 self.rod_offset = 130 self.map_res = 80 + self.image_warp_tl = 0.26 + self.image_warp_tr = 0.26 class ImageTransformer(Node): - def __init__(self): + def __init__(self, dir = "left"): super().__init__("autonav_vision_transformer") + self.dir = dir + + def directionify(self, topic): + return topic + "/" + self.dir def init(self): - self.cameraSubscriber = self.create_subscription(CompressedImage, "/autonav/camera/compressed", self.onImageReceived, 1) - self.rawMapPublisher = self.create_publisher(OccupancyGrid, "/autonav/cfg_space/raw", 1) - self.filteredImagePublisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw/image", 1) + 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.set_device_state(DeviceStateEnum.OPERATING) @@ -67,22 +73,8 @@ def regionOfDisinterest(self, img, vertices): masked_image = cv2.bitwise_and(img, mask) return masked_image - def flattenImage(self, img): - top_left = (int)(img.shape[1] * 0.26), (int)(img.shape[0]) - top_right = (int)(img.shape[1] - img.shape[1] * 0.26), (int)(img.shape[0]) - bottom_left = 0, 0 - bottom_right = (int)(img.shape[1]), 0 - - src_pts = np.float32([[top_left], [top_right], [bottom_left], [bottom_right]]) - dest_pts = np.float32([[0, 480], [640, 480], [0, 0], [640, 0]]) - - matrix = cv2.getPerspectiveTransform(dest_pts, src_pts) - output = cv2.warpPerspective(img, matrix, (640, 480)) - return output - def publishOccupancyMap(self, img): - datamap = cv2.resize(img, dsize=(self.config.map_res, self.config.map_res), - interpolation=cv2.INTER_LINEAR) / 2 + 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) @@ -105,23 +97,29 @@ def onImageReceived(self, image = CompressedImage): # Apply region of disinterest and flattening height = img.shape[0] width = img.shape[1] - region_of_disinterest_vertices = [ - (0, height), - (width / 2, height / 2 + self.config.rod_offset), - (width, height) - ] + 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) + ] # Apply region of disinterest and flattening mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) mask[mask < 250] = 0 - mask = self.flattenImage(mask) # Actually generate the map self.publishOccupancyMap(mask) # 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) + # 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" @@ -132,8 +130,9 @@ def onImageReceived(self, image = CompressedImage): def main(): rclpy.init() - node = ImageTransformer() - Node.run_node(node) + node_left = ImageTransformer(dir = "left") + node_right = ImageTransformer(dir = "right") + Node.run_nodes([node_left, node_right]) rclpy.shutdown() diff --git a/autonav_ws/src/scr/include/scr/node.hpp b/autonav_ws/src/scr/include/scr/node.hpp index 749cd024..9917f694 100644 --- a/autonav_ws/src/scr/include/scr/node.hpp +++ b/autonav_ws/src/scr/include/scr/node.hpp @@ -78,6 +78,10 @@ namespace SCR /// @param node static void run_node(std::shared_ptr node); + /// @brief Runs the nodes with the correct ROS parameters and specifications + /// @param nodes + static void run_nodes(std::vector> nodes); + protected: /// @brief Called after a node is first discovered by the network. The device state will be set to BOOTING virtual void init() = 0; diff --git a/autonav_ws/src/scr/include/scr/states.hpp b/autonav_ws/src/scr/include/scr/states.hpp index 657cd226..c3ed86c1 100644 --- a/autonav_ws/src/scr/include/scr/states.hpp +++ b/autonav_ws/src/scr/include/scr/states.hpp @@ -14,25 +14,25 @@ namespace SCR ERRORED = 5 }; - // std::string toString(DeviceState state) - // { - // switch (state) - // { - // case DeviceState::OFF: - // return "Off"; - // case DeviceState::BOOTING: - // return "Booting"; - // case DeviceState::STANDBY: - // return "Standby"; - // case DeviceState::READY: - // return "Ready"; - // case DeviceState::OPERATING: - // return "Operating"; - // case DeviceState::ERRORED: - // return "Errored"; - // } - // return "Unknown"; - // } + std::string deviceStateToString(DeviceState state) + { + switch (state) + { + case DeviceState::OFF: + return "Off"; + case DeviceState::BOOTING: + return "Booting"; + case DeviceState::STANDBY: + return "Standby"; + case DeviceState::READY: + return "Ready"; + case DeviceState::OPERATING: + return "Operating"; + case DeviceState::ERRORED: + return "Errored"; + } + return "Unknown"; + } enum SystemState : uint16_t { @@ -42,21 +42,21 @@ namespace SCR SHUTDOWN = 3 }; - // std::string toString(SystemState state) - // { - // switch (state) - // { - // case SystemState::DISABLED: - // return "Disabled"; - // case SystemState::AUTONOMOUS: - // return "Autonomous"; - // case SystemState::MANUAL: - // return "Manual"; - // case SystemState::SHUTDOWN: - // return "Shutdown"; - // } - // return "Unknown"; - // } + std::string systemStateToString(SystemState state) + { + switch (state) + { + case SystemState::DISABLED: + return "Disabled"; + case SystemState::AUTONOMOUS: + return "Autonomous"; + case SystemState::MANUAL: + return "Manual"; + case SystemState::SHUTDOWN: + return "Shutdown"; + } + return "Unknown"; + } enum SystemMode : uint32_t { @@ -65,17 +65,17 @@ namespace SCR PRACTICE = 2, }; - // std::string toString(SystemMode state) - // { - // switch (state) - // { - // case SystemMode::COMPETITION: - // return "Competition"; - // case SystemMode::SIMULATION: - // return "Simulation"; - // case SystemMode::PRACTICE: - // return "Practice"; - // } - // return "Unknown"; - // } + std::string systemModeToString(SystemMode state) + { + switch (state) + { + case SystemMode::COMPETITION: + return "Competition"; + case SystemMode::SIMULATION: + return "Simulation"; + case SystemMode::PRACTICE: + return "Practice"; + } + return "Unknown"; + } } \ 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 5cea64b8..3f82247c 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -40,6 +40,8 @@ def __init__(self, node_name): self.device_state = DeviceStateEnum.OFF self.system_state = SystemStateEnum.DISABLED self.system_mode = SystemModeEnum.COMPETITION + self.device_states = {} + self.node_configs = {} self.mobility = False self.perf_measurements = {} @@ -66,6 +68,7 @@ def on_device_state(self, msg: DeviceState): :param msg: The device state message. """ + self.device_states[msg.device] = msg.state if msg.device is None or msg.device != self.identifier: return @@ -113,6 +116,7 @@ def on_system_state(self, msg: SystemState): self.mobility = msg.mobility 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 @@ -144,7 +148,6 @@ def config_updated(self, json): Called when the configuration is updated. """ - self.get_logger().warn("Config updated method not overridden: config_updated(json)") pass def get_default_config(self): @@ -152,7 +155,6 @@ def get_default_config(self): Gets the default configuration for the node. """ - self.get_logger().warn("Default config method not overridden: get_default_config()") return {} def set_device_state(self, state: DeviceStateEnum): @@ -268,5 +270,19 @@ def run_node(node): executor.spin() executor.remove_node(node) + def run_nodes(nodes): + """ + Runs the nodes with the correct ROS parameters and specifications + + :param nodes: The nodes to run. + """ + + executor = MultiThreadedExecutor() + for node in nodes: + executor.add_node(node) + executor.spin() + for node in nodes: + executor.remove_node(node) + def log(self, message: str): rclpy.logging.get_logger("scr." + self.identifier).info(message) diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index d500019d..8124c5fd 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -50,12 +50,6 @@ namespace SCR SCR::SystemMode newMode = static_cast(msg.mode); SCR::SystemMode oldMode = static_cast(oldState.mode); - //RCLCPP_WARN(this->get_logger(), "SYSTEM STATE CHANGE: %s -> %s", SCR::toString(oldStateEnum).c_str(), SCR::toString(newStateEnum).c_str()); - //RCLCPP_WARN(this->get_logger(), "SYSTEM MODE CHANGE: %s -> %s", SCR::toString(oldMode).c_str(), SCR::toString(newMode).c_str()); - RCLCPP_WARN(this->get_logger(), "SYSTEM STATE CHANGE: DISABLED SYSTEM STATE TOSTRING -> DISABLED SYSTEM STATE TOSTRING"); - RCLCPP_WARN(this->get_logger(), "SYSTEM MODE CHANGE: DISABLED SYSTEM STATE TOSTRING -> DISABLED SYSTEM STATE TOSTRING"); - RCLCPP_WARN(this->get_logger(), "MOBILITY CHANGE: %d -> %d", oldState.mobility, msg.mobility); - system_state_transition(oldState, msg); system_mode = static_cast(msg.mode); @@ -78,9 +72,6 @@ namespace SCR } device_state = static_cast(msg.state); - //RCLCPP_INFO(this->get_logger(), "Received Topic: Device state changed to %s", SCR::toString(device_state).c_str()); - RCLCPP_INFO(this->get_logger(), "Received Topic: Device state changed to DISABLED DEVICE STATE TOSTRING"); - if (device_state == SCR::DeviceState::BOOTING) { // Get the default config and push it to the server @@ -169,8 +160,7 @@ namespace SCR auto result = result_future.get(); if (!result) { - //RCLCPP_ERROR(this->get_logger(), "Failed to set device state to: %s", SCR::toString(state).c_str()); - RCLCPP_ERROR(this->get_logger(), "Failed to set device state to: DISABLED DEVICE STATE TOSTRING"); + RCLCPP_ERROR(this->get_logger(), "Failed to set device state to: %s", SCR::deviceStateToString(state).c_str()); } } @@ -255,5 +245,26 @@ namespace SCR // Shutdown executor.remove_node(node); + + rclcpp::shutdown(); + } + + void Node::run_nodes(std::vector> nodes) + { + rclcpp::executors::MultiThreadedExecutor executor; + for (auto node : nodes) + { + executor.add_node(node); + } + + executor.spin(); + + for (auto node : nodes) + { + executor.remove_node(node); + } + + rclcpp::shutdown(); } + } \ 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 e96fb6ab..dbc343d7 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -95,8 +95,6 @@ class CoreNode : public rclcpp::Node } // Update the device state - //RCLCPP_INFO(this->get_logger(), "Device %s state changed to %s", request->device.c_str(), SCR::toString((SCR::DeviceState)request->state).c_str()); - RCLCPP_INFO(this->get_logger(), "Device %s state changed to DISABLED DEVICE STATE TOSTRING"); if (device_states.find(request->device) == device_states.end()) { // This is the first time we've seen this device, so we need to publish the system state, device state, and all known configs @@ -157,7 +155,6 @@ class CoreNode : public rclcpp::Node response->success = true; // Publish the new config - RCLCPP_INFO(this->get_logger(), "Config updated for device %s -> %s", request->device.c_str(), request->json.c_str()); scr_msgs::msg::ConfigUpdated config_updated_message; config_updated_message.device = request->device; config_updated_message.json = request->json; diff --git a/display/index.html b/display/index.html index eb296dc7..dc9a5ceb 100644 --- a/display/index.html +++ b/display/index.html @@ -5,7 +5,7 @@ - SCR Weeb Wagon + SCR Danger Zone @@ -41,7 +41,7 @@ -

Weeb Wagon

+

Danger Zone

 @@ -122,8 +122,11 @@

Device States

Camera / Filtered

- - + + + + +
@@ -230,7 +233,7 @@

System

-

Waiting for the Weeb Wagon

+

Waiting for the Danger Zone

diff --git a/display/scripts/globals.js b/display/scripts/globals.js index d5742eba..1052e765 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -16,6 +16,7 @@ var deviceStates = {}; var logs = []; var iterator = 0; var iterators = []; +var debug = false; var addressKeys = { "autonav_serial_imu": { @@ -34,14 +35,24 @@ var addressKeys = { "autonav_vision_transformer": { "internal_title": "[Vision] Transformer", "lower_hue": "int", - "lower_saturation": "int", - "lower_value": "int", + "lower_sat": "int", + "lower_val": "int", "upper_hue": "int", - "upper_saturation": "int", - "upper_value": "int", - "blur": "int", + "upper_sat": "int", + "upper_val": "int", + "blur_weight": "int", "blur_iterations": "int", - "region_of_disinterest_offset": "int" + "rod_offset": "int", + "map_res": "int", + "image_warp_tl": "float", + "image_warp_tr": "float" + }, + + "autonav_vision_expandifier": { + "internal_title": "[Vision] Expandifier", + "horizontal_fov": "int", + "map_res": "int", + "vertical_fov": "float" }, "autonav_filters": { @@ -66,8 +77,8 @@ var addressKeys = { "autonav_nav_astar": { "internal_title": "[Navigation] A*", - "pop_distance": "float", - "direction": { + "waypointPopDistance": "float", + "waypointDirection": { 0: "North", 1: "South", 2: "Misc 1", @@ -76,8 +87,8 @@ var addressKeys = { 5: "Misc 4", 6: "Misc 5", }, - "use_only_waypoints": "bool", - "waypoint_delay": "float", + "useOnlyWaypoints": "bool", + "waypointDelay": "float", }, "autonav_nav_resolver": { @@ -91,17 +102,10 @@ var addressKeys = { "max_angular_speed": "float" }, - "autonav_playback": { - "internal_title": "[Playback]", - "record_imu": "bool", - "record_gps": "bool", - "record_position": "bool", - "record_feedback": "bool", - "record_objectdetection": "bool", - "record_manual": "bool", - "record_autonomous": "bool", - "record_input": "bool", - "record_debugfeedback": "bool", + "autonav_image_combiner": { + "internal_title": "[Image Combiner]", + "overlap": "int", + "map_res": "int" } } diff --git a/display/scripts/main.js b/display/scripts/main.js index a0ed897d..d16495e7 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -62,6 +62,7 @@ $(document).ready(function () { } if (op == "get_nodes_callback") { + console.log(obj); for (let i = 0; i < obj.nodes.length; i++) { const node = obj.nodes[i]; send({ @@ -70,13 +71,35 @@ $(document).ready(function () { opcode: 4, iterator: iterate() }); + + const statemap = obj.states; + if (node in statemap) { + if (node == "rosbridge_websocket" || node == "rosapi" || node == "scr_core" || node == "rosapi_params") + { + continue; + } + + deviceStates[node] = statemap[node]; + unorderedListElement = $("#element_device_states"); + unorderedListElement.empty(); + for (const id in deviceStates) { + const state = deviceStates[id]; + unorderedListElement.append(`
${id}: ${deviceStateToName(state)}
`); + } + } } + + for (const key in obj.configs) + { + config[key] = obj.configs[key]; + } + regenerateConfig(); } } }; websocket.onclose = function (event) { - $("#connecting-state").text("Waiting for the Weeb Wagon"); + $("#connecting-state").text("Waiting for the Danger Zone"); $(".connecting").show(); $(".connecting-input").show(); $("#main").hide(); @@ -97,10 +120,6 @@ $(document).ready(function () { var sendQueue = []; - function send(obj) { - sendQueue.push(obj); - } - function setSystemState() { send({ op: "set_system_state", @@ -110,148 +129,6 @@ $(document).ready(function () { }); } - function generateElementForConfiguration(data, type, device, text) { - if (type == "bool") { - const checked = fromBytesToBool(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 () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromBoolToBytes(select.value == 1)), - iterator: iterate() - }); - } - - 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; - } - else if (type == "float") { - 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"); - input.value = fromBytesToFloat(data).toFixed(6); - input.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromFloatToBytes(input.value)), - iterator: iterate() - }); - } - - 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 == "int") { - 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"); - input.value = fromBytesToInt(data); - input.onchange = function () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromIntToBytes(input.value)), - iterator: iterate() - }); - } - - const span = document.createElement("span"); - span.classList.add("input-group-text"); - span.innerText = text; - - div.appendChild(span); - div.appendChild(input); - return div; - } - else { - const options = addressKeys[device][text]; - - if (typeof options == "object") { - const index = fromBytesToInt(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 () { - send({ - op: "configuration", - opcode: 1, - device: device, - address: text, - data: Array.from(fromIntToBytes(select.value)), - iterator: iterate() - }); - } - - for (let i = 0; i < Object.keys(options).length; i++) { - const option = document.createElement("option"); - option.value = i; - option.selected = i == index; - 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; - } - } - } - function generateElementForConbus(data, type, text, deviceId, address, readonly = false) { if (type == "bool") { const checked = fromBytesToBool(data); @@ -441,54 +318,7 @@ $(document).ready(function () { } if (topic == "/scr/configuration") { - const { device, opcode, data, address } = msg; - if (opcode == 4) { - return; - } - - if (opcode == 2 || opcode == 3) { - if (!(device in config)) { - config[device] = {}; - } - - config[device][address] = data; - - const configElement = $("#configuration"); - configElement.empty(); - - for (const deviceId in config) { - const deviceConfig = config[deviceId]; - 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); - } - } + console.log(msg); return; } @@ -560,15 +390,34 @@ $(document).ready(function () { return; } - if (topic == "/autonav/camera/compressed") { + if (topic == "/autonav/camera/compressed/left") { // Set to - const imgElement = document.getElementById("target_raw_camera"); + const imgElement = document.getElementById("target_raw_camera_left"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; return; } - if (topic == "/autonav/cfg_space/raw/image") { - const imgElement = document.getElementById("target_filtered_camera"); + if (topic == "/autonav/camera/compressed/right") { + // Set to + const imgElement = document.getElementById("target_raw_camera_right"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/raw/image/left") { + const imgElement = document.getElementById("target_filtered_camera_left"); + imgElement.src = `data:image/jpeg;base64,${msg.data}`; + return; + } + + if (topic == "/autonav/cfg_space/raw/image/right") { + const imgElement = document.getElementById("target_filtered_camera_right"); + 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}`; return; } @@ -701,4 +550,187 @@ $(document).ready(function () { logs = []; $("#log_body").empty(); }); + + 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; + 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; + } + else if (type == "float") { + 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"); + input.value = data; + input.onchange = function () { + config[device][text] = parseFloat(input.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(input); + return div; + } + else if (type == "int") { + 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"); + input.value = data; + input.onchange = function () { + config[device][text] = parseInt(input.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(input); + 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 () { + config[device][text] = select.value; + send({ + op: "configuration", + device: device, + json: config[device], + }); + } + + for (let i = 0; i < Object.keys(options).length; i++) { + const option = document.createElement("option"); + option.value = i; + option.selected = i == index; + 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"); + 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); + 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); + } }) \ No newline at end of file diff --git a/display/scripts/utilities.js b/display/scripts/utilities.js index 0bc62169..c529122d 100644 --- a/display/scripts/utilities.js +++ b/display/scripts/utilities.js @@ -121,7 +121,7 @@ const radiansToDegrees = (radians) => { } const deviceStateToName = (state) => { - return state == 0 ? "Off" : state == 1 ? "Standby" : state == 2 ? "Ready" : "Operating"; + return ["Off", "Booting", "Standby", "Ready", "Operating", "Errored"][state]; } const clearGlobals = () => { diff --git a/display/styles/index.css b/display/styles/index.css index 0b6edefb..2db3d8cd 100644 --- a/display/styles/index.css +++ b/display/styles/index.css @@ -76,20 +76,34 @@ h5 > span { flex-direction: column; } +/* Off */ span[data-state="0"] { color: --var(-text-color); } +/* Booting */ span[data-state="1"] { - color: #C21858; + color: #FF9800; } +/* Standby */ span[data-state="2"] { - color: #57ACDC; + color: #FFC107; } +/* Ready */ span[data-state="3"] { - color: #60C689; + color: #4CAF50; +} + +/* Operating */ +span[data-state="4"] { + color: #57ACDC; +} + +/* Errored */ +span[data-state="5"] { + color: #C21858; } #element_device_states > h5 > span {