Merge pull request #701 from JdeRobot/cpp_common_interfaces

Cpp common interfaces

Author: javizqh

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()

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

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

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