laby-escape exercise update

database/exercises/db.sql

@@ -136,7 +136,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
 23	digital_image_processing	Digital Image Processing Exercises	Digital Image Processing Exercises	["ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/digital_image_processing
 24	car_junction	Car Junction	Autonomous Navigation through traffic at road junction.	["AUTONOMOUS DRIVING","ROS2", "MULTILANGUAGE"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/car_junction
 25	machine_vision	Machine Vision	Machine Vision exercise	["ROS2", "MULTILANGUAGE"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/machine_vision
-26	labyrinth_escape	Labyrinth Escape	Labyrinth Escape exercise	["ROS2","Drones"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
+26	labyrinth_escape	Labyrinth Escape	Labyrinth Escape exercise	["ROS2","Drones", "MULTILANGUAGE"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
 27	conveyor_exercise	Conveyor Belt Exercise	Control a conveyor belt with ROS2	["ROS2","INDUSTRIAL"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/conveyor_exercise
 28	drone_cat_mouse	Drone Cat Mouse	Two-drone chase exercise: program the cat drone to catch the mouse drone	["ROS2","Drones","MULTI-AGENT"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/drone_cat_mouse
 \.

exercises/labyrinth_escape/cpp_lib/CMakeLists.txt

@@ -0,0 +1,58 @@
+cmake_minimum_required(VERSION 3.10)
+project(LabyrinthEscapeLibs CXX)
+
+# Forzar el estándar C++17 para máxima compatibilidad con ROS 2 Humble
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+find_package(ament_cmake REQUIRED)
+
+set(ROS2_PACKAGES
+    rclcpp
+    nav_msgs
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+    tf2
+    tf2_geometry_msgs
+    ros_gz_interfaces
+    cv_bridge
+    jderobot_drones_cpp
+    as2_core
+    as2_msgs
+    as2_motion_reference_handlers
+)
+
+foreach(pkg IN LISTS ROS2_PACKAGES)
+    find_package(${pkg} REQUIRED)
+endforeach()
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(nlohmann_json REQUIRED)
+find_package(OpenCV REQUIRED)
+
+set(COMMON_INTERFACES_INCLUDE "/home/ws/install/common_interfaces_cpp/include")
+set(COMMON_INTERFACES_LIB "/home/ws/install/common_interfaces_cpp/lib/libcommon_interfaces_cpp.so")
+
+# Dependencias por objetivo
+set(WEBGUI_ROS_DEPS  rclcpp nav_msgs tf2 tf2_geometry_msgs sensor_msgs cv_bridge)
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json)
+set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs cv_bridge jderobot_drones_cpp as2_core as2_msgs as2_motion_reference_handlers)
+
+
+# --- libFrequency.so ---
+add_library(Frequency SHARED src/Frequency.cpp)
+target_include_directories(Frequency PUBLIC include)
+
+# --- libWebGUI.so ---
+add_library(WebGUI SHARED src/WebGUI.cpp)
+target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
+ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS})
+target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS} ${COMMON_INTERFACES_LIB} ${OpenCV_LIBS})
+
+# --- libHAL.so ---
+add_library(HAL SHARED src/HAL.cpp)
+target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
+ament_target_dependencies(HAL ${HAL_ROS_DEPS})
+target_link_libraries(HAL ${OpenCV_LIBS})

exercises/labyrinth_escape/cpp_lib/include/Frequency.hpp

@@ -0,0 +1,22 @@
+#ifndef INCLUDE_FREQUENCY_HPP_
+#define INCLUDE_FREQUENCY_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+class Frequency
+{
+private:
+  std::chrono::high_resolution_clock::time_point last_time;
+  int is_first_iteration;
+
+public:
+  Frequency();
+  ~Frequency();
+
+  static int rate;
+  void tick(int ideal_cycle = 50);
+};
+
+#endif

exercises/labyrinth_escape/cpp_lib/include/HAL.hpp

@@ -0,0 +1,63 @@
+#ifndef INCLUDE_HAL_HPP_
+#define INCLUDE_HAL_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <memory>
+#include <thread>
+
+// Forward declarations
+class DroneWrapper;
+class CameraNode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
+
+class HAL
+{
+public:
+    struct Pose3d {
+        double x;
+        double y;
+        double z;
+        double h;
+        double yaw;
+        double pitch;
+        double roll;
+        double timeStamp;
+    };
+
+    struct Velocity3d {
+        double vx;
+        double vy;
+        double vz;
+        double yaw_rate;
+    };
+
+    // Prevent instantiation
+    HAL() = delete;
+
+    static cv::Mat get_frontal_image();
+    static cv::Mat get_ventral_image();
+
+    static Pose3d get_pose3d();
+    static Velocity3d get_velocity();
+    static int get_landed_state();
+
+    static void set_cmd_pos(float x, float y, float z, float az);
+    static void set_cmd_vel(float vx, float vy, float vz, float az);
+    static void set_cmd_mix(float vx, float vy, float z, float az);
+
+    static void takeoff(float h = 3.0f);
+    static void land();
+
+private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    // Hidden internal state
+    static std::shared_ptr<DroneWrapper> drone_node_;
+    static std::shared_ptr<CameraNode> frontal_camera_node_;
+    static std::shared_ptr<CameraNode> ventral_camera_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/labyrinth_escape/cpp_lib/include/WebGUI.hpp

@@ -0,0 +1,31 @@
+#ifndef INCLUDE_WEBGUI_HPP_
+#define INCLUDE_WEBGUI_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <memory>
+#include <thread>
+
+// Forward declarations to avoid heavy ROS 2 includes
+class WebGUINode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
+
+class WebGUI
+{
+public:
+    // Prevent instantiation. WebGUI acts as a global static utility.
+    WebGUI() = delete;
+
+    static void show_right_image(const cv::Mat& image);
+    static void show_left_image(const cv::Mat& image);
+
+private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    // Hidden internal state. Not accessible to the user.
+    static std::shared_ptr<WebGUINode> gui_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/labyrinth_escape/cpp_lib/src/Frequency.cpp

@@ -0,0 +1,34 @@
+#include "Frequency.hpp"
+
+int Frequency::rate = 0;
+
+Frequency::Frequency() : is_first_iteration(1) {}
+
+Frequency::~Frequency() {}
+
+void Frequency::tick(int ideal_cycle)
+{
+  const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
+  const auto now = std::chrono::high_resolution_clock::now();
+
+  if (is_first_iteration == 1)
+  {
+    last_time = now;
+    is_first_iteration = 0;
+    return;
+  }
+
+  const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
+
+  if (iter_ms < ideal_ms)
+  {
+    rate = std::round(1000.0 / ideal_ms.count());
+    std::this_thread::sleep_for(ideal_ms - iter_ms);
+  }
+  else
+  {
+    rate = std::round(1000.0 / iter_ms.count());
+  }
+
+  last_time = now;
+}

exercises/labyrinth_escape/cpp_lib/src/HAL.cpp

@@ -0,0 +1,121 @@
+#include "HAL.hpp"
+#include "jderobot_drones_cpp/drone_wrapper.hpp"
+#include "common_interfaces_cpp/hal/camera.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+std::shared_ptr<DroneWrapper> HAL::drone_node_ = nullptr;
+std::shared_ptr<CameraNode> HAL::frontal_camera_node_ = nullptr;
+std::shared_ptr<CameraNode> HAL::ventral_camera_node_ = nullptr;
+std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> HAL::executor_ = nullptr;
+std::thread HAL::spin_thread_;
+
+void HAL::init()
+{
+    if (!drone_node_) {
+        drone_node_ = std::make_shared<DroneWrapper>("drone0");
+
+        // Setup standard CameraNodes with their respective topics
+        frontal_camera_node_ = std::make_shared<CameraNode>("/drone0/frontal_cam/image_raw", "hal_frontal_camera");
+        ventral_camera_node_ = std::make_shared<CameraNode>("/drone0/ventral_cam/image_raw", "hal_ventral_camera");
+
+        // MultiThreadedExecutor spins nodes concurrently
+        executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
+        executor_->add_node(drone_node_);
+        executor_->add_node(frontal_camera_node_);
+        executor_->add_node(ventral_camera_node_);
+
+        spin_thread_ = std::thread([]() {
+            executor_->spin();
+        });
+        spin_thread_.detach();
+    }
+}
+
+cv::Mat HAL::get_frontal_image()
+{
+    if (!frontal_camera_node_) return cv::Mat();
+
+    auto image = frontal_camera_node_->getImage();
+    return (image) ? image->data.clone() : cv::Mat();
+}
+
+cv::Mat HAL::get_ventral_image()
+{
+    if (!ventral_camera_node_) return cv::Mat();
+
+    auto image = ventral_camera_node_->getImage();
+    return (image) ? image->data.clone() : cv::Mat();
+}
+
+HAL::Pose3d HAL::get_pose3d()
+{
+    // Safe default when node is not ready
+    if (!drone_node_) return HAL::Pose3d{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
+
+    auto pos = drone_node_->getPosition();
+    auto ori = drone_node_->getOrientation();
+
+    return HAL::Pose3d{
+        pos.size() > 0 ? pos[0] : 0.0,
+        pos.size() > 1 ? pos[1] : 0.0,
+        pos.size() > 2 ? pos[2] : 0.0,
+        0.0,
+        ori.size() > 2 ? ori[2] : 0.0,
+        ori.size() > 1 ? ori[1] : 0.0,
+        ori.size() > 0 ? ori[0] : 0.0,
+        0.0
+    };
+}
+
+HAL::Velocity3d HAL::get_velocity()
+{
+    // Safe default when node is not ready
+    if (!drone_node_) return HAL::Velocity3d{0.0, 0.0, 0.0, 0.0};
+
+    auto vel = drone_node_->getVelocity();
+    float yaw_rate = drone_node_->getYawRate();
+
+    return HAL::Velocity3d{
+        vel.size() > 0 ? vel[0] : 0.0,
+        vel.size() > 1 ? vel[1] : 0.0,
+        vel.size() > 2 ? vel[2] : 0.0,
+        static_cast<double>(yaw_rate)
+    };
+}
+
+int HAL::get_landed_state()
+{
+    return drone_node_ ? drone_node_->getLandedState() : -1;
+}
+
+// Motion commands
+
+void HAL::set_cmd_pos(float x, float y, float z, float az)
+{
+    if (drone_node_) drone_node_->setCmdPos(x, y, z, az);
+}
+
+void HAL::set_cmd_vel(float vx, float vy, float vz, float az)
+{
+    if (drone_node_) drone_node_->setCmdVel(vx, vy, vz, az);
+}
+
+void HAL::set_cmd_mix(float vx, float vy, float z, float az)
+{
+    if (drone_node_) drone_node_->setCmdMix(vx, vy, z, az);
+}
+
+// High-level blocking commands
+
+void HAL::takeoff(float h)
+{
+    if (drone_node_) drone_node_->takeoff(h);
+}
+
+void HAL::land()
+{
+    if (drone_node_) drone_node_->land();
+}