Lidar + new action + rewrite action executor + cpu usage (gui & enemy manager)

Desktop/no-gui.ros2_launch_marcel.desktop

@@ -0,0 +1,8 @@
+[Desktop Entry]
+Type=Application
+Name=No GUI
+Comment=Lance le système ROS 2 Sans GUI
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_gui:=false
+Icon=utilities-terminal
+Terminal=true
+Categories=Utility;

no_lidar.joy.ros2_launch_marcel.desktop → Desktop/no_lidar.joy.ros2_launch_marcel.desktop

@@ -2,7 +2,7 @@
 Type=Application
 Name=Joy no Lidar
 Comment=Lance le système ROS 2 avec GUI
-Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_rplidar:=false with_enemy_manager:=false
 Icon=utilities-terminal
 Terminal=true
 Categories=Utility;

no_lidar.ros2_launch_marcel.desktop → Desktop/no_lidar.ros2_launch_marcel.desktop

@@ -2,9 +2,7 @@
 Type=Application
 Name=No Lidar
 Comment=Lance le système ROS 2 avec GUI
-
-Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false
-
+Exec=/home/modelec/Modelec-ROS2/start_ros2.sh with_joy:=false with_rplidar:=false with_enemy_manager:=false
 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_EXPORT_COMPILE_COMMANDS=ON # -DCMAKE_BUILD_TYPE=Debug
+MAKEFLAGS="-j1" colcon build --symlink-install --executor sequential # --cmake-args -DCMAKE_BUILD_TYPE=Debug
 
 source install/setup.bash

build.sh

@@ -1,4 +1,4 @@
 #!/bin/bash
-colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
+colcon build --symlink-install --cmake-args # -DCMAKE_EXPORT_COMPILE_COMMANDS=ON
 
 source install/setup.bash

fastdds_setup.xml

@@ -28,7 +28,7 @@
                     </locator>
                     <locator>
                         <udpv4>
-                            <address>100.73.69.106</address>
+                            <address>100.91.204.77</address>
                         </udpv4>
                     </locator>
                 </initialPeersList>

install.sh

@@ -27,9 +27,9 @@ git submodule init
 git submodule update
 
 echo "source /opt/ros/jazzy/setup.bash
-source ~/Modelec-ROS/install/setup.bash
+source ~/Modelec-ROS2/install/setup.bash
 export RMW_IMPLEMENTATION=rmw_fastrtps_cpp
-export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS/fastdds_setup.xml
+export FASTRTPS_DEFAULT_PROFILES_FILE=~/Modelec-ROS2/fastdds_setup.xml
 export ROS_DOMAIN_ID=128" >> ~/.bashrc
 
 source ~/.bashrc
@@ -39,9 +39,10 @@ source src/rplidar_ros/scripts/create_udev_rules.sh
 
 cd ../..
 
-cp ./*.desktop ~/Desktop
+cp ./Desktop/*.desktop ~/Desktop
 chmod +x ~/Desktop/*.desktop
 gio set ~/Desktop/no_lidar.joy.ros2_launch_marcel.desktop "metadata::trusted" true
 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/ros2_launch_marcel.desktop "metadata::trusted" true
+gio set ~/Desktop/no-gui.ros2_launch_marcel.desktop "metadata::trusted" true

simulated_pcb/odo.py

@@ -125,6 +125,9 @@ def handle_message(self, msg):
 
         elif msg.startswith("SET;START;"):
             self.start = msg.split(';')[2] == '1'
+            if not self.start:
+                self.waypoints.clear()
+                self.waypoint_queue.clear()
             self.ser.write(b"OK;START;1\n")
 
         # --- MULTI WAYPOINT (clears previous) ---

src/modelec_com/CMakeLists.txt

@@ -10,10 +10,13 @@ find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(Boost REQUIRED COMPONENTS system)
 find_package(ament_index_cpp REQUIRED)
 find_package(fmt)
 
+find_package(OpenCV REQUIRED)
+
 find_package(modelec_interfaces REQUIRED)
 find_package(modelec_utils REQUIRED)
 
@@ -35,10 +38,35 @@ target_include_directories(pcb_action_interface PUBLIC
         $<INSTALL_INTERFACE:include>
 )
 
+# Color Detector Node
+add_executable(color_detector
+  src/color_detector.cpp
+)
+
+ament_target_dependencies(color_detector
+  rclcpp
+  std_msgs
+  std_srvs
+  modelec_interfaces
+  ament_index_cpp
+)
+
+target_link_libraries(color_detector
+  ${OpenCV_LIBS}
+  modelec_utils::utils
+  modelec_utils::config
+)
+
+target_include_directories(color_detector PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
 # Install targets
 install(TARGETS
         pcb_odo_interface
         pcb_action_interface
+        color_detector
         DESTINATION lib/${PROJECT_NAME}
 )
 

src/modelec_com/include/modelec_com/color_detector.hpp

@@ -0,0 +1,95 @@
+#pragma once
+
+#include <rclcpp/rclcpp.hpp>
+#include <opencv2/opencv.hpp>
+#include <std_srvs/srv/trigger.hpp>
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include <modelec_utils/config.hpp>
+
+namespace Modelec
+{
+    struct ColorSetting
+    {
+        std::string name;
+        double h_min;
+        double h_max;
+    };
+
+    class ColorDetector : public rclcpp::Node
+    {
+    public:
+        ColorDetector();
+
+        ~ColorDetector() override;
+    private:
+        bool processSnapshot(std::vector<std::string>& colors, std::string& error);
+
+        void onRequest(
+            const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+            std::shared_ptr<std_srvs::srv::Trigger::Response> response);
+
+        std::vector<std::string> classifyROIs(const cv::Mat& hsv, cv::Mat& debug_img) const;
+
+        std::string classify(const cv::Vec3d& hsv_roi) const;
+
+        std::string generateImagePath() const;
+
+        rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr service_;
+
+        rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr ask_sub_;
+        rclcpp::Publisher<std_msgs::msg::String>::SharedPtr color_pub_;
+
+        std::vector<cv::Rect> rois_;
+
+        std::string link_;
+        bool save_to_file_ = true;
+        std::string save_directory_ = "./";
+        bool enable_ = false;
+        bool headless_ = true;
+
+        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)
+        {
+            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)
+        {
+            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;
+        }
+}