change pkg structure and add hal interfaces for cpp

Author: aquintan4

common_interfaces_cpp/CMakeLists.txt

@@ -5,33 +5,50 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
   add_compile_options(-Wall -Wextra -Wpedantic)
 endif()
 
-# Define a list of dependencies for easy modularity and future additions
+# 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
 )
 
 # Find ament_cmake (required for the build system itself)
 find_package(ament_cmake REQUIRED)
 
-# Loop through the list to find all specified packages automatically
+# Loop through the list to find all specified ROS 2 packages automatically
 foreach(dependency IN LISTS ros2_dependencies)
   find_package(${dependency} REQUIRED)
 endforeach()
 
-# Create the shared library from the source files
+# 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/odometry.cpp
+  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 dependencies to the library
+# 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/

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 "gazebo_msgs/msg/contacts_state.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<gazebo_msgs::msg::ContactsState>& contacts);
+
+/* ### HAL INTERFACE ### */
+class BumperNode : public rclcpp::Node {
+public:
+    BumperNode(const std::vector<std::string>& topics);
+    BumperData getBumperData() const;
+
+private:
+    void right_callback(const gazebo_msgs::msg::ContactsState::SharedPtr contact);
+    void center_callback(const gazebo_msgs::msg::ContactsState::SharedPtr contact);
+    void left_callback(const gazebo_msgs::msg::ContactsState::SharedPtr contact);
+
+    std::vector<std::string> topics;
+    std::vector<rclcpp::Subscription<gazebo_msgs::msg::ContactsState>::SharedPtr> subs_;
+    std::vector<gazebo_msgs::msg::ContactsState> contact_states_;
+};
+
+#endif

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

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

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

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

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

…clude/common_interfaces_cpp/odometry.hpp …e/common_interfaces_cpp/hal/odometry.hppcommon_interfaces_cpp/include/common_interfaces_cpp/odometry.hpp renamed to common_interfaces_cpp/include/common_interfaces_cpp/hal/odometry.hpp common_interfaces_cpp/include/common_interfaces_cpp/odometry.hpp renamed to common_interfaces_cpp/include/common_interfaces_cpp/hal/odometry.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