Merge pull request #3782 from JdeRobot/cpp-follow-line-update

C++ update Follow Line

Author: javizqh

exercises/follow_line/cpp_lib/CMakeLists.txt

@@ -0,0 +1,55 @@
+cmake_minimum_required(VERSION 3.10)
+project(VacuumCleanerLibs CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+find_package(ament_cmake REQUIRED)
+
+set(ROS2_PACKAGES
+    rclcpp
+    nav_msgs
+    gazebo_msgs
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+    tf2
+    tf2_geometry_msgs
+    ros_gz_interfaces
+    cv_bridge
+)
+
+foreach(pkg IN LISTS ROS2_PACKAGES)
+    find_package(${pkg} REQUIRED)
+endforeach()
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(nlohmann_json REQUIRED)
+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(WEBGUI_ROS_DEPS  rclcpp nav_msgs gazebo_msgs tf2 tf2_geometry_msgs sensor_msgs cv_bridge)
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json)
+set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs std_msgs ros_gz_interfaces cv_bridge)
+
+
+# --- libFrequency.so ---
+add_library(Frequency SHARED src/Frequency.cpp)
+target_include_directories(Frequency PUBLIC include)
+
+# --- libWebGUI.so ---
+add_library(WebGUI SHARED src/WebGUI.cpp src/Lap.cpp)
+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} ${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/follow_line/cpp_lib/include/Frequency.hpp

@@ -0,0 +1,22 @@
+#ifndef INCLUDE_FREQUENCY_HPP_
+#define INCLUDE_FREQUENCY_HPP_
+
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+class Frequency
+{
+private:
+  std::chrono::high_resolution_clock::time_point last_time;
+  int is_first_iteration;
+
+public:
+  Frequency();
+  ~Frequency();
+
+  static int rate;
+  void tick(int ideal_cycle = 50);
+};
+
+#endif

exercises/follow_line/cpp_lib/include/HAL.hpp

@@ -0,0 +1,36 @@
+#ifndef INCLUDE_HAL_HPP_
+#define INCLUDE_HAL_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <memory>
+#include <thread>
+
+// Forward declarations to speed up compilation by avoiding heavy ROS 2 includes.
+// - MotorsNode links to "common_interfaces_cpp/hal/motors.hpp"
+// - CameraNode links to "common_interfaces_cpp/hal/camera.hpp"
+class MotorsNode;
+class CameraNode;
+namespace rclcpp::executors { class SingleThreadedExecutor; }
+
+class HAL
+{
+public:
+    // 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 cv::Mat get_image();
+
+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<CameraNode> camera_node_;
+    static std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/follow_line/cpp_lib/include/Lap.hpp

@@ -0,0 +1,29 @@
+#ifndef LAP_HPP_
+#define LAP_HPP_
+
+#include "common_interfaces_cpp/hal/odometry.hpp"
+#include <chrono>
+#include <string>
+#include <memory>
+#include <mutex>
+
+class Lap {
+public:
+    explicit Lap(std::shared_ptr<OdometryNode> pose3d);
+    std::string check_threshold();
+    std::string return_lap_time();
+    void reset();
+    void pause();
+    void unpause();
+
+private:
+    std::shared_ptr<OdometryNode> pose3d_;
+    std::chrono::system_clock::time_point start_time_;
+    std::chrono::duration<double> lap_time_;
+    bool lap_rest_;
+    bool buffer_;
+    bool pause_condition_;
+    std::mutex lap_mutex_;
+};
+
+#endif

exercises/follow_line/cpp_lib/include/WebGUI.hpp

@@ -0,0 +1,31 @@
+#ifndef INCLUDE_WEBGUI_HPP_
+#define INCLUDE_WEBGUI_HPP_
+
+#include <opencv2/opencv.hpp>
+#include <memory>
+#include <thread>
+
+// Forward declarations to speed up compilation by avoiding heavy ROS 2 includes.
+// - WebGUINode links to "common_interfaces_cpp/webgui/WebGUIBridge.hpp"
+class WebGUINode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
+
+class WebGUI
+{
+public:
+    // Prevent instantiation. WebGUI acts as a global static utility.
+    WebGUI() = delete;
+
+    static void show_image(const cv::Mat& image);
+
+private:
+    static void init();
+    friend class SystemBootstrapper;
+
+    // Hidden internal state. Not accessible to the user.
+    static std::shared_ptr<WebGUINode> gui_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
+};
+
+#endif

exercises/follow_line/cpp_lib/src/Frequency.cpp

@@ -0,0 +1,34 @@
+#include "Frequency.hpp"
+
+int Frequency::rate = 0;
+
+Frequency::Frequency() : is_first_iteration(1) {}
+
+Frequency::~Frequency() {}
+
+void Frequency::tick(int ideal_cycle)
+{
+  const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
+  const auto now = std::chrono::high_resolution_clock::now();
+
+  if (is_first_iteration == 1)
+  {
+    last_time = now;
+    is_first_iteration = 0;
+    return;
+  }
+
+  const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
+
+  if (iter_ms < ideal_ms)
+  {
+    rate = std::round(1000.0 / ideal_ms.count());
+    std::this_thread::sleep_for(ideal_ms - iter_ms);
+  }
+  else
+  {
+    rate = std::round(1000.0 / iter_ms.count());
+  }
+
+  last_time = now;
+}

exercises/follow_line/cpp_lib/src/HAL.cpp

@@ -0,0 +1,50 @@
+#include "HAL.hpp"
+#include "common_interfaces_cpp/hal/motors.hpp"
+#include "common_interfaces_cpp/hal/camera.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
+
+using namespace std::chrono_literals;
+
+std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
+std::shared_ptr<CameraNode> HAL::camera_node_ = nullptr;
+std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> HAL::executor_ = nullptr;
+std::thread HAL::spin_thread_;
+
+void HAL::init()
+{
+    if (!motors_node_) {
+        motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 4.0, 0.3);
+        camera_node_ = std::make_shared<CameraNode>("/cam_f1_left/image_raw");
+
+        executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+        executor_->add_node(motors_node_);
+        executor_->add_node(camera_node_);
+
+        spin_thread_ = std::thread([]() {
+            executor_->spin();
+        });
+        spin_thread_.detach();
+    }
+}
+
+void HAL::set_v(const float velocity)
+{
+    if (motors_node_) motors_node_->sendV(static_cast<double>(velocity));
+}
+
+void HAL::set_w(const float velocity)
+{
+    if (motors_node_) motors_node_->sendW(static_cast<double>(velocity));
+}
+
+cv::Mat HAL::get_image()
+{
+    if (!camera_node_) return cv::Mat();
+    auto image = camera_node_->getImage();
+    while (!image && rclcpp::ok()) {
+        std::this_thread::sleep_for(5ms);
+        image = camera_node_->getImage();
+    }
+    return (image) ? image->data.clone() : cv::Mat();
+}

exercises/follow_line/cpp_lib/src/Lap.cpp

@@ -0,0 +1,73 @@
+#include "Lap.hpp"
+#include <iomanip>
+#include <sstream>
+#include <cmath>
+
+Lap::Lap(std::shared_ptr<OdometryNode> pose3d) : pose3d_(pose3d) {
+    reset();
+}
+
+std::string Lap::check_threshold() {
+    std::lock_guard<std::mutex> lock(lap_mutex_);
+
+    if (!pause_condition_) {
+        if (start_time_.time_since_epoch().count() != 0 && !lap_rest_) {
+            auto now = std::chrono::system_clock::now();
+            if (lap_time_.count() == 0) {
+                lap_time_ = now - start_time_;
+            } else {
+                lap_time_ += now - start_time_;
+            }
+            start_time_ = now;
+        }
+
+        if (start_time_.time_since_epoch().count() == 0 && lap_rest_) {
+            start_time_ = std::chrono::system_clock::now();
+            lap_rest_ = false;
+        }
+    }
+
+    if (lap_time_.count() == 0) {
+        return "0";
+    }
+
+    double total_seconds = lap_time_.count();
+    int hours = static_cast<int>(total_seconds / 3600);
+    int minutes = static_cast<int>((total_seconds - hours * 3600) / 60);
+    double seconds = total_seconds - hours * 3600 - minutes * 60;
+
+    std::stringstream ss;
+    ss << hours << ":"
+       << std::setfill('0') << std::setw(2) << minutes << ":"
+       << std::setfill('0') << std::setw(2) << static_cast<int>(seconds) << "."
+       << std::setfill('0') << std::setw(6) << static_cast<int>(std::round((seconds - std::floor(seconds)) * 1000000));
+
+    return ss.str();
+}
+
+std::string Lap::return_lap_time() {
+    std::lock_guard<std::mutex> lock(lap_mutex_);
+    return std::to_string(lap_time_.count());
+}
+
+void Lap::reset() {
+    std::lock_guard<std::mutex> lock(lap_mutex_);
+    start_time_ = std::chrono::system_clock::time_point();
+    lap_time_ = std::chrono::duration<double>::zero();
+    lap_rest_ = true;
+    buffer_ = false;
+    pause_condition_ = false;
+}
+
+void Lap::pause() {
+    std::lock_guard<std::mutex> lock(lap_mutex_);
+    pause_condition_ = true;
+}
+
+void Lap::unpause() {
+    std::lock_guard<std::mutex> lock(lap_mutex_);
+    if (pause_condition_) {
+        start_time_ = std::chrono::system_clock::now();
+    }
+    pause_condition_ = false;
+}