Feat/feelers

.gitignore

@@ -21,3 +21,9 @@ Zone.Identifier
 
 deps/songs
 autonav_ws/src/scr/include/scr/websocketpp/**/*:Zone.Identifier
+
+# python
+**/venv
+
+# temporary feeler node testing
+autonav_ws/data

Dockerfile

@@ -1,16 +1,17 @@
 FROM osrf/ros:jazzy-desktop-full
 
 RUN mkdir -p /autonav/autonav_ws/src
-# RUN mkdir -p /autonav/setup
+RUN mkdir -p /autonav/setup
 # RUN mkdir -p /autonav/deps
 
 COPY autonav_ws/src /autonav/autonav_ws/src
-# COPY setup /autonav/setup
-# COPY vectorsecrets.txt /autonav/setup/vectorsecrets.txt
+COPY setup /autonav/setup
+COPY vectorsecrets.txt /autonav/setup/vectorsecrets.txt
 # COPY deps /autonav/deps
 
-# WORKDIR /autonav/setup
-# RUN /bin/bash -c "./setup.sh"
+WORKDIR /autonav/setup
+RUN /bin/bash -c "sudo chmod +x ./setup.sh"
+RUN /bin/bash -c "./setup.sh"
 
 WORKDIR /autonav/autonav_ws
 RUN /bin/bash -c "source /opt/ros/jazzy/setup.bash; colcon build"

autonav_ws/src/autonav_example_cpp/src/example.cpp

@@ -1,10 +1,36 @@
 #include "autonav_shared/node.hpp"
 
+
+struct ExampleCPPConfig
+{
+    float alpha;
+    std::string beta;
+    int gamma;
+    bool delta;
+    std::vector<float> epsilon;
+    std::vector<std::string> zeta;
+    std::map<std::string, std::string> eta;
+    int rid;
+
+    NLOHMANN_DEFINE_TYPE_INTRUSIVE(ExampleCPPConfig, alpha, beta, gamma, delta, epsilon, zeta)
+};
+
 class ExampleCPP : public AutoNav::Node
 {
     public:
         ExampleCPP() : AutoNav::Node("example_cpp") 
         {
+            auto config = ExampleCPPConfig();
+            config.alpha = 0.5;
+            config.beta = "Hello";
+            config.gamma = 42;
+            config.delta = true;
+            config.epsilon = {0.1, 0.2, 0.3};
+            config.zeta = {"A", "B", "C"};
+            config.eta = {{"A", "1"}, {"B", "2"}, {"C", "3"}};
+            config.rid = 1;
+            write_config(config);
+
             log("Hello from ExampleCPP", AutoNav::Logging::DEBUG);
             log("Hello from ExampleCPP", AutoNav::Logging::INFO);
             log("Hello from ExampleCPP", AutoNav::Logging::WARN);
@@ -20,6 +46,10 @@ class ExampleCPP : public AutoNav::Node
 
             perf_start("ExampleCPP::init");
             perf_stop("ExampleCPP::init", true);
+
+            request_all_configs();
+
+            log("Beta (CONFIG): " + config["beta"].get<std::string>(), AutoNav::Logging::DEBUG);
         }
 };
 

autonav_ws/src/autonav_example_py/src/example.py

@@ -3,12 +3,24 @@
 from autonav_shared.node import Node
 from autonav_shared.types import LogLevel, DeviceState, SystemState
 import rclpy
-import time
 
 
-class Example(Node):
+class ExamplePyConfig:
+    def __init__(self):
+        self.alpha = 0.5
+        self.beta = "Hello"
+        self.gamma = 42
+        self.delta = True
+        self.epsilon = [0.1, 0.2, 0.3]
+        self.zeta = ["A", "B", "C"]
+        self.eta = {"A": 1, "A": 2, "A": 3}
+        self.rid = 2
+
+
+class ExamplePy(Node):
     def __init__(self):
         super().__init__("autonav_example_py")
+        self.write_config(ExamplePyConfig())
 
         self.log("Hello from ExamplePy", LogLevel.DEBUG)
         self.log("Hello from ExamplePy", LogLevel.INFO)
@@ -22,10 +34,14 @@ def init(self):
 
         self.perf_start("example")
         self.perf_stop("example", True)
+
+        self.request_all_configs()
+
+        self.log(f"Beta (CONFIG): {self.config.beta}", LogLevel.DEBUG)
 
 def main():
     rclpy.init()
-    example = Example()
+    example = ExamplePy()
     rclpy.spin(example)
     rclpy.shutdown()
 

autonav_ws/src/autonav_feelers/CMakeLists.txt

@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(autonav_feelers)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclpy REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(autonav_msgs REQUIRED)
+find_package(autonav_shared REQUIRED)
+find_package(cv_bridge REQUIRED)
+find_package(image_transport REQUIRED)
+find_package(OpenCV REQUIRED)
+
+# Install Python programs
+install(PROGRAMS
+  src/fake_vision_node.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# C++
+add_executable(feeler_node src/feeler_node.cpp)
+ament_target_dependencies(feeler_node rclcpp autonav_msgs cv_bridge image_transport autonav_shared OpenCV)
+
+install(TARGETS
+  feeler_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+ament_package()

autonav_ws/src/autonav_feelers/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autonav_feelers</name>
+  <version>2025.0.1</version>
+  <description>Package to take all of the obstacle and GPS data and output commands to move the robot to avoid obstacles and hit GPS waypoints using SCR FEELERS (C) TM.</description>
+  <maintainer email="[email protected]">Daniel Brown</maintainer>
+  <license>MIT</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>autonav_msgs</depend>
+  <depend>autonav_shared</depend>
+  <depend>cv_bridge</depend>
+  <depend>image_transport</depend>
+
+  <build_depend>rosidl_default_generators</build_depend>  
+  <exec_depend>rosidl_default_runtime</exec_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

autonav_ws/src/autonav_feelers/src/fake_vision_node.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python3
+
+import rclpy
+import cv2, time
+import numpy as np
+
+import rclpy.qos
+from sensor_msgs.msg import CompressedImage
+from cv_bridge import CvBridge
+
+from autonav_shared.node import Node
+from autonav_shared.types import LogLevel, DeviceState, SystemState
+
+
+
+bridge = CvBridge()
+
+IMAGE_WIDTH = 640
+IMAGE_HEIGHT = 480
+
+#TODO use like max() or min() in case we play with the dimensions
+# or like, combine them better so the total combined image is only 640x480 or something
+COMBINED_IMAGE_WIDTH = IMAGE_HEIGHT*2 # left and right cameras are 480 pixels tall but turned on their sides so it's 480 pixels wide and then next to each other, and the top/bottom cameras are only 640 wide so this is fine
+COMBINED_IMAGE_HEIGHT = IMAGE_HEIGHT*2 + IMAGE_WIDTH # top and bottom cameras are 480 pixels tall, plus the left/right cameras turned on their side which adds 640 pixels
+
+
+class ImageCombiner(Node):
+    def __init__(self):
+        super().__init__("autonav_vision_combiner")
+
+    def init(self):
+        self.set_device_state(DeviceState.WARMING)
+
+        # subscribers
+        self.feeler_debug_image_subscriber = self.create_subscription(CompressedImage, "/autonav/feelers/debug", self.on_feelers_received, 1) # TEMP TODO FIXME
+        self.feeler_debug_image = None
+
+        self.feeler_video_writer = cv2.VideoWriter("./data/debug_feeler_test.mp4", cv2.VideoWriter.fourcc(*"mp4v"), 15.0, (COMBINED_IMAGE_WIDTH, COMBINED_IMAGE_HEIGHT)) #TODO
+
+        # publishers
+        self.combined_image_publisher = self.create_publisher(CompressedImage, "/autonav/vision/combined/filtered", 1)
+        self.video_reader = cv2.VideoCapture("./data/combined.mp4")
+
+        self.combined_debug_image_publisher = self.create_publisher(CompressedImage, "/autonav/vision/combined/debug", 1)
+        self.debug_video_reader = cv2.VideoCapture("./data/debug_combined.mp4")
+
+        #TODO make timers
+        self.publish_timer = self.create_timer(.1, self.publish_combined_images)
+
+        self.frame = 0
+        self.writtenFrames = 0
+
+        self.log("starting image combiner...", LogLevel.WARN)
+        self.set_device_state(DeviceState.READY)
+
+
+    # just write that straight to output
+    def on_feelers_received(self, msg):
+        self.writtenFrames += 1
+        print(f"Receiving frame: {self.writtenFrames}")
+        self.feeler_video_writer.write(bridge.compressed_imgmsg_to_cv2(msg))
+
+    def publish_combined_images(self):
+        print(f"Publishing frame: {self.frame}")
+
+        ret1, combined_image = self.video_reader.read()
+        ret2, debug_image = self.debug_video_reader.read()
+
+        # if not ret1 or not ret2 or self.frame > 3:
+        if not ret1 or not ret2:
+            self.log("OUT OF IMAGES", LogLevel.FATAL)
+
+            self.video_reader.release()
+            self.debug_video_reader.release()
+            self.feeler_video_writer.release()
+
+            raise SystemExit
+
+        self.frame += 1
+
+        self.combined_image_publisher.publish(bridge.cv2_to_compressed_imgmsg(combined_image))
+        self.combined_debug_image_publisher.publish(bridge.cv2_to_compressed_imgmsg(debug_image)) #FIXME there's got to be a better way to handle this feelers-side, but for now just send this first so that feelers can draw on it before it gets overwritten
+
+
+def main():
+    rclpy.init()
+
+    node = ImageCombiner()
+    rclpy.spin(node)
+
+    node.feeler_video_writer.release()
+    cv2.destroyAllWindows() # just in case
+
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()