Develop strat 7 (#32)

Co-authored-by: modelec <modelec-isen@gmail.com>

Author: Ackimixs

Desktop/cam.desktop

@@ -0,0 +1,8 @@
+[Desktop Entry]
+Type=Application
+Name=Cam
+Comment=Cam
+Exec=/home/modelec/Modelec-ROS2/start_cam_ros2.sh
+Icon=utilities-terminal
+Terminal=true
+Categories=Utility;

build.rasp.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
-MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug
+MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential --cmake-args -DBUILD_FOR_RPI=ON --no-warn-unused-cli
 
 source install/setup.bash

install.sh

@@ -21,7 +21,7 @@ sudo apt upgrade -y
 
 sudo apt install ros-dev-tools ros-jazzy-desktop -y
 
-sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y
+sudo apt-get install qt6-base-dev qt6-svg-dev qt6-multimedia-dev libxml2-dev socat -y
 
 git submodule init
 git submodule update
@@ -45,4 +45,4 @@ gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" tr
 gio set ~/Desktop/no_lidar.ros2_launch_marcel.desktop "metadata::trusted" true
 gio set ~/Desktop/joy.ros2_launch_marcel.desktop "metadata::trusted" true
 gio set ~/Desktop/ros2_launch_marcel.desktop "metadata::trusted" true
-gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true
+gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true

simulated_pcb/serial.modelec.py

@@ -35,16 +35,19 @@ def read_serial(ser, log_lines, log_lock):
                     log_lines.append(f"[Serial Error] {e}")
         time.sleep(0.05)
 
-def curses_main(stdscr, port, baudrate):
+def curses_main(stdscr, port, baudrate, filter_start=None):
     global stop_thread
     curses.curs_set(1)
     stdscr.nodelay(True)
     stdscr.timeout(100)
 
+    if filter_start is None:
+        filter_start = ["SET;POS"]
+
     log_lines = []
     log_lock = threading.Lock()
     cmd_history = []
-    history_index = -1  # For navigating command history
+    history_index = -1
 
     with serial.Serial(port, baudrate, timeout=1) as ser:
         time.sleep(2)
@@ -57,24 +60,36 @@ def curses_main(stdscr, port, baudrate):
             stdscr.clear()
             h, w = stdscr.getmaxyx()
 
-            # Split screen horizontally (70% logs, 30% command history)
-            split_x = int(w * 0.7)
+            # --- Split into 3 vertical columns ---
+            col_width = w // 3
+            col1_x = 0
+            col2_x = col_width
+            col3_x = 2 * col_width
+
             log_height = h - 2
 
-            # --- Left panel: logs ---
+            # --- Separate logs into filtered and others ---
             with log_lock:
-                visible_logs = log_lines[-log_height:]
-            for i, line in enumerate(visible_logs):
-                stdscr.addnstr(i, 0, line, split_x - 1)
-
-            # --- Right panel: command history ---
-            stdscr.vline(0, split_x, "|", log_height)
-            history_start_x = split_x + 2
-            stdscr.addstr(0, history_start_x, "Command History:")
-
-            visible_history = cmd_history[-(log_height - 2):]
+                filtered_logs = [line for line in log_lines if any(line.startswith(p) for p in filter_start)]
+                other_logs = [line for line in log_lines if not any(line.startswith(p) for p in filter_start)]
+                visible_filtered = filtered_logs[-log_height:]
+                visible_other = other_logs[-log_height:]
+                visible_history = cmd_history[-log_height:]
+
+            # --- Column 1: Filtered serial output ---
+            stdscr.addstr(0, col1_x, f"Filtered ({', '.join(filter_start)}):")
+            for i, line in enumerate(visible_filtered):
+                stdscr.addnstr(i + 1, col1_x, line, col_width - 1)
+
+            # --- Column 2: Other serial output ---
+            stdscr.addstr(0, col2_x, "Other Logs:")
+            for i, line in enumerate(visible_other):
+                stdscr.addnstr(i + 1, col2_x, line, col_width - 1)
+
+            # --- Column 3: Command history ---
+            stdscr.addstr(0, col3_x, "Command History:")
             for i, cmd in enumerate(visible_history):
-                stdscr.addnstr(i + 1, history_start_x, cmd, w - history_start_x - 1)
+                stdscr.addnstr(i + 1, col3_x, cmd, col_width - 1)
 
             # --- Input line ---
             stdscr.addstr(log_height, 0, "-" * (w - 1))

src/modelec_com/CMakeLists.txt

@@ -5,6 +5,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
+option(BUILD_FOR_RPI "Build with Raspberry Pi libcamera support" OFF)
+
 # find dependencies
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
@@ -16,6 +18,18 @@ find_package(ament_index_cpp REQUIRED)
 find_package(fmt)
 
 find_package(OpenCV REQUIRED)
+find_package(PkgConfig REQUIRED)
+
+if(BUILD_FOR_RPI)
+  message(STATUS "Configuring for Raspberry Pi Hardware...")
+  add_definitions(-DRPI_BUILD)
+  pkg_check_modules(LIBCAMERA REQUIRED libcamera libcamera-base)
+  include_directories(${LIBCAMERA_INCLUDE_DIRS})
+  link_directories(${LIBCAMERA_LIBRARY_DIRS} /usr/local/lib/aarch64-linux-gnu)
+  add_definitions(${LIBCAMERA_CFLAGS_OTHER})
+else()
+  message(STATUS "Configuring for PC (standard OpenCV)...")
+endif()
 
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
@@ -57,6 +71,16 @@ target_link_libraries(color_detector
   modelec_utils::config
 )
 
+if(BUILD_FOR_RPI)
+  target_link_libraries(color_detector
+          -Wl,--no-as-needed
+          "/usr/local/lib/aarch64-linux-gnu/libcamera.so"
+          "/usr/local/lib/aarch64-linux-gnu/libcamera-base.so"
+          -Wl,--as-needed
+          cam2opencv
+  )
+endif()
+
 target_include_directories(color_detector PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>

src/modelec_com/include/modelec_com/color_detector.hpp

@@ -5,9 +5,18 @@
 #include <std_srvs/srv/trigger.hpp>
 #include <std_msgs/msg/empty.hpp>
 #include <std_msgs/msg/string.hpp>
+#include <modelec_utils/config.hpp>
+#include <mutex>
+#include <memory>
+
+#ifdef RPI_BUILD
+#include <libcam2opencv.h>
+#endif
 
 namespace Modelec
 {
+    struct CamCallback;
+
     struct ColorSetting
     {
         std::string name;
@@ -34,6 +43,17 @@ namespace Modelec
 
         std::string generateImagePath() const;
 
+        #ifdef RPI_BUILD
+                Libcam2OpenCV camera_;
+                std::unique_ptr<CamCallback> my_callback_;
+        #else
+                cv::VideoCapture pc_cap_;
+        #endif
+
+        cv::Mat latest_frame_;
+
+        std::mutex frame_mutex_;
+
         rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_;
 
         rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr ask_sub_;
@@ -49,4 +69,45 @@ namespace Modelec
 
         std::vector<ColorSetting> color_configs_;
     };
+
+    template<>
+    inline std::vector<ColorSetting>
+    Config::get<std::vector<ColorSetting>>(
+        const std::string& prefix,
+        const std::vector<ColorSetting>& default_value, bool)
+    {
+        auto result = Config::getArray<ColorSetting>(
+            prefix,
+            [](const std::string& base)
+            {
+                return ColorSetting{
+                    Config::get<std::string>(base + "@name"),
+                    Config::get<double>(base + "@hue_min"),
+                    Config::get<double>(base + "@hue_max")
+                };
+            });
+
+        return result.empty() ? default_value : result;
+    }
+
+    template<>
+    inline std::vector<cv::Rect>
+    Config::get<std::vector<cv::Rect>>(
+        const std::string& prefix,
+        const std::vector<cv::Rect>& default_value, bool)
+    {
+        auto result = Config::getArray<cv::Rect>(
+            prefix,
+            [](const std::string& base)
+            {
+                return cv::Rect(
+                    Config::get<int>(base + "@x", 0),
+                    Config::get<int>(base + "@y", 0),
+                    Config::get<int>(base + "@w", 100),
+                    Config::get<int>(base + "@h", 100)
+                );
+            });
+
+        return result.empty() ? default_value : result;
+    }
 }

src/modelec_com/include/modelec_com/pcb_action_interface.hpp

@@ -12,6 +12,8 @@
 #include <modelec_interfaces/msg/action_servo_timed_array.hpp>
 #include <modelec_interfaces/msg/action_servo_timed.hpp>
 
+#include "modelec_utils/config.hpp"
+
 #define TIMER_SERVO_TIMED_MS 10 // 100 Hz
 
 namespace Modelec
@@ -37,10 +39,10 @@ namespace Modelec
         std::map<int, int> asc_value_mapper_;
 
         int asc_state_ = 0;
-        std::map<int, double> servo_value_;
-        std::map<int, bool> relay_value_;
+        std::unordered_map<int, double> servo_value_;
+        std::unordered_map<int, bool> relay_value_;
 
-        std::map<int, ServoTimedSet> servo_timed_buffer_;
+        std::vector<ServoTimedSet> servo_timed_buffer_;
 
         rclcpp::TimerBase::SharedPtr servo_timed_timer_;
 
@@ -82,10 +84,29 @@ namespace Modelec
         void SendToPCB(const std::string& order, const std::string& elem,
                        const std::vector<std::string>& data = {});
 
-        // TODO redo thos func to accept arrays, poc without them atm
         void GetData(const std::string& elem, const std::vector<std::string>& data = {});
         void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
         void SendMove(const std::string& elem, const std::vector<std::string>& data = {});
         void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {});
     };
+
+    template<>
+    inline std::unordered_map<int, double>
+    Config::get<std::unordered_map<int, double>>(
+        const std::string& prefix,
+        const std::unordered_map<int, double>& default_value, bool)
+    {
+        auto result = Config::getMap<int, double>(
+            prefix,
+            [](const std::string& base)
+            {
+                return std::make_pair(
+                    Config::get<int>(base + "@id"),
+                    Config::get<double>(base + "@angle")
+                );
+            });
+
+        return result.empty() ? default_value : result;
+    }
+
 }

src/modelec_com/include/modelec_com/pcb_odo_interface.hpp

@@ -19,6 +19,8 @@
 
 #include <sensor_msgs/msg/joy.hpp>
 
+#include "modelec_utils/config.hpp"
+
 namespace Modelec
 {
     class PCBOdoInterface : public rclcpp::Node, public SerialListener
@@ -37,9 +39,11 @@ namespace Modelec
 
         struct PIDData
         {
-            float p;
-            float i;
-            float d;
+            float p = 0.0f;
+            float i = 0.0f;
+            float d = 0.0f;
+            std::optional<float> min;
+            std::optional<float> max;
         };
 
     private:
@@ -95,7 +99,26 @@ namespace Modelec
         void GetPID();
         void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
         void SetPID(std::string name, float p, float i, float d, std::optional<float> min = std::nullopt, std::optional<float> max = std::nullopt);
+        void SetPID(std::string name, const PIDData& pid_data);
 
         void SetMotor(int left, int right);
     };
+
+    template<>
+    inline PCBOdoInterface::PIDData
+    Config::get<PCBOdoInterface::PIDData>(
+        const std::string& prefix,
+        const PCBOdoInterface::PIDData& default_value, bool)
+    {
+        PCBOdoInterface::PIDData result;
+        result.p = Config::get<float>(prefix + "@p", default_value.p);
+        result.i = Config::get<float>(prefix + "@i", default_value.i);
+        result.d = Config::get<float>(prefix + "@d", default_value.d);
+        if (Config::has(prefix + "@min"))
+            result.min = Config::get<float>(prefix + "@min");
+        if (Config::has(prefix + "@max"))
+            result.max = Config::get<float>(prefix + "@max");
+        return result;
+    }
+
 } // namespace Modelec