Skip to content

Commit 542e3c8

Browse files
aquintan4aquintan4
committed
laby-escape exercise update (same webgui as other drone exercises + c++ support)
1 parent 0d3a457 commit 542e3c8

18 files changed

Lines changed: 892 additions & 11 deletions

File tree

‎database/exercises/db.sql‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -136,7 +136,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
136136
23 digital_image_processing Digital Image Processing Exercises Digital Image Processing Exercises ["ROS2"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/ComputerVision/digital_image_processing
137137
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
138138
25 machine_vision Machine Vision Machine Vision exercise ["ROS2", "MULTILANGUAGE"] PROTOTYPE https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/machine_vision
139-
26 labyrinth_escape Labyrinth Escape Labyrinth Escape exercise ["ROS2","Drones"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
139+
26 labyrinth_escape Labyrinth Escape Labyrinth Escape exercise ["ROS2","Drones", "MULTILANGUAGE"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
140140
27 conveyor_exercise Conveyor Belt Exercise Control a conveyor belt with ROS2 ["ROS2","INDUSTRIAL"] PROTOTYPE https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/conveyor_exercise
141141
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
142142
\.

‎exercises/labyrinth_escape/cpp_lib/CMakeLists.txt‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
cmake_minimum_required(VERSION 3.10)
2+
project(LabyrinthEscapeLibs CXX)
3+
4+
# Forzar el estándar C++17 para máxima compatibilidad con ROS 2 Humble
5+
set(CMAKE_CXX_STANDARD 17)
6+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
7+
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
8+
9+
find_package(ament_cmake REQUIRED)
10+
11+
set(ROS2_PACKAGES
12+
rclcpp
13+
nav_msgs
14+
geometry_msgs
15+
sensor_msgs
16+
std_msgs
17+
tf2
18+
tf2_geometry_msgs
19+
ros_gz_interfaces
20+
cv_bridge
21+
jderobot_drones_cpp
22+
as2_core
23+
as2_msgs
24+
as2_motion_reference_handlers
25+
)
26+
27+
foreach(pkg IN LISTS ROS2_PACKAGES)
28+
find_package(${pkg} REQUIRED)
29+
endforeach()
30+
31+
find_package(Boost REQUIRED COMPONENTS system)
32+
find_package(nlohmann_json REQUIRED)
33+
find_package(OpenCV REQUIRED)
34+
35+
set(COMMON_INTERFACES_INCLUDE "/home/ws/install/common_interfaces_cpp/include")
36+
set(COMMON_INTERFACES_LIB "/home/ws/install/common_interfaces_cpp/lib/libcommon_interfaces_cpp.so")
37+
38+
# Dependencias por objetivo
39+
set(WEBGUI_ROS_DEPS rclcpp nav_msgs tf2 tf2_geometry_msgs sensor_msgs cv_bridge)
40+
set(WEBGUI_SYS_DEPS Frequency Boost::system nlohmann_json::nlohmann_json)
41+
set(HAL_ROS_DEPS rclcpp geometry_msgs sensor_msgs cv_bridge jderobot_drones_cpp as2_core as2_msgs as2_motion_reference_handlers)
42+
43+
44+
# --- libFrequency.so ---
45+
add_library(Frequency SHARED src/Frequency.cpp)
46+
target_include_directories(Frequency PUBLIC include)
47+
48+
# --- libWebGUI.so ---
49+
add_library(WebGUI SHARED src/WebGUI.cpp)
50+
target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
51+
ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS})
52+
target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS} ${COMMON_INTERFACES_LIB} ${OpenCV_LIBS})
53+
54+
# --- libHAL.so ---
55+
add_library(HAL SHARED src/HAL.cpp)
56+
target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE} ${OpenCV_INCLUDE_DIRS})
57+
ament_target_dependencies(HAL ${HAL_ROS_DEPS})
58+
target_link_libraries(HAL ${OpenCV_LIBS})

‎exercises/labyrinth_escape/cpp_lib/include/Frequency.hpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef INCLUDE_FREQUENCY_HPP_
2+
#define INCLUDE_FREQUENCY_HPP_
3+
4+
#include <chrono>
5+
#include <cmath>
6+
#include <thread>
7+
8+
class Frequency
9+
{
10+
private:
11+
std::chrono::high_resolution_clock::time_point last_time;
12+
int is_first_iteration;
13+
14+
public:
15+
Frequency();
16+
~Frequency();
17+
18+
static int rate;
19+
void tick(int ideal_cycle = 50);
20+
};
21+
22+
#endif

‎exercises/labyrinth_escape/cpp_lib/include/HAL.hpp‎

Lines changed: 63 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,63 @@
1+
#ifndef INCLUDE_HAL_HPP_
2+
#define INCLUDE_HAL_HPP_
3+
4+
#include <opencv2/opencv.hpp>
5+
#include <memory>
6+
#include <thread>
7+
8+
// Forward declarations
9+
class DroneWrapper;
10+
class CameraNode;
11+
namespace rclcpp::executors { class MultiThreadedExecutor; }
12+
13+
class HAL
14+
{
15+
public:
16+
struct Pose3d {
17+
double x;
18+
double y;
19+
double z;
20+
double h;
21+
double yaw;
22+
double pitch;
23+
double roll;
24+
double timeStamp;
25+
};
26+
27+
struct Velocity3d {
28+
double vx;
29+
double vy;
30+
double vz;
31+
double yaw_rate;
32+
};
33+
34+
// Prevent instantiation
35+
HAL() = delete;
36+
37+
static cv::Mat get_frontal_image();
38+
static cv::Mat get_ventral_image();
39+
40+
static Pose3d get_pose3d();
41+
static Velocity3d get_velocity();
42+
static int get_landed_state();
43+
44+
static void set_cmd_pos(float x, float y, float z, float az);
45+
static void set_cmd_vel(float vx, float vy, float vz, float az);
46+
static void set_cmd_mix(float vx, float vy, float z, float az);
47+
48+
static void takeoff(float h = 3.0f);
49+
static void land();
50+
51+
private:
52+
static void init();
53+
friend class SystemBootstrapper;
54+
55+
// Hidden internal state
56+
static std::shared_ptr<DroneWrapper> drone_node_;
57+
static std::shared_ptr<CameraNode> frontal_camera_node_;
58+
static std::shared_ptr<CameraNode> ventral_camera_node_;
59+
static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
60+
static std::thread spin_thread_;
61+
};
62+
63+
#endif

‎exercises/labyrinth_escape/cpp_lib/include/WebGUI.hpp‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#ifndef INCLUDE_WEBGUI_HPP_
2+
#define INCLUDE_WEBGUI_HPP_
3+
4+
#include <opencv2/opencv.hpp>
5+
#include <memory>
6+
#include <thread>
7+
8+
// Forward declarations to avoid heavy ROS 2 includes
9+
class WebGUINode;
10+
namespace rclcpp::executors { class MultiThreadedExecutor; }
11+
12+
class WebGUI
13+
{
14+
public:
15+
// Prevent instantiation. WebGUI acts as a global static utility.
16+
WebGUI() = delete;
17+
18+
static void show_right_image(const cv::Mat& image);
19+
static void show_left_image(const cv::Mat& image);
20+
21+
private:
22+
static void init();
23+
friend class SystemBootstrapper;
24+
25+
// Hidden internal state. Not accessible to the user.
26+
static std::shared_ptr<WebGUINode> gui_node_;
27+
static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
28+
static std::thread spin_thread_;
29+
};
30+
31+
#endif

‎exercises/labyrinth_escape/cpp_lib/src/Frequency.cpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#include "Frequency.hpp"
2+
3+
int Frequency::rate = 0;
4+
5+
Frequency::Frequency() : is_first_iteration(1) {}
6+
7+
Frequency::~Frequency() {}
8+
9+
void Frequency::tick(int ideal_cycle)
10+
{
11+
const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
12+
const auto now = std::chrono::high_resolution_clock::now();
13+
14+
if (is_first_iteration == 1)
15+
{
16+
last_time = now;
17+
is_first_iteration = 0;
18+
return;
19+
}
20+
21+
const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
22+
23+
if (iter_ms < ideal_ms)
24+
{
25+
rate = std::round(1000.0 / ideal_ms.count());
26+
std::this_thread::sleep_for(ideal_ms - iter_ms);
27+
}
28+
else
29+
{
30+
rate = std::round(1000.0 / iter_ms.count());
31+
}
32+
33+
last_time = now;
34+
}

‎exercises/labyrinth_escape/cpp_lib/src/HAL.cpp‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,121 @@
1+
#include "HAL.hpp"
2+
#include "jderobot_drones_cpp/drone_wrapper.hpp"
3+
#include "common_interfaces_cpp/hal/camera.hpp"
4+
#include "rclcpp/rclcpp.hpp"
5+
#include <chrono>
6+
7+
using namespace std::chrono_literals;
8+
9+
std::shared_ptr<DroneWrapper> HAL::drone_node_ = nullptr;
10+
std::shared_ptr<CameraNode> HAL::frontal_camera_node_ = nullptr;
11+
std::shared_ptr<CameraNode> HAL::ventral_camera_node_ = nullptr;
12+
std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> HAL::executor_ = nullptr;
13+
std::thread HAL::spin_thread_;
14+
15+
void HAL::init()
16+
{
17+
if (!drone_node_) {
18+
drone_node_ = std::make_shared<DroneWrapper>("drone0");
19+
20+
// Setup standard CameraNodes with their respective topics
21+
frontal_camera_node_ = std::make_shared<CameraNode>("/drone0/frontal_cam/image_raw", "hal_frontal_camera");
22+
ventral_camera_node_ = std::make_shared<CameraNode>("/drone0/ventral_cam/image_raw", "hal_ventral_camera");
23+
24+
// MultiThreadedExecutor spins nodes concurrently
25+
executor_ = std::make_shared<rclcpp::executors::MultiThreadedExecutor>();
26+
executor_->add_node(drone_node_);
27+
executor_->add_node(frontal_camera_node_);
28+
executor_->add_node(ventral_camera_node_);
29+
30+
spin_thread_ = std::thread([]() {
31+
executor_->spin();
32+
});
33+
spin_thread_.detach();
34+
}
35+
}
36+
37+
cv::Mat HAL::get_frontal_image()
38+
{
39+
if (!frontal_camera_node_) return cv::Mat();
40+
41+
auto image = frontal_camera_node_->getImage();
42+
return (image) ? image->data.clone() : cv::Mat();
43+
}
44+
45+
cv::Mat HAL::get_ventral_image()
46+
{
47+
if (!ventral_camera_node_) return cv::Mat();
48+
49+
auto image = ventral_camera_node_->getImage();
50+
return (image) ? image->data.clone() : cv::Mat();
51+
}
52+
53+
HAL::Pose3d HAL::get_pose3d()
54+
{
55+
// Safe default when node is not ready
56+
if (!drone_node_) return HAL::Pose3d{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
57+
58+
auto pos = drone_node_->getPosition();
59+
auto ori = drone_node_->getOrientation();
60+
61+
return HAL::Pose3d{
62+
pos.size() > 0 ? pos[0] : 0.0,
63+
pos.size() > 1 ? pos[1] : 0.0,
64+
pos.size() > 2 ? pos[2] : 0.0,
65+
0.0,
66+
ori.size() > 2 ? ori[2] : 0.0,
67+
ori.size() > 1 ? ori[1] : 0.0,
68+
ori.size() > 0 ? ori[0] : 0.0,
69+
0.0
70+
};
71+
}
72+
73+
HAL::Velocity3d HAL::get_velocity()
74+
{
75+
// Safe default when node is not ready
76+
if (!drone_node_) return HAL::Velocity3d{0.0, 0.0, 0.0, 0.0};
77+
78+
auto vel = drone_node_->getVelocity();
79+
float yaw_rate = drone_node_->getYawRate();
80+
81+
return HAL::Velocity3d{
82+
vel.size() > 0 ? vel[0] : 0.0,
83+
vel.size() > 1 ? vel[1] : 0.0,
84+
vel.size() > 2 ? vel[2] : 0.0,
85+
static_cast<double>(yaw_rate)
86+
};
87+
}
88+
89+
int HAL::get_landed_state()
90+
{
91+
return drone_node_ ? drone_node_->getLandedState() : -1;
92+
}
93+
94+
// Motion commands
95+
96+
void HAL::set_cmd_pos(float x, float y, float z, float az)
97+
{
98+
if (drone_node_) drone_node_->setCmdPos(x, y, z, az);
99+
}
100+
101+
void HAL::set_cmd_vel(float vx, float vy, float vz, float az)
102+
{
103+
if (drone_node_) drone_node_->setCmdVel(vx, vy, vz, az);
104+
}
105+
106+
void HAL::set_cmd_mix(float vx, float vy, float z, float az)
107+
{
108+
if (drone_node_) drone_node_->setCmdMix(vx, vy, z, az);
109+
}
110+
111+
// High-level blocking commands
112+
113+
void HAL::takeoff(float h)
114+
{
115+
if (drone_node_) drone_node_->takeoff(h);
116+
}
117+
118+
void HAL::land()
119+
{
120+
if (drone_node_) drone_node_->land();
121+
}

0 commit comments

Comments
 (0)