Merge pull request #3794 from JdeRobot/cpp-global-nav-update

Global nav Cpp update

Author: javizqh

exercises/global_navigation/cpp_lib/CMakeLists.txt

@@ -12,7 +12,9 @@ set(ROS2_PACKAGES
     nav_msgs
     geometry_msgs
     sensor_msgs
+    std_msgs
     cv_bridge
+    nav_msgs
 )
 
 foreach(pkg IN LISTS ROS2_PACKAGES)
@@ -26,29 +28,22 @@ 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")
 
-set(HAL_ROS_DEPS     rclcpp geometry_msgs nav_msgs)
 set(WEBGUI_ROS_DEPS  rclcpp nav_msgs geometry_msgs sensor_msgs cv_bridge)
-set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json ${OpenCV_LIBS})
-
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json)
+set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs std_msgs cv_bridge nav_msgs)
 
 # --- libFrequency.so ---
 add_library(Frequency SHARED src/Frequency.cpp)
 target_include_directories(Frequency PUBLIC include)
 
-# --- libHAL.so ---
-add_library(HAL SHARED src/HAL.cpp)
-target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE})
-
-ament_target_dependencies(HAL ${HAL_ROS_DEPS})
-target_link_libraries(HAL ${COMMON_INTERFACES_LIB})
-
 # --- libWebGUI.so ---
 add_library(WebGUI SHARED src/WebGUI.cpp src/Map.cpp)
-target_include_directories(WebGUI PUBLIC 
-    include 
-    ${COMMON_INTERFACES_INCLUDE} 
-    ${OpenCV_INCLUDE_DIRS}
-)
-
+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})
+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 ${COMMON_INTERFACES_LIB} ${OpenCV_LIBS})

exercises/global_navigation/cpp_lib/include/HAL.hpp

@@ -3,23 +3,34 @@
 
 #include <memory>
 #include <thread>
-#include <chrono>
-#include "rclcpp/rclcpp.hpp"
-#include "common_interfaces_cpp/hal/motors.hpp"
 #include "common_interfaces_cpp/hal/odometry.hpp"
 
-class HAL : public rclcpp::Node
+// Forward declarations to speed up compilation by avoiding heavy ROS 2 includes.
+// - MotorsNode links to "common_interfaces_cpp/hal/motors.hpp"
+// - OdometryNode links to "common_interfaces_cpp/hal/odometry.hpp"
+class MotorsNode;
+class OdometryNode;
+namespace rclcpp::executors { class SingleThreadedExecutor; }
+
+class HAL
 {
 public:
-    HAL();
-    static void set_v(const float speed);
-    static void set_w(const float speed);
+    // Prevent instantiation. HAL acts as a global static utility.
+    HAL() = delete;
+
+    static void set_v(const float velocity);
+    static void set_w(const float velocity);
     static Pose3d get_pose3d();
 
 private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    // Hidden internal state. Not accessible to the user.
     static std::shared_ptr<MotorsNode> motors_node_;
     static std::shared_ptr<OdometryNode> odometry_node_;
-    std::thread spin_thread_;
+    static std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
+    static std::thread spin_thread_;
 };
 
 #endif

exercises/global_navigation/cpp_lib/include/Map.hpp

@@ -4,6 +4,7 @@
 #include <vector>
 #include <string>
 #include <functional>
+#include <mutex>
 #include <opencv2/opencv.hpp>
 #include "common_interfaces_cpp/hal/odometry.hpp"
 
@@ -44,6 +45,7 @@ class Map
 private:
     std::function<Pose3d()> pose_callback_;
     cv::Mat grid_;
+    std::mutex grid_mtx_;
 };
 
 #endif

exercises/global_navigation/cpp_lib/include/WebGUI.hpp

@@ -1,73 +1,34 @@
 #ifndef INCLUDE_WEBGUI_HPP_
 #define INCLUDE_WEBGUI_HPP_
 
-#include <boost/beast/core.hpp>
-#include <boost/beast/websocket.hpp>
-#include <boost/asio/connect.hpp>
-#include <boost/asio/ip/tcp.hpp>
-#include <boost/asio/strand.hpp>
-#include <nlohmann/json.hpp>
 #include <opencv2/opencv.hpp>
-#include <mutex>
+#include <vector>
 #include <memory>
 #include <string>
-#include <vector>
-#include "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "nav_msgs/msg/path.hpp"
-#include "sensor_msgs/msg/image.hpp"
-#include "common_interfaces_cpp/hal/odometry.hpp"
-#include "Map.hpp"
+#include <thread>
 
-namespace beast = boost::beast;
-namespace websocket = beast::websocket;
-namespace net = boost::asio;
-using tcp = net::ip::tcp;
-using json = nlohmann::json;
+class WebGUINode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
 
 class WebGUI
 {
 public:
-    WebGUI();
-
-    static void showNumpy(const cv::Mat& image);
-    static void showPath(const std::vector<std::vector<int>>& array);
-    static std::vector<double> getTargetPose();
-    static cv::Mat getMap(const std::string& url);
-    static std::vector<int> rowColumn(const std::vector<double>& pose);
-    static std::vector<int> worldToGrid(const std::vector<double>& pose);
-    static std::vector<double> gridToWorld(const std::vector<int>& cell);
-    static void reset_gui();
-    static std::string payloadImage();
-
-    static std::vector<double> worldXY;
-    static std::string array_str;
-
-private:
-    static cv::Mat image_to_be_shown;
-    static bool image_to_be_shown_updated;
-    static std::mutex image_show_lock;
-    static std::mutex array_lock;
-};
+    WebGUI() = delete;
 
-class WebGUINode : public rclcpp::Node
-{
-public:
-    WebGUINode();
-    
-    static std::shared_ptr<Map> map_;
-    static std::shared_ptr<OdometryNode> pose3d_node_;
-    
-    static void publish_target(double x, double y);
+    static void show_image(const cv::Mat& image);
+    static void show_path(const std::vector<std::vector<int>>& path);
+    static std::vector<double> get_target_pose();
+    static cv::Mat get_map(const std::string& url);
+    static std::vector<int> world_to_grid(const std::vector<double>& pose);
+    static std::vector<double> grid_to_world(const std::vector<double>& cell);
 
 private:
-    void path_callback(nav_msgs::msg::Path::UniquePtr msg);
-    void image_callback(sensor_msgs::msg::Image::UniquePtr msg);
-
-    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_sub_;
-    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr image_sub_;
+    static void init();
+    friend class SystemBootstrapper;
 
-    static std::shared_ptr<rclcpp::Publisher<geometry_msgs::msg::Point>> target_pub_;
+    static std::shared_ptr<WebGUINode> gui_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
 };
 
 #endif

exercises/global_navigation/cpp_lib/src/HAL.cpp

@@ -1,46 +1,45 @@
 #include "HAL.hpp"
+#include "common_interfaces_cpp/hal/motors.hpp"
+#include "common_interfaces_cpp/hal/odometry.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
 
 using namespace std::chrono_literals;
 
 std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
 std::shared_ptr<OdometryNode> HAL::odometry_node_ = nullptr;
+std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> HAL::executor_ = nullptr;
+std::thread HAL::spin_thread_;
 
-HAL::HAL() : Node("hal_node")
+void HAL::init()
 {
-    motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 4.0, 0.3);
-    odometry_node_ = std::make_shared<OdometryNode>("/odom", "hal_odometry_node");
-
-    spin_thread_ = std::thread([]() {
-        rclcpp::executors::SingleThreadedExecutor executor;
-        executor.add_node(HAL::motors_node_);
-        executor.add_node(HAL::odometry_node_);
-        
-        while (rclcpp::ok()) {
-            executor.spin_some();
-            std::this_thread::sleep_for(std::chrono::milliseconds(10));
-        }
-    });
-    spin_thread_.detach();
+    if (!motors_node_) {
+        motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 4.0, 0.3, "hal_motors");
+        odometry_node_ = std::make_shared<OdometryNode>("/odom", "hal_odometry_node");
+
+        executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+        executor_->add_node(motors_node_);
+        executor_->add_node(odometry_node_);
+
+        spin_thread_ = std::thread([]() {
+            executor_->spin();
+        });
+        spin_thread_.detach();
+    }
 }
 
-void HAL::set_v(const float speed)
+void HAL::set_v(const float velocity)
 {
-    if (motors_node_) {
-        motors_node_->sendV(static_cast<double>(speed));
-    }
+    if (motors_node_) motors_node_->sendV(static_cast<double>(velocity));
 }
 
-void HAL::set_w(const float speed)
+void HAL::set_w(const float velocity)
 {
-    if (motors_node_) {
-        motors_node_->sendW(static_cast<double>(speed));
-    }
+    if (motors_node_) motors_node_->sendW(static_cast<double>(velocity));
 }
 
 Pose3d HAL::get_pose3d()
 {
-    if (odometry_node_) {
-        return odometry_node_->getPose3d();
-    }
+    if (odometry_node_) return odometry_node_->getPose3d();
     return Pose3d();
 }

exercises/global_navigation/cpp_lib/src/Map.cpp

@@ -75,11 +75,13 @@ cv::Mat Map::RTFormula()
 
 double Map::getGridVal(int x, int y)
 {
+    std::lock_guard<std::mutex> lock(grid_mtx_);
     return grid_.at<double>(y, x);
 }
 
 void Map::setGridVal(int x, int y, double val)
 {
+    std::lock_guard<std::mutex> lock(grid_mtx_);
     grid_.at<double>(y, x) = val;
 }