diff --git a/common_interfaces_cpp/CMakeLists.txt b/common_interfaces_cpp/CMakeLists.txt
new file mode 100644
index 000000000..4c5b1a3e4
--- /dev/null
+++ b/common_interfaces_cpp/CMakeLists.txt
@@ -0,0 +1,72 @@
+cmake_minimum_required(VERSION 3.8)
+project(common_interfaces_cpp)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# Define a list of ROS 2 dependencies for easy modularity
+set(ros2_dependencies
+  rclcpp
+  nav_msgs
+  sensor_msgs
+  geometry_msgs
+  gazebo_msgs
+  rosgraph_msgs
+  cv_bridge
+  ros_gz_interfaces
+)
+
+# Find ament_cmake (required for the build system itself)
+find_package(ament_cmake REQUIRED)
+
+# Loop through the list to find all specified ROS 2 packages automatically
+foreach(dependency IN LISTS ros2_dependencies)
+  find_package(${dependency} REQUIRED)
+endforeach()
+
+# Find OpenCV explicitly (Required by Camera and Classnet for cv::Mat and cv::dnn)
+find_package(OpenCV REQUIRED)
+
+# Create the shared library from the source files matching the new structure
+add_library(${PROJECT_NAME} SHARED
+  src/hal/bumper.cpp
+  src/hal/camera.cpp
+  src/hal/classnet.cpp
+  src/hal/laser.cpp
+  src/hal/lidar.cpp
+  src/hal/motors.cpp
+  src/hal/odometry.cpp
+  src/hal/sim_time.cpp
+)
+
+# Specify the include directories so the compiler finds the .hpp files
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+  ${OpenCV_INCLUDE_DIRS}
+)
+
+# Link all listed ROS 2 dependencies and OpenCV to the library
+ament_target_dependencies(${PROJECT_NAME} ${ros2_dependencies})
+target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBRARIES})
+
+# Install the include directory so other packages can do #include
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# Install the library and export the target for downstream usage
+install(TARGETS ${PROJECT_NAME}
+  EXPORT ${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+# Export include directories, targets, and dependencies
+ament_export_include_directories(include)
+ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(${ros2_dependencies})
+
+ament_package()
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/bumper.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/bumper.hpp
new file mode 100644
index 000000000..111f4d614
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/bumper.hpp
@@ -0,0 +1,42 @@
+#ifndef COMMON_INTERFACES_CPP__BUMPER_HPP_
+#define COMMON_INTERFACES_CPP__BUMPER_HPP_
+
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "ros_gz_interfaces/msg/contacts.hpp"
+
+/* ### AUXILIARY FUNCTIONS ### */
+
+const int RIGHT_BUMPER = 0;
+const int CENTER_BUMPER = 1;
+const int LEFT_BUMPER = 2;
+
+class BumperData {
+public:
+    BumperData();
+    int state;
+    int bumper;
+
+    std::string to_string() const;
+};
+
+BumperData contactsToBumperData(const std::vector<ros_gz_interfaces::msg::Contacts>& contacts);
+
+/* ### HAL INTERFACE ### */
+class BumperNode : public rclcpp::Node {
+public:
+    BumperNode(const std::vector<std::string>& topics);
+    BumperData getBumperData() const;
+
+private:
+    void right_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact);
+    void center_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact);
+    void left_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact);
+
+    std::vector<std::string> topics;
+    std::vector<rclcpp::Subscription<ros_gz_interfaces::msg::Contacts>::SharedPtr> subs_;
+    std::vector<ros_gz_interfaces::msg::Contacts> contact_states_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp
new file mode 100644
index 000000000..86c553328
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/camera.hpp
@@ -0,0 +1,66 @@
+#ifndef COMMON_INTERFACES_CPP__CAMERA_HPP_
+#define COMMON_INTERFACES_CPP__CAMERA_HPP_
+
+#include <string>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/image.hpp"
+#include <opencv2/opencv.hpp>
+#include <cv_bridge/cv_bridge.h>
+
+/* ### AUXILIARY FUNCTIONS ### */
+
+const int MAXRANGE = 8;  // max length received from imageD
+const int MINRANGE = 0;
+
+/* Represents a camera image with metadata and pixel data. */
+class Image {
+public:
+    Image();
+    int height;          // Image height [pixels]
+    int width;           // Image width [pixels]
+    double timeStamp;    // Time stamp [s] */
+    std::string format;  // Image format string (RGB8, BGR,...)
+    cv::Mat data;        // The image data itself
+
+    std::string to_string() const;
+};
+
+// Declaración de función auxiliar implícita en tu código Python
+cv::Mat depthToRGB8(const cv::Mat& gray_img_buff, const std::string& encoding);
+
+/*
+ * Convert a ROS Image message to a JdeRobot Image object.
+ *
+ * Args:
+ * img: ROS sensor_msgs/Image message.
+ *
+ * Returns:
+ * Image object with BGR data, or None if the message is empty.
+ */
+std::shared_ptr<Image> imageMsg2Image(const sensor_msgs::msg::Image& img);
+
+/* ### HAL INTERFACE ### */
+
+/* ROS2 node that subscribes to a camera topic and stores the latest image. */
+class CameraNode : public rclcpp::Node {
+public:
+    CameraNode(const std::string& topic);
+    
+    /*
+     * Return the latest camera image.
+     *
+     * Returns:
+     * Image object with BGR data, or None if no image has been received.
+     */
+    std::shared_ptr<Image> getImage() const;
+
+private:
+    /* Store the latest image message received from the topic. */
+    void listener_callback(const sensor_msgs::msg::Image::SharedPtr msg);
+    
+    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr sub_;
+    sensor_msgs::msg::Image last_img_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/classnet.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/classnet.hpp
new file mode 100644
index 000000000..90522bd34
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/classnet.hpp
@@ -0,0 +1,43 @@
+#ifndef COMMON_INTERFACES_CPP__CLASSNET_HPP_
+#define COMMON_INTERFACES_CPP__CLASSNET_HPP_
+
+#include <string>
+#include <vector>
+#include <map>
+#include <opencv2/opencv.hpp>
+#include <opencv2/dnn.hpp>
+
+extern const std::string FROZEN_GRAPH;
+extern const std::string PB_TXT;
+extern const int SIZE;
+
+extern std::map<int, std::string> LABEL_MAP;
+
+class BoundingBox {
+public:
+    BoundingBox(int identifier, const std::string& class_id_str, float score, float xmin, float ymin, float xmax, float ymax);
+    
+    int id;
+    std::string class_id;
+    float score;
+    float xmin;
+    float ymin;
+    float xmax;
+    float ymax;
+
+    std::string to_string() const;
+};
+
+class NeuralNetwork {
+public:
+    NeuralNetwork();
+    cv::Mat detect(const cv::Mat& img);
+    
+    // Get bounding boxes function
+    std::vector<BoundingBox> getBoundingBoxes(const cv::Mat& img);
+
+private:
+    cv::dnn::Net net;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/laser.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/laser.hpp
new file mode 100644
index 000000000..f7d9c3c8d
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/laser.hpp
@@ -0,0 +1,46 @@
+#ifndef COMMON_INTERFACES_CPP__LASER_HPP_
+#define COMMON_INTERFACES_CPP__LASER_HPP_
+
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/laser_scan.hpp"
+
+/* ### AUXILIARY FUNCTIONS */
+
+class LaserData {
+public:
+    LaserData();
+    std::vector<float> values;  // meters
+    double minAngle;            // Angle of first value (rads)
+    double maxAngle;            // Angle of last value (rads)
+    double minRange;            // Min Range possible (meters)
+    double maxRange;            // Max Range possible (meters)
+    double timeStamp;           // seconds
+
+    std::string to_string() const;
+};
+
+/*
+ * Translates from ROS LaserScan to JderobotTypes LaserData.
+ * @param scan: ROS LaserScan to translate
+ * @type scan: LaserScan
+ * @return a LaserData translated from scan
+ */
+LaserData laserScan2LaserData(const sensor_msgs::msg::LaserScan& scan);
+
+
+/* ### HAL INTERFACE ### */
+class LaserNode : public rclcpp::Node {
+public:
+    LaserNode(const std::string& topic);
+    LaserData getLaserData() const;
+
+private:
+    void listener_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan);
+    
+    rclcpp::Subscription<sensor_msgs::msg::LaserScan>::SharedPtr sub_;
+    sensor_msgs::msg::LaserScan last_scan_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/lidar.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/lidar.hpp
new file mode 100644
index 000000000..c512da339
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/lidar.hpp
@@ -0,0 +1,42 @@
+#ifndef COMMON_INTERFACES_CPP__LIDAR_HPP_
+#define COMMON_INTERFACES_CPP__LIDAR_HPP_
+
+#include <vector>
+#include <string>
+#include <memory>
+#include <mutex>
+#include <array>
+#include "rclcpp/rclcpp.hpp"
+#include "sensor_msgs/msg/point_cloud2.hpp"
+
+class LidarData {
+public:
+    LidarData();
+    std::vector<std::array<float, 3>> points;
+    std::vector<float> intensities;
+    double timeStamp;
+    double min_range;
+    double max_range;
+    std::pair<double, double> field_of_view;
+    bool is_dense;
+
+    std::string to_string() const;
+};
+
+LidarData pointCloud2LidarData(const sensor_msgs::msg::PointCloud2::SharedPtr cloud);
+
+class LidarNode : public rclcpp::Node {
+public:
+    LidarNode(const std::string& topic);
+    LidarData getLidarData();
+    sensor_msgs::msg::PointCloud2::SharedPtr get_point_cloud();
+
+private:
+    void pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud);
+
+    std::mutex lock_;
+    sensor_msgs::msg::PointCloud2::SharedPtr last_cloud_;
+    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/motors.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/motors.hpp
new file mode 100644
index 000000000..3ce74fdb5
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/motors.hpp
@@ -0,0 +1,34 @@
+#ifndef COMMON_INTERFACES_CPP__MOTORS_HPP_
+#define COMMON_INTERFACES_CPP__MOTORS_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "geometry_msgs/msg/twist.hpp"
+
+/* ### HAL INTERFACE ### */
+
+/* ROS2 node that publishes linear and angular velocity commands to a robot. */
+class MotorsNode : public rclcpp::Node {
+public:
+    MotorsNode(const std::string& topic, double maxV, double maxW);
+
+    /* Publish a linear velocity command.
+     *
+     * Args:
+     * v (float): Linear velocity in m/s.
+     */
+    void sendV(double v);
+
+    /* Publish an angular velocity command.
+     *
+     * Args:
+     * w (float): Angular velocity in rad/s.
+     */
+    void sendW(double w);
+
+private:
+    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr pub;
+    geometry_msgs::msg::Twist last_twist;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/odometry.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/odometry.hpp
new file mode 100644
index 000000000..cfd56dac0
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/odometry.hpp
@@ -0,0 +1,31 @@
+#ifndef COMMON_INTERFACES_CPP__ODOMETRY_HPP_
+#define COMMON_INTERFACES_CPP__ODOMETRY_HPP_
+
+#include <vector>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/odometry.hpp"
+
+struct Pose3d {
+    double x, y, z, h, yaw, pitch, roll, timeStamp;
+    std::vector<double> q;
+    std::string to_string() const;
+};
+
+double quat2Yaw(double qw, double qx, double qy, double qz);
+double quat2Pitch(double qw, double qx, double qy, double qz);
+double quat2Roll(double qw, double qx, double qy, double qz);
+Pose3d odometry2Pose3D(const nav_msgs::msg::Odometry& odom);
+
+class OdometryNode : public rclcpp::Node {
+public:
+    OdometryNode(const std::string& topic, const std::string& node_name = "odometry_node");
+    Pose3d getPose3d() const;
+
+private:
+    void listener_callback(const nav_msgs::msg::Odometry::SharedPtr msg);
+    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_;
+    nav_msgs::msg::Odometry last_pose_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/include/common_interfaces_cpp/hal/sim_time.hpp b/common_interfaces_cpp/include/common_interfaces_cpp/hal/sim_time.hpp
new file mode 100644
index 000000000..1d7e30304
--- /dev/null
+++ b/common_interfaces_cpp/include/common_interfaces_cpp/hal/sim_time.hpp
@@ -0,0 +1,42 @@
+#ifndef COMMON_INTERFACES_CPP__SIM_TIME_HPP_
+#define COMMON_INTERFACES_CPP__SIM_TIME_HPP_
+
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "rosgraph_msgs/msg/clock.hpp"
+
+/* ### AUXILIARY FUNCTIONS */
+
+class SimTimeData {
+public:
+    SimTimeData();
+    int seconds;
+    int nanoseconds;
+
+    std::string to_string() const;
+};
+
+/*
+ * Translates from ROS Clock to JderobotTypes SimTimeData.
+ * @param clock: ROS Clock to translate
+ * @type clock: Clock
+ * @return a SimTimeData translated from clock
+ */
+SimTimeData simTime2SimTimeData(const rosgraph_msgs::msg::Clock& clock);
+
+
+/* ### HAL INTERFACE ### */
+
+class SimTimeNode : public rclcpp::Node {
+public:
+    SimTimeNode();
+    SimTimeData getSimTime() const;
+
+private:
+    void listener_callback(const rosgraph_msgs::msg::Clock::SharedPtr msg);
+
+    rclcpp::Subscription<rosgraph_msgs::msg::Clock>::SharedPtr sub;
+    rosgraph_msgs::msg::Clock last_sim_time_;
+};
+
+#endif
\ No newline at end of file
diff --git a/common_interfaces_cpp/package.xml b/common_interfaces_cpp/package.xml
new file mode 100644
index 000000000..2645085fc
--- /dev/null
+++ b/common_interfaces_cpp/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>common_interfaces_cpp</name>
+  <version>0.0.0</version>
+  <description>C++ shared library providing ROS 2 interfaces for hardware components.</description>
+  <maintainer email="dummy@dummy.com">dummy</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>cv_bridge</depend>
+  <depend>opencv</depend>
+
+  <depend>nav_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>gazebo_msgs</depend>
+  <depend>rosgraph_msgs</depend>
+  <depend>ros_gz_interfaces</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/bumper.cpp b/common_interfaces_cpp/src/hal/bumper.cpp
new file mode 100644
index 000000000..4b02e25a3
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/bumper.cpp
@@ -0,0 +1,61 @@
+#include "common_interfaces_cpp/hal/bumper.hpp"
+#include <sstream>
+#include <functional>
+
+BumperData::BumperData() {
+    state = 0;
+    bumper = CENTER_BUMPER;
+}
+
+std::string BumperData::to_string() const {
+    std::ostringstream oss;
+    oss << "Bumper: {\n   state: " << state
+        << "\n   bumper: " << bumper << "\n}";
+    return oss.str();
+}
+
+BumperData contactsToBumperData(const std::vector<ros_gz_interfaces::msg::Contacts>& contacts) {
+    BumperData bumper_data;
+
+    for (size_t i = 0; i < contacts.size(); ++i) {
+        if (contacts[i].contacts.size() > 0) {
+            bumper_data.state = 1;
+            bumper_data.bumper = i;
+            break;
+        }
+    }
+
+    return bumper_data;
+}
+
+BumperNode::BumperNode(const std::vector<std::string>& topics_list) 
+    : Node("bumper_node"), topics(topics_list), contact_states_(3) {
+    
+    std::vector<std::function<void(const ros_gz_interfaces::msg::Contacts::SharedPtr)>> callbacks_ = {
+        std::bind(&BumperNode::right_callback, this, std::placeholders::_1),
+        std::bind(&BumperNode::center_callback, this, std::placeholders::_1),
+        std::bind(&BumperNode::left_callback, this, std::placeholders::_1)
+    };
+
+    for (size_t i = 0; i < topics.size(); ++i) {
+        subs_.push_back(this->create_subscription<ros_gz_interfaces::msg::Contacts>(
+            topics[i], 10, callbacks_[i]
+        ));
+    }
+}
+
+void BumperNode::right_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact) {
+    contact_states_[0] = *contact;
+}
+
+void BumperNode::center_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact) {
+    contact_states_[1] = *contact;
+}
+
+void BumperNode::left_callback(const ros_gz_interfaces::msg::Contacts::SharedPtr contact) {
+    contact_states_[2] = *contact;
+}
+
+BumperData BumperNode::getBumperData() const {
+    return contactsToBumperData(contact_states_);
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/camera.cpp b/common_interfaces_cpp/src/hal/camera.cpp
new file mode 100644
index 000000000..93f498e1a
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/camera.cpp
@@ -0,0 +1,70 @@
+#include "common_interfaces_cpp/hal/camera.hpp"
+#include <sstream>
+#include <cmath>
+
+const double PI = M_PI;
+
+Image::Image() {
+    height = 480;  // Image height [pixels]
+    width = 640;   // Image width [pixels]
+    timeStamp = 0; // Time stamp [s] */
+    format = "";   // Image format string (RGB8, BGR,...)
+    data = cv::Mat::zeros(height, width, CV_8UC3); // The image data itself
+}
+
+std::string Image::to_string() const {
+    std::ostringstream oss;
+    oss << "Image: {\n   height: " << height
+        << "\n   width: " << width
+        << "\n   format: " << format 
+        << "\n   timeStamp: " << timeStamp
+        << "\n   data: " << data << "\n}";
+    return oss.str();
+}
+
+std::shared_ptr<Image> imageMsg2Image(const sensor_msgs::msg::Image& img) {
+    if (img.data.empty()) {
+        return nullptr;
+    }
+
+    auto image = std::make_shared<Image>();
+
+    image->width = img.width;
+    image->height = img.height;
+    image->format = "BGR8";
+    image->timeStamp = img.header.stamp.sec + (img.header.stamp.nanosec * 1e-9);
+
+    cv::Mat cv_image;
+    if (img.encoding.length() >= 2 && img.encoding.substr(img.encoding.length() - 2) == "C1") {
+        cv::Mat gray_img_buff = cv_bridge::toCvCopy(img, img.encoding)->image;
+        cv_image = depthToRGB8(gray_img_buff, img.encoding);
+    } else {
+        cv_image = cv_bridge::toCvCopy(img, "bgr8")->image;
+    }
+
+    image->data = cv_image;
+    return image;
+}
+
+cv::Mat depthToRGB8(const cv::Mat& gray_img_buff, const std::string& encoding) {
+    cv::Mat color_img;
+    if (!gray_img_buff.empty()) {
+        cv::cvtColor(gray_img_buff, color_img, cv::COLOR_GRAY2BGR);
+    }
+    return color_img;
+}
+
+CameraNode::CameraNode(const std::string& topic) 
+    : Node("camera_node") {
+    sub_ = this->create_subscription<sensor_msgs::msg::Image>(
+        topic, 10, std::bind(&CameraNode::listener_callback, this, std::placeholders::_1)
+    );
+}
+
+void CameraNode::listener_callback(const sensor_msgs::msg::Image::SharedPtr msg) {
+    last_img_ = *msg;
+}
+
+std::shared_ptr<Image> CameraNode::getImage() const {
+    return imageMsg2Image(last_img_);
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/classnet.cpp b/common_interfaces_cpp/src/hal/classnet.cpp
new file mode 100644
index 000000000..c0c263f4e
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/classnet.cpp
@@ -0,0 +1,117 @@
+#include "common_interfaces_cpp/hal/classnet.hpp"
+#include <sstream>
+
+const std::string FROZEN_GRAPH = "/resources/models/ssd_inception_v2_coco.pb";
+const std::string PB_TXT = "/resources/models/ssd_inception_v2_coco.pbtxt";
+const int SIZE = 300;
+
+std::map<int, std::string> LABEL_MAP = {
+    {0, "unlabeled"}, {1, "person"}, {2, "bicycle"}, {3, "car"}, {4, "motorcycle"},
+    {5, "airplane"}, {6, "bus"}, {7, "train"}, {8, "truck"}, {9, "boat"},
+    {10, "traffic"}, {11, "fire"}, {12, "street"}, {13, "stop"}, {14, "parking"},
+    {15, "bench"}, {16, "bird"}, {17, "cat"}, {18, "dog"}, {19, "horse"},
+    {20, "sheep"}, {21, "cow"}, {22, "elephant"}, {23, "bear"}, {24, "zebra"},
+    {25, "giraffe"}, {26, "hat"}, {27, "backpack"}, {28, "umbrella"}, {29, "shoe"},
+    {30, "eye"}, {31, "handbag"}, {32, "tie"}, {33, "suitcase"}, {34, "frisbee"},
+    {35, "skis"}, {36, "snowboard"}, {37, "sports"}, {38, "kite"}, {39, "baseball"},
+    {40, "baseball"}, {41, "skateboard"}, {42, "surfboard"}, {43, "tennis"}, {44, "bottle"},
+    {45, "plate"}, {46, "wine"}, {47, "cup"}, {48, "fork"}, {49, "knife"},
+    {50, "spoon"}, {51, "bowl"}, {52, "banana"}, {53, "apple"}, {54, "sandwich"},
+    {55, "orange"}, {56, "broccoli"}, {57, "carrot"}, {58, "hot"}, {59, "pizza"},
+    {60, "donut"}, {61, "cake"}, {62, "chair"}, {63, "couch"}, {64, "potted"},
+    {65, "bed"}, {66, "mirror"}, {67, "dining"}, {68, "window"}, {69, "desk"},
+    {70, "toilet"}, {71, "door"}, {72, "tv"}, {73, "laptop"}, {74, "mouse"},
+    {75, "remote"}, {76, "keyboard"}, {77, "cell"}, {78, "microwave"}, {79, "oven"},
+    {80, "toaster"}, {81, "sink"}, {82, "refrigerator"}, {83, "blender"}, {84, "book"},
+    {85, "clock"}, {86, "vase"}, {87, "scissors"}, {88, "teddy"}, {89, "hair"},
+    {90, "toothbrush"}, {91, "hair"}, {92, "banner"}, {93, "blanket"}, {94, "branch"},
+    {95, "bridge"}, {96, "building"}, {97, "bush"}, {98, "cabinet"}, {99, "cage"},
+    {100, "cardboard"}, {101, "carpet"}, {102, "ceiling"}, {103, "ceiling"}, {104, "cloth"},
+    {105, "clothes"}, {106, "clouds"}, {107, "counter"}, {108, "cupboard"}, {109, "curtain"},
+    {110, "desk"}, {111, "dirt"}, {112, "door"}, {113, "fence"}, {114, "floor"},
+    {115, "floor"}, {116, "floor"}, {117, "floor"}, {118, "floor"}, {119, "flower"},
+    {120, "fog"}, {121, "food"}, {122, "fruit"}, {123, "furniture"}, {124, "grass"},
+    {125, "gravel"}, {126, "ground"}, {127, "hill"}, {128, "house"}, {129, "leaves"},
+    {130, "light"}, {131, "mat"}, {132, "metal"}, {133, "mirror"}, {134, "moss"},
+    {135, "mountain"}, {136, "mud"}, {137, "napkin"}, {138, "net"}, {139, "paper"},
+    {140, "pavement"}, {141, "pillow"}, {142, "plant"}, {143, "plastic"}, {144, "platform"},
+    {145, "playingfield"}, {146, "railing"}, {147, "railroad"}, {148, "river"}, {149, "road"},
+    {150, "rock"}, {151, "roof"}, {152, "rug"}, {153, "salad"}, {154, "sand"},
+    {155, "sea"}, {156, "shelf"}, {157, "sky"}, {158, "skyscraper"}, {159, "snow"},
+    {160, "solid"}, {161, "stairs"}, {162, "stone"}, {163, "straw"}, {164, "structural"},
+    {165, "table"}, {166, "tent"}, {167, "textile"}, {168, "towel"}, {169, "tree"},
+    {170, "vegetable"}, {171, "wall"}, {172, "wall"}, {173, "wall"}, {174, "wall"},
+    {175, "wall"}, {176, "wall"}, {177, "wall"}, {178, "water"}, {179, "waterdrops"},
+    {180, "window"}, {181, "window"}, {182, "wood"}
+};
+
+BoundingBox::BoundingBox(int identifier, const std::string& class_id_str, float score, float xmin, float ymin, float xmax, float ymax)
+    : id(identifier), class_id(class_id_str), score(score), xmin(xmin), ymin(ymin), xmax(xmax), ymax(ymax) {}
+
+std::string BoundingBox::to_string() const {
+    std::ostringstream oss;
+    oss << "[id:" << id << "\nclass:" << class_id << "\nscore:" << score
+        << "\nxmin:" << xmin << "\nymin:" << ymin << "\nxmax:" << xmax << "\nymax:" << ymax << "\n";
+    return oss.str();
+}
+
+NeuralNetwork::NeuralNetwork() {
+    net = cv::dnn::readNetFromTensorflow(FROZEN_GRAPH, PB_TXT);
+}
+
+cv::Mat NeuralNetwork::detect(const cv::Mat& img) {
+    int rows = img.rows;
+    int cols = img.cols;
+    
+    cv::Mat blob = cv::dnn::blobFromImage(
+        img,
+        1.0 / 127.5,
+        cv::Size(SIZE, SIZE),
+        cv::Scalar(127.5, 127.5, 127.5),
+        true, // swapRB
+        false // crop
+    );
+    
+    net.setInput(blob);
+    cv::Mat cvOut = net.forward();
+    
+    // cvOut dimension is typically [1, 1, N, 7]. Reshape to [N, 7] to mimic cvOut[0, 0, :, :]
+    cv::Mat detections(cvOut.size[2], cvOut.size[3], CV_32F, cvOut.ptr<float>());
+    return detections;
+}
+
+// Get bounding boxes function
+std::vector<BoundingBox> NeuralNetwork::getBoundingBoxes(const cv::Mat& img) {
+    int rows = img.rows;
+    int cols = img.cols;
+    cv::Mat detections = detect(img);
+    std::vector<BoundingBox> bounding_boxes;
+
+    for (int i = 0; i < detections.rows; ++i) {
+        int identifier = static_cast<int>(detections.at<float>(i, 1));
+        float score = detections.at<float>(i, 2);
+        float xmin = detections.at<float>(i, 3) * cols;
+        float ymin = detections.at<float>(i, 4) * rows;
+        float xmax = detections.at<float>(i, 5) * cols;
+        float ymax = detections.at<float>(i, 6) * rows;
+
+        std::string class_name = "";
+        auto it = LABEL_MAP.find(identifier);
+        if (it != LABEL_MAP.end()) {
+            class_name = it->second;
+        }
+
+        BoundingBox bounding_box(
+            identifier,
+            class_name,
+            score,
+            xmin,
+            ymin,
+            xmax,
+            ymax
+        );
+        bounding_boxes.push_back(bounding_box);
+    }
+
+    return bounding_boxes;
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/laser.cpp b/common_interfaces_cpp/src/hal/laser.cpp
new file mode 100644
index 000000000..1dab9c4b3
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/laser.cpp
@@ -0,0 +1,70 @@
+#include "common_interfaces_cpp/hal/laser.hpp"
+#include <sstream>
+#include <cmath>
+
+const double PI = M_PI;
+
+LaserData::LaserData() {
+    minAngle = 0;  // Angle of first value (rads)
+    maxAngle = 0;  // Angle of last value (rads)
+    minRange = 0;  // Min Range possible (meters)
+    maxRange = 0;  // Max Range possible (meters)
+    timeStamp = 0; // seconds
+}
+
+std::string LaserData::to_string() const {
+    std::ostringstream oss;
+    oss << "LaserData: {\n   minAngle: " << minAngle
+        << "\n   maxAngle: " << maxAngle
+        << "\n   minRange: " << minRange
+        << "\n   maxRange: " << maxRange
+        << "\n   timeStamp: " << timeStamp
+        << "\n   values: [";
+    
+    // Mimics Python's str(self.values) list printing
+    for (size_t i = 0; i < values.size(); ++i) {
+        oss << values[i];
+        if (i < values.size() - 1) {
+            oss << ", ";
+        }
+    }
+    oss << "]\n}";
+    
+    return oss.str();
+}
+
+LaserData laserScan2LaserData(const sensor_msgs::msg::LaserScan& scan) {
+    LaserData laser;
+    laser.values = scan.ranges;
+    
+    /* ROS Angle Map      JdeRobot Angle Map
+                0                  PI/2
+                |                   |
+                |                   |
+       PI/2 --------- -PI/2  PI --------- 0
+                |                   |
+                |                   |
+    */
+    laser.minAngle = scan.angle_min + PI / 2.0;
+    laser.maxAngle = scan.angle_max + PI / 2.0;
+    laser.maxRange = scan.range_max;
+    laser.minRange = scan.range_min;
+    laser.timeStamp = scan.header.stamp.sec + (scan.header.stamp.nanosec * 1e-9);
+    
+    return laser;
+}
+
+LaserNode::LaserNode(const std::string& topic) 
+    : Node("laser_node") {
+    sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
+        topic, 10, std::bind(&LaserNode::listener_callback, this, std::placeholders::_1)
+    );
+}
+
+void LaserNode::listener_callback(const sensor_msgs::msg::LaserScan::SharedPtr scan) {
+    last_scan_ = *scan;
+}
+
+LaserData LaserNode::getLaserData() const {
+    return laserScan2LaserData(last_scan_);
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/lidar.cpp b/common_interfaces_cpp/src/hal/lidar.cpp
new file mode 100644
index 000000000..479f80f07
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/lidar.cpp
@@ -0,0 +1,95 @@
+#include "common_interfaces_cpp/hal/lidar.hpp"
+#include <sstream>
+#include <cmath>
+#include "sensor_msgs/point_cloud2_iterator.hpp"
+
+const double PI = M_PI;
+
+LidarData::LidarData() {
+    timeStamp = 0.0;
+    min_range = 0.1;
+    max_range = 15.0;
+    field_of_view = {2.0 * PI / 3.0, PI / 18.0};
+    is_dense = true;
+}
+
+std::string LidarData::to_string() const {
+    std::ostringstream oss;
+    oss << "LiDARData:\n"
+        << "  timestamp: " << timeStamp << "\n"
+        << "  points: " << points.size() << "\n"
+        << "  range: [" << min_range << ", " << max_range << "]\n"
+        << "  FOV: (" << field_of_view.first << ", " << field_of_view.second << ")\n"
+        << "  is_dense: " << (is_dense ? "True" : "False");
+    return oss.str();
+}
+
+LidarData pointCloud2LidarData(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) {
+    LidarData lidar;
+    if (!cloud || cloud->width * cloud->height == 0) {
+        return lidar;
+    }
+
+    // Read XYZ points
+    sensor_msgs::PointCloud2ConstIterator<float> iter_x(*cloud, "x");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_y(*cloud, "y");
+    sensor_msgs::PointCloud2ConstIterator<float> iter_z(*cloud, "z");
+
+    lidar.points.reserve(cloud->width * cloud->height);
+    for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z) {
+        lidar.points.push_back({*iter_x, *iter_y, *iter_z});
+    }
+
+    // Read intensities if available
+    bool has_intensity = false;
+    for (const auto& field : cloud->fields) {
+        if (field.name == "intensity") {
+            has_intensity = true;
+            break;
+        }
+    }
+
+    if (has_intensity) {
+        sensor_msgs::PointCloud2ConstIterator<float> iter_intensity(*cloud, "intensity");
+        lidar.intensities.reserve(cloud->width * cloud->height);
+        for (; iter_intensity != iter_intensity.end(); ++iter_intensity) {
+            lidar.intensities.push_back(*iter_intensity);
+        }
+    }
+
+    // Timestamp (ROS 2 Time -> seconds)
+    lidar.timeStamp = cloud->header.stamp.sec + (cloud->header.stamp.nanosec * 1e-9);
+
+    // Validate point cloud
+    lidar.is_dense = true;
+    for (const auto& point : lidar.points) {
+        if (!std::isfinite(point[0]) || !std::isfinite(point[1]) || !std::isfinite(point[2])) {
+            lidar.is_dense = false;
+            break;
+        }
+    }
+
+    return lidar;
+}
+
+LidarNode::LidarNode(const std::string& topic) 
+    : Node("lidar_node"), last_cloud_(nullptr) {
+    sub_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
+        topic, 10, std::bind(&LidarNode::pointcloud_callback, this, std::placeholders::_1)
+    );
+}
+
+void LidarNode::pointcloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr cloud) {
+    std::lock_guard<std::mutex> guard(lock_);
+    last_cloud_ = cloud;
+}
+
+LidarData LidarNode::getLidarData() {
+    std::lock_guard<std::mutex> guard(lock_);
+    return pointCloud2LidarData(last_cloud_);
+}
+
+sensor_msgs::msg::PointCloud2::SharedPtr LidarNode::get_point_cloud() {
+    std::lock_guard<std::mutex> guard(lock_);
+    return last_cloud_;
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/motors.cpp b/common_interfaces_cpp/src/hal/motors.cpp
new file mode 100644
index 000000000..e628d3bd4
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/motors.cpp
@@ -0,0 +1,19 @@
+#include "common_interfaces_cpp/hal/motors.hpp"
+
+MotorsNode::MotorsNode(const std::string& topic, double maxV, double maxW) 
+    : Node("motors_node") {
+    
+    // maxV and maxW are kept in the constructor signature to match the Python interface
+    
+    pub = this->create_publisher<geometry_msgs::msg::Twist>(topic, 10);
+}
+
+void MotorsNode::sendV(double v) {
+    last_twist.linear.x = v;
+    pub->publish(last_twist);
+}
+
+void MotorsNode::sendW(double w) {
+    last_twist.angular.z = w;
+    pub->publish(last_twist);
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/odometry.cpp b/common_interfaces_cpp/src/hal/odometry.cpp
new file mode 100644
index 000000000..fb0f62956
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/odometry.cpp
@@ -0,0 +1,81 @@
+#include "common_interfaces_cpp/hal/odometry.hpp"
+#include <sstream>
+#include <cmath>
+
+const double PI = M_PI;
+
+std::string Pose3d::to_string() const {
+    std::ostringstream oss;
+    oss << "Pose3D: {\n   x: " << x << "\n   y: " << y
+        << "\n   z: " << z << "\n   H: " << h
+        << "\n   Yaw: " << yaw << "\n   Pitch: " << pitch
+        << "\n   Roll: " << roll
+        << "\n   quaternion: [" << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << "]"
+        << "\n   timeStamp: " << timeStamp << "\n}";
+    return oss.str();
+}
+
+double quat2Yaw(double qw, double qx, double qy, double qz) {
+    double rotateZa0 = 2.0 * (qx * qy + qw * qz);
+    double rotateZa1 = qw * qw + qx * qx - qy * qy - qz * qz;
+    double rotateZ = 0.0;
+    if (rotateZa0 != 0.0 && rotateZa1 != 0.0) {
+        rotateZ = std::atan2(rotateZa0, rotateZa1);
+    }
+    return rotateZ;
+}
+
+double quat2Pitch(double qw, double qx, double qy, double qz) {
+    double rotateYa0 = -2.0 * (qx * qz - qw * qy);
+    double rotateY = 0.0;
+    if (rotateYa0 >= 1.0) {
+        rotateY = PI / 2.0;
+    } else if (rotateYa0 <= -1.0) {
+        rotateY = -PI / 2.0;
+    } else {
+        rotateY = std::asin(rotateYa0);
+    }
+    return rotateY;
+}
+
+double quat2Roll(double qw, double qx, double qy, double qz) {
+    double rotateXa0 = 2.0 * (qy * qz + qw * qx);
+    double rotateXa1 = qw * qw - qx * qx - qy * qy + qz * qz;
+    double rotateX = 0.0;
+
+    if (rotateXa0 != 0.0 && rotateXa1 != 0.0) {
+        rotateX = std::atan2(rotateXa0, rotateXa1);
+    }
+    return rotateX;
+}
+
+Pose3d odometry2Pose3D(const nav_msgs::msg::Odometry& odom) {
+    Pose3d pose;
+    auto ori = odom.pose.pose.orientation;
+
+    pose.x = odom.pose.pose.position.x;
+    pose.y = odom.pose.pose.position.y;
+    pose.z = odom.pose.pose.position.z;
+    pose.yaw = quat2Yaw(ori.w, ori.x, ori.y, ori.z);
+    pose.pitch = quat2Pitch(ori.w, ori.x, ori.y, ori.z);
+    pose.roll = quat2Roll(ori.w, ori.x, ori.y, ori.z);
+    pose.q = {ori.w, ori.x, ori.y, ori.z};
+    pose.timeStamp = odom.header.stamp.sec + (odom.header.stamp.nanosec * 1e-9);
+
+    return pose;
+}
+
+OdometryNode::OdometryNode(const std::string& topic, const std::string& node_name) 
+    : Node(node_name) {
+    sub_ = this->create_subscription<nav_msgs::msg::Odometry>(
+        topic, 10, std::bind(&OdometryNode::listener_callback, this, std::placeholders::_1)
+    );
+}
+
+Pose3d OdometryNode::getPose3d() const {
+    return odometry2Pose3D(last_pose_);
+}
+
+void OdometryNode::listener_callback(const nav_msgs::msg::Odometry::SharedPtr msg) {
+    last_pose_ = *msg;
+}
\ No newline at end of file
diff --git a/common_interfaces_cpp/src/hal/sim_time.cpp b/common_interfaces_cpp/src/hal/sim_time.cpp
new file mode 100644
index 000000000..16a64bf3d
--- /dev/null
+++ b/common_interfaces_cpp/src/hal/sim_time.cpp
@@ -0,0 +1,42 @@
+#include "common_interfaces_cpp/hal/sim_time.hpp"
+#include <sstream>
+
+SimTimeData::SimTimeData() {
+    seconds = 0;
+    nanoseconds = 0;
+}
+
+std::string SimTimeData::to_string() const {
+    std::ostringstream oss;
+    oss << "SimTimeData: {\n   sec: " << seconds
+        << "\n   nanosec: " << nanoseconds;
+    return oss.str();
+}
+
+SimTimeData simTime2SimTimeData(const rosgraph_msgs::msg::Clock& clock) {
+    SimTimeData clockData;
+    clockData.seconds = clock.clock.sec;
+    clockData.nanoseconds = clock.clock.nanosec;
+    return clockData;
+}
+
+SimTimeNode::SimTimeNode() 
+    : Node("simulation_time_node") {
+    
+    rclcpp::QoS qos_policy(rclcpp::KeepLast(1));
+    qos_policy.best_effort();
+
+    sub = this->create_subscription<rosgraph_msgs::msg::Clock>(
+        "/clock", 
+        qos_policy, 
+        std::bind(&SimTimeNode::listener_callback, this, std::placeholders::_1)
+    );
+}
+
+void SimTimeNode::listener_callback(const rosgraph_msgs::msg::Clock::SharedPtr msg) {
+    last_sim_time_ = *msg;
+}
+
+SimTimeData SimTimeNode::getSimTime() const {
+    return simTime2SimTimeData(last_sim_time_);
+}
\ No newline at end of file