From 487b8da9fd17b97bf19da9d4e5417032ac1d81c5 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 20 Feb 2024 18:39:53 -0600 Subject: [PATCH 01/15] Add ability to spin multiple nodes --- autonav_ws/src/scr/include/scr/node.hpp | 4 ++++ autonav_ws/src/scr/scr/node.py | 14 ++++++++++++++ autonav_ws/src/scr/src/node.cpp | 21 +++++++++++++++++++++ 3 files changed, 39 insertions(+) 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/scr/node.py b/autonav_ws/src/scr/scr/node.py index 5cea64b8..5987e2fd 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -268,5 +268,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 a0694579..9f4cf94c 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -251,5 +251,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 From 3b87e033ec83da8bfff6be92ecfdd92286cfd8b6 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 20 Feb 2024 18:40:09 -0600 Subject: [PATCH 02/15] Transformation can now handle multiple cameras --- .../src/autonav_vision/src/transformations.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index cf14069d..192beaba 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -40,13 +40,17 @@ def __init__(self): 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/raw"), 1) + self.filteredImagePublisher = self.create_publisher(CompressedImage, self.directionify("/autonav/cfg_space/raw/image"), 1) self.set_device_state(DeviceStateEnum.OPERATING) @@ -132,8 +136,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_node([node_left, node_right]) rclpy.shutdown() From 27cb732fa37626413d921c7cf311dcfbe4b72d1d Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 20 Feb 2024 20:04:14 -0600 Subject: [PATCH 03/15] idk --- autonav_ws/src/autonav_display/src/display.py | 65 ++++++++++++++--- .../src/autonav_launch/launch/simulation.xml | 1 + autonav_ws/src/autonav_launch/launch/test.xml | 1 + autonav_ws/src/autonav_vision/CMakeLists.txt | 7 +- .../src/autonav_vision/src/combination.py | 70 +++++++++++++++++++ .../src/autonav_vision/src/transformations.py | 4 +- display/index.html | 7 +- display/scripts/main.js | 27 +++++-- 8 files changed, 159 insertions(+), 23 deletions(-) create mode 100644 autonav_ws/src/autonav_vision/src/combination.py diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 80f2307e..428dce47 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -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", 5) self.limiter.setLimit("/autonav/debug/astar/image", 5) self.get_logger().info("Broadcasting on ws://{}:{}".format(self.host, self.port)) @@ -87,8 +89,11 @@ 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.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", self.bigboiCallback, 20) self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, 20) self.get_logger().info("Starting event loop") @@ -336,8 +341,8 @@ 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() @@ -345,13 +350,13 @@ def cameraCallback(self, msg: CompressedImage): self.pushSendQueue(json.dumps({ "op": "data", - "topic": "/autonav/camera/compressed", + "topic": "/autonav/camera/compressed/left", "format": msg.format, "data": base64_str })) - def filteredCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw/image"): + def cameraCallbackRight(self, msg: CompressedImage): + if not self.limiter.use("/autonav/camera/compressed/right"): return byts = msg.data.tobytes() @@ -359,7 +364,49 @@ def filteredCallback(self, msg: CompressedImage): self.pushSendQueue(json.dumps({ "op": "data", - "topic": "/autonav/cfg_space/raw/image", + "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"): + return + + byts = msg.data.tobytes() + base64_str = base64.b64encode(byts).decode("utf-8") + + self.pushSendQueue(json.dumps({ + "op": "data", + "topic": "/autonav/cfg_space/raw", "format": msg.format, "data": base64_str })) 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_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..37c6d6ff --- /dev/null +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 + +from types import SimpleNamespace +import rclpy +import json + +from scr.node import Node +from scr.states import DeviceStateEnum +from sensor_msgs.msg import CompressedImage + + +IMAGE_WIDTH = 480 +IMAGE_HEIGHT = 640 + + +class ImageCombinerConfig: + def __init__(self): + self.blah = 0 + + +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/preraw/left", self.image_received_left, 10) + self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/preraw/right", self.image_received_right, 10) + self.combined_image_publisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw", 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 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 + combined_image = CompressedImage() + combined_image.header.stamp = self.get_clock().now().to_msg() + combined_image.format = "jpeg" + combined_image.data = self.image_left.data + self.image_right.data + self.image_left = None + self.image_right = None + self.combined_image_publisher.publish(combined_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() + + +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/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 192beaba..80b1878d 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -49,7 +49,7 @@ def directionify(self, topic): 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/raw"), 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) @@ -138,7 +138,7 @@ def main(): rclpy.init() node_left = ImageTransformer(dir = "left") node_right = ImageTransformer(dir = "right") - Node.run_node([node_left, node_right]) + Node.run_nodes([node_left, node_right]) rclpy.shutdown() diff --git a/display/index.html b/display/index.html index eb296dc7..61407e35 100644 --- a/display/index.html +++ b/display/index.html @@ -122,8 +122,11 @@

Device States

Camera / Filtered

- - + + + + +
diff --git a/display/scripts/main.js b/display/scripts/main.js index a0ed897d..d7711726 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -560,15 +560,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") { + const imgElement = document.getElementById("target_camera_raw"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; return; } From 6d485bf61bad70d12bd00ebdb2571b58e3dcdd37 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 08:44:08 -0500 Subject: [PATCH 04/15] Fix debug topic in autonav display and expandify --- autonav_ws/src/autonav_display/src/display.py | 8 ++-- .../src/autonav_vision/src/combination.py | 45 ++++++++++++++----- .../src/autonav_vision/src/expandify.cpp | 6 +-- .../src/autonav_vision/src/transformations.py | 24 ++++++---- display/scripts/main.js | 2 +- 5 files changed, 57 insertions(+), 28 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index 428dce47..e5e34f80 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -64,7 +64,7 @@ def __init__(self): 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", 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)) @@ -93,7 +93,7 @@ def __init__(self): 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", self.bigboiCallback, 20) + self.bigboiSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/raw/debug", self.bigboiCallback, 20) self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/debug/astar/image", self.debugAStarCallback, 20) self.get_logger().info("Starting event loop") @@ -398,7 +398,7 @@ def filteredCallbackRight(self, msg: CompressedImage): })) def bigboiCallback(self, msg: CompressedImage): - if not self.limiter.use("/autonav/cfg_space/raw"): + if not self.limiter.use("/autonav/cfg_space/raw/debug"): return byts = msg.data.tobytes() @@ -406,7 +406,7 @@ def bigboiCallback(self, msg: CompressedImage): self.pushSendQueue(json.dumps({ "op": "data", - "topic": "/autonav/cfg_space/raw", + "topic": "/autonav/cfg_space/raw/debug", "format": msg.format, "data": base64_str })) diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 37c6d6ff..2d8255ce 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.py @@ -3,19 +3,36 @@ 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 -IMAGE_WIDTH = 480 -IMAGE_HEIGHT = 640 +g_bridge = CvBridge() +g_mapData = MapMetaData() +g_mapData.width = 200 +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.blah = 0 + self.overlap = 0 + self.map_res = 80 class ImageCombiner(Node): @@ -25,9 +42,10 @@ def __init__(self): def init(self): self.image_left = None self.image_right = None - self.image_left_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/preraw/left", self.image_received_left, 10) - self.image_right_subscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/preraw/right", self.image_received_right, 10) - self.combined_image_publisher = self.create_publisher(CompressedImage, "/autonav/cfg_space/raw", 10) + 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): @@ -44,13 +62,18 @@ def try_combine_images(self): # 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 - combined_image = CompressedImage() - combined_image.header.stamp = self.get_clock().now().to_msg() - combined_image.format = "jpeg" - combined_image.data = self.image_left.data + self.image_right.data + 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]) + datamap = cv2.resize(combined, dsize=(self.config.map_res * 2, 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(combined_image) + 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)) diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 9caf181d..61f85883 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -95,10 +95,10 @@ 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 * 2) * config.map_res); std::fill(cfg_space.begin(), cfg_space.end(), 0); - for (int x = 0; x < config.map_res; x++) + for (int x = 0; x < config.map_res * 2; x++) { for (int y = 1; y < config.map_res; y++) { @@ -107,7 +107,7 @@ class ExpandifyNode : public SCR::Node for (Circle &circle : circles) { auto idx = (x + circle.x) + config.map_res * (y + circle.y); - auto expr_x = (x + circle.x) < config.map_res && (x + circle.x) >= 0; + auto expr_x = (x + circle.x) < config.map_res * 2 && (x + circle.x) >= 0; auto expr_y = (y + circle.y) < config.map_res && (y + circle.y) >= 0; if (expr_x && expr_y) { diff --git a/autonav_ws/src/autonav_vision/src/transformations.py b/autonav_ws/src/autonav_vision/src/transformations.py index 80b1878d..508ffeaf 100644 --- a/autonav_ws/src/autonav_vision/src/transformations.py +++ b/autonav_ws/src/autonav_vision/src/transformations.py @@ -78,15 +78,14 @@ def flattenImage(self, img): 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]]) + dest_pts = np.float32([[0, 640], [480, 640], [0, 0], [480, 0]]) matrix = cv2.getPerspectiveTransform(dest_pts, src_pts) - output = cv2.warpPerspective(img, matrix, (640, 480)) + output = cv2.warpPerspective(img, matrix, (480, 640)) 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) @@ -109,11 +108,18 @@ 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 / 2), + (0, height) + ] + else: + region_of_disinterest_vertices = [ + (0, height), + (0, height / 2), + (width, height) + ] # Apply region of disinterest and flattening mask = self.regionOfDisinterest(mask, np.array([region_of_disinterest_vertices], np.int32)) diff --git a/display/scripts/main.js b/display/scripts/main.js index d7711726..770486da 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -586,7 +586,7 @@ $(document).ready(function () { return; } - if (topic == "/autonav/cfg_space/raw") { + if (topic == "/autonav/cfg_space/raw/debug") { const imgElement = document.getElementById("target_camera_raw"); imgElement.src = `data:image/jpeg;base64,${msg.data}`; return; From 02de8d26117a53aea206261a3fd9e3549e4ae4c3 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 09:16:22 -0500 Subject: [PATCH 05/15] Set ROS distro to "humble" in settings. --- .vscode/settings.json | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 From d2f51e35fa23ab366567675fa22be325079edb50 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 12:46:44 -0500 Subject: [PATCH 06/15] Update deviceStateToName function and color styles --- display/scripts/utilities.js | 2 +- display/styles/index.css | 20 +++++++++++++++++--- 2 files changed, 18 insertions(+), 4 deletions(-) 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 { From bae35e63e97080ffd9613fb5d8bfe65e27cf0723 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 12:46:50 -0500 Subject: [PATCH 07/15] Add device_states dictionary to Node class --- autonav_ws/src/scr/scr/node.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 5987e2fd..92935350 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -40,6 +40,7 @@ def __init__(self, node_name): self.device_state = DeviceStateEnum.OFF self.system_state = SystemStateEnum.DISABLED self.system_mode = SystemModeEnum.COMPETITION + self.device_states = {} self.mobility = False self.perf_measurements = {} @@ -66,6 +67,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 From a62432fe86d9468e944b0dbefcb729eceebfb473 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 12:47:00 -0500 Subject: [PATCH 08/15] Add device states to node information on boot --- autonav_ws/src/autonav_display/src/display.py | 6 +++++- display/scripts/main.js | 11 +++++++++++ 2 files changed, 16 insertions(+), 1 deletion(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index e5e34f80..a1442bdd 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -155,9 +155,13 @@ async def consumer(self, websocket): 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 }), unique_id) if obj["op"] == "set_system_state": diff --git a/display/scripts/main.js b/display/scripts/main.js index 770486da..de962380 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -70,6 +70,17 @@ $(document).ready(function () { opcode: 4, iterator: iterate() }); + + const statemap = obj.states; + if (node in statemap) { + deviceStates[node] = statemap[node]; + unorderedListElement = $("#element_device_states"); + unorderedListElement.empty(); + for (const id in deviceStates) { + const state = deviceStates[id]; + unorderedListElement.append(`
${id}: ${deviceStateToName(state)}
`); + } + } } } } From 42d563e0ed0166cbce1dc9251fdb6b50c645197a Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 13:40:20 -0500 Subject: [PATCH 09/15] Fix the configuration system --- autonav_ws/src/autonav_display/src/display.py | 22 +- autonav_ws/src/autonav_filters/src/filters.py | 29 +- autonav_ws/src/autonav_nav/src/astar.py | 10 +- .../src/autonav_nav/src/path_resolver.py | 36 +- autonav_ws/src/scr/scr/node.py | 2 + display/scripts/globals.js | 44 +- display/scripts/main.js | 385 +++++++++--------- 7 files changed, 274 insertions(+), 254 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index a1442bdd..bc319140 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 @@ -74,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) @@ -142,14 +143,12 @@ 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() @@ -161,7 +160,8 @@ async def consumer(self, websocket): self.pushSendQueue(json.dumps({ "op": "get_nodes_callback", "nodes": nodes, - "states": node_states + "states": node_states, + "configs": self.node_configs }), unique_id) if obj["op"] == "set_system_state": diff --git a/autonav_ws/src/autonav_filters/src/filters.py b/autonav_ws/src/autonav_filters/src/filters.py index 3ff6bf5c..27af58bb 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: @@ -86,14 +89,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): - 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_nav/src/astar.py b/autonav_ws/src/autonav_nav/src/astar.py index 3c7c3de7..f64da18e 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/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 92935350..1704f859 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -41,6 +41,7 @@ def __init__(self, node_name): self.system_state = SystemStateEnum.DISABLED self.system_mode = SystemModeEnum.COMPETITION self.device_states = {} + self.node_configs = {} self.mobility = False self.perf_measurements = {} @@ -115,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 diff --git a/display/scripts/globals.js b/display/scripts/globals.js index d5742eba..810b75f8 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,22 @@ 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" + }, + + "autonav_vision_expandifier": { + "internal_title": "[Vision] Expandifier", + "horizontal_fov": "int", + "map_res": "int", + "vertical_fov": "float" }, "autonav_filters": { @@ -66,8 +75,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 +85,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 +100,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 de962380..9f5da35f 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({ @@ -82,6 +83,12 @@ $(document).ready(function () { } } } + + for (const key in obj.configs) + { + config[key] = obj.configs[key]; + } + regenerateConfig(); } } }; @@ -108,10 +115,6 @@ $(document).ready(function () { var sendQueue = []; - function send(obj) { - sendQueue.push(obj); - } - function setSystemState() { send({ op: "set_system_state", @@ -121,148 +124,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); @@ -452,54 +313,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; } @@ -731,4 +545,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] = 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] = 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 From 03bcb64e2ceb2084734c8fc5bd0abda871804e3c Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 12 Mar 2024 13:44:06 -0500 Subject: [PATCH 10/15] Update page title to Danger Zone --- display/index.html | 6 +++--- display/scripts/main.js | 7 ++++++- 2 files changed, 9 insertions(+), 4 deletions(-) diff --git a/display/index.html b/display/index.html index 61407e35..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

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

System

-

Waiting for the Weeb Wagon

+

Waiting for the Danger Zone

diff --git a/display/scripts/main.js b/display/scripts/main.js index 9f5da35f..c0c87708 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -74,6 +74,11 @@ $(document).ready(function () { 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(); @@ -94,7 +99,7 @@ $(document).ready(function () { }; 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(); From 8dfd84b4b8ef78d7dbf01a36070291aec0de2f15 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Thu, 28 Mar 2024 19:59:26 -0500 Subject: [PATCH 11/15] Add image_warp_tl and image_warp_tr to globals.js and transform.py --- autonav_ws/src/autonav_display/src/display.py | 2 +- .../src/autonav_vision/src/combination.py | 60 ++++++++++++++++++- .../src/autonav_vision/src/expandify.cpp | 10 ++-- .../src/autonav_vision/src/transformations.py | 24 ++------ display/scripts/globals.js | 4 +- display/scripts/main.js | 4 +- 6 files changed, 75 insertions(+), 29 deletions(-) diff --git a/autonav_ws/src/autonav_display/src/display.py b/autonav_ws/src/autonav_display/src/display.py index bc319140..5ede35a5 100644 --- a/autonav_ws/src/autonav_display/src/display.py +++ b/autonav_ws/src/autonav_display/src/display.py @@ -95,7 +95,7 @@ def __init__(self): 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/debug/astar/image", self.debugAStarCallback, 20) + self.debugAStarSubscriber = self.create_subscription(CompressedImage, "/autonav/cfg_space/expanded/image", self.debugAStarCallback, 20) self.get_logger().info("Starting event loop") diff --git a/autonav_ws/src/autonav_vision/src/combination.py b/autonav_ws/src/autonav_vision/src/combination.py index 2d8255ce..527f256a 100644 --- a/autonav_ws/src/autonav_vision/src/combination.py +++ b/autonav_ws/src/autonav_vision/src/combination.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() @@ -56,6 +56,60 @@ 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 @@ -65,7 +119,9 @@ def try_combine_images(self): 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]) - datamap = cv2.resize(combined, dsize=(self.config.map_res * 2, self.config.map_res), interpolation=cv2.INTER_LINEAR) / 2 + 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) diff --git a/autonav_ws/src/autonav_vision/src/expandify.cpp b/autonav_ws/src/autonav_vision/src/expandify.cpp index 61f85883..89456876 100644 --- a/autonav_ws/src/autonav_vision/src/expandify.cpp +++ b/autonav_ws/src/autonav_vision/src/expandify.cpp @@ -95,10 +95,10 @@ class ExpandifyNode : public SCR::Node return; } - std::vector cfg_space = std::vector((config.map_res * 2) * 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 * 2; x++) + for (int x = 0; x < config.map_res; x++) { for (int y = 1; y < config.map_res; y++) { @@ -107,7 +107,7 @@ class ExpandifyNode : public SCR::Node for (Circle &circle : circles) { auto idx = (x + circle.x) + config.map_res * (y + circle.y); - auto expr_x = (x + circle.x) < config.map_res * 2 && (x + circle.x) >= 0; + auto expr_x = (x + circle.x) < config.map_res && (x + circle.x) >= 0; auto expr_y = (y + circle.y) < config.map_res && (y + circle.y) >= 0; if (expr_x && expr_y) { @@ -151,8 +151,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 508ffeaf..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,6 +37,8 @@ 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): @@ -71,19 +73,6 @@ 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, 640], [480, 640], [0, 0], [480, 0]]) - - matrix = cv2.getPerspectiveTransform(dest_pts, src_pts) - output = cv2.warpPerspective(img, matrix, (480, 640)) - return output - def publishOccupancyMap(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)) @@ -111,27 +100,26 @@ def onImageReceived(self, image = CompressedImage): if self.dir == "left": region_of_disinterest_vertices = [ (width, height), - (width, height / 2), + (width, height / 1.8), (0, height) ] else: region_of_disinterest_vertices = [ (0, height), - (0, height / 2), + (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" diff --git a/display/scripts/globals.js b/display/scripts/globals.js index 810b75f8..1052e765 100644 --- a/display/scripts/globals.js +++ b/display/scripts/globals.js @@ -43,7 +43,9 @@ var addressKeys = { "blur_weight": "int", "blur_iterations": "int", "rod_offset": "int", - "map_res": "int" + "map_res": "int", + "image_warp_tl": "float", + "image_warp_tr": "float" }, "autonav_vision_expandifier": { diff --git a/display/scripts/main.js b/display/scripts/main.js index c0c87708..d16495e7 100644 --- a/display/scripts/main.js +++ b/display/scripts/main.js @@ -602,7 +602,7 @@ $(document).ready(function () { input.classList.add("form-control"); input.value = data; input.onchange = function () { - config[device][text] = input.value; + config[device][text] = parseFloat(input.value); send({ op: "configuration", device: device, @@ -628,7 +628,7 @@ $(document).ready(function () { input.classList.add("form-control"); input.value = data; input.onchange = function () { - config[device][text] = input.value; + config[device][text] = parseInt(input.value); send({ op: "configuration", device: device, From 649cf79a27324bf7777fe2f71f6e23a0c4663768 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 9 Apr 2024 17:22:39 -0500 Subject: [PATCH 12/15] Rename toString functions and remove unneeded logs --- autonav_ws/src/scr/include/scr/states.hpp | 6 +++--- autonav_ws/src/scr/scr/node.py | 2 -- autonav_ws/src/scr/src/node.cpp | 8 +------- autonav_ws/src/scr_controller/src/core.cpp | 2 -- 4 files changed, 4 insertions(+), 14 deletions(-) diff --git a/autonav_ws/src/scr/include/scr/states.hpp b/autonav_ws/src/scr/include/scr/states.hpp index 269d8149..ef87c9b6 100644 --- a/autonav_ws/src/scr/include/scr/states.hpp +++ b/autonav_ws/src/scr/include/scr/states.hpp @@ -14,7 +14,7 @@ namespace SCR ERRORED = 5 }; - std::string toString(DeviceState state) + std::string deviceStateToString(DeviceState state) { switch (state) { @@ -42,7 +42,7 @@ namespace SCR SHUTDOWN = 3 }; - std::string toString(SystemState state) + std::string systemStateToString(SystemState state) { switch (state) { @@ -65,7 +65,7 @@ namespace SCR PRACTICE = 2, }; - std::string toString(SystemMode state) + std::string systemModeToString(SystemMode state) { switch (state) { diff --git a/autonav_ws/src/scr/scr/node.py b/autonav_ws/src/scr/scr/node.py index 1704f859..3f82247c 100644 --- a/autonav_ws/src/scr/scr/node.py +++ b/autonav_ws/src/scr/scr/node.py @@ -148,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): @@ -156,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): diff --git a/autonav_ws/src/scr/src/node.cpp b/autonav_ws/src/scr/src/node.cpp index 9f4cf94c..8124c5fd 100644 --- a/autonav_ws/src/scr/src/node.cpp +++ b/autonav_ws/src/scr/src/node.cpp @@ -50,10 +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(), "MOBILITY CHANGE: %d -> %d", oldState.mobility, msg.mobility); - system_state_transition(oldState, msg); system_mode = static_cast(msg.mode); @@ -76,8 +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()); - if (device_state == SCR::DeviceState::BOOTING) { // Get the default config and push it to the server @@ -166,7 +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: %s", SCR::deviceStateToString(state).c_str()); } } diff --git a/autonav_ws/src/scr_controller/src/core.cpp b/autonav_ws/src/scr_controller/src/core.cpp index 0c39c823..dbc343d7 100644 --- a/autonav_ws/src/scr_controller/src/core.cpp +++ b/autonav_ws/src/scr_controller/src/core.cpp @@ -95,7 +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()); 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 @@ -156,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; From 5eb0a6d1f74e2231766ea42fa9b8e2cbe8abd796 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 9 Apr 2024 17:55:55 -0500 Subject: [PATCH 13/15] Start manual upgrade --- .../autonav_launch/launch/managed_steam.xml | 1 + .../src/remote_steamcontroller.cpp | 26 +++++++++++++++---- autonav_ws/src/autonav_serial/src/camera.py | 3 +-- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 8e17dd93..763ffdb2 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -20,4 +20,5 @@ + \ 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..55d2bd41 100644 --- a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp +++ b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp @@ -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,25 @@ 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; } if (abs(msg.lpad_x) > steeringDeadzone) { - steering = msg.lpad_x; + target_steering = msg.lpad_x; } + // Generate a forward/angular velocity command that ramps up/down smoothly using a linear filter + const float throttleRate = 0.1; + const float steeringRate = 0.1; + current_throttle = lerp(current_throttle, target_throttle, throttleRate); + current_steering = lerp(current_steering, target_steering, steeringRate); + 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.forward_speed, -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 +116,10 @@ class SteamJoyNode : public SCR::Node private: SteamJoyNodeConfig config; + float target_throttle = 0; + float target_steering = 0; + float current_throttle = 0; + float current_steering = 0; }; int main(int argc, char *argv[]) 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() From 9d1dd5f01e8c013b3aac9ff1dec0269ee220e704 Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 9 Apr 2024 18:08:36 -0500 Subject: [PATCH 14/15] Fix managed_steam file --- .../autonav_launch/launch/managed_steam.xml | 23 +++++++------------ 1 file changed, 8 insertions(+), 15 deletions(-) diff --git a/autonav_ws/src/autonav_launch/launch/managed_steam.xml b/autonav_ws/src/autonav_launch/launch/managed_steam.xml index 763ffdb2..5b1cfed5 100644 --- a/autonav_ws/src/autonav_launch/launch/managed_steam.xml +++ b/autonav_ws/src/autonav_launch/launch/managed_steam.xml @@ -1,24 +1,17 @@ - - + + + - - - - - - - - - - - - + + + + + - \ No newline at end of file From 99a8b5aaeff2a82aa821995240f551e64dfa585b Mon Sep 17 00:00:00 2001 From: Dylan Zemlin Date: Tue, 9 Apr 2024 18:18:40 -0500 Subject: [PATCH 15/15] Update throttle and steering deadzones --- .../src/remote_steamcontroller.cpp | 31 ++++++++++++++----- autonav_ws/src/autonav_manual/src/steam.py | 3 ++ 2 files changed, 26 insertions(+), 8 deletions(-) diff --git a/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp b/autonav_ws/src/autonav_manual/src/remote_steamcontroller.cpp index 55d2bd41..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; @@ -88,23 +88,37 @@ class SteamJoyNode : public SCR::Node if (abs(msg.ltrig) > throttleDeadzone || abs(msg.rtrig) > throttleDeadzone) { target_throttle = msg.rtrig - msg.ltrig; + is_working = true; } if (abs(msg.lpad_x) > steeringDeadzone) { target_steering = msg.lpad_x; + is_working = true; } - // Generate a forward/angular velocity command that ramps up/down smoothly using a linear filter - const float throttleRate = 0.1; - const float steeringRate = 0.1; - current_throttle = lerp(current_throttle, target_throttle, throttleRate); - current_steering = lerp(current_steering, target_steering, steeringRate); + 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(current_throttle * config.forward_speed, -config.max_forward_speed, config.max_forward_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); } @@ -120,6 +134,7 @@ class SteamJoyNode : public SCR::Node 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