Merge pull request #3791 from JdeRobot/obstacle-avoidance-cpp-update

obstacle avoidance cpp update

Author: javizqh

exercises/obstacle_avoidance/cpp_lib/CMakeLists.txt

@@ -21,33 +21,31 @@ 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 geometry_msgs std_msgs nav_msgs sensor_msgs)
-set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json)
+set(WEBGUI_SYS_DEPS  Frequency Boost::system nlohmann_json::nlohmann_json ${OpenCV_LIBS})
 set(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs nav_msgs)
-
+set(HAL_SYS_DEPS     ${OpenCV_LIBS})
 
 # --- 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/Map.cpp 
-    src/Lap.cpp
 )
-target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE})
+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})
 
-
 # --- libHAL.so ---
 add_library(HAL SHARED src/HAL.cpp)
-target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE})
+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})
+target_link_libraries(HAL ${HAL_SYS_DEPS} ${COMMON_INTERFACES_LIB})

exercises/obstacle_avoidance/cpp_lib/include/HAL.hpp

@@ -1,29 +1,35 @@
 #ifndef INCLUDE_HAL_HPP_
 #define INCLUDE_HAL_HPP_
 
-#include <vector>
 #include <memory>
 #include <thread>
-#include <chrono>
-#include "rclcpp/rclcpp.hpp"
-#include "common_interfaces_cpp/hal/motors.hpp"
-#include "common_interfaces_cpp/hal/laser.hpp"
-#include "common_interfaces_cpp/hal/odometry.hpp"
+#include <vector>
+#include <array>
+
+class MotorsNode;
+class OdometryNode;
+class LaserNode;
+namespace rclcpp::executors { class MultiThreadedExecutor; }
 
-class HAL : public rclcpp::Node
+class HAL
 {
 public:
-    HAL();
-    static void set_v(const double velocity);
-    static void set_w(const double velocity);
-    static Pose3d get_pose3d();
-    static const LaserData *get_laser_data();
+    HAL() = delete;
+
+    static void set_v(const float velocity);
+    static void set_w(const float velocity);
+    static std::array<double, 3> get_pose3d();
+    static std::vector<float> get_laser_data();
 
 private:
-    static std::shared_ptr<OdometryNode> pose3d_node_;
+    static void init();
+    friend class SystemBootstrapper;
+
     static std::shared_ptr<MotorsNode> motors_node_;
+    static std::shared_ptr<OdometryNode> odometry_node_;
     static std::shared_ptr<LaserNode> laser_node_;
-    std::thread spin_thread_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
 };
 
 #endif

exercises/obstacle_avoidance/cpp_lib/include/Lap.hpp

@@ -1,53 +1,72 @@
-#ifndef INCLUDE_MAP_HPP_
-#define INCLUDE_MAP_HPP_
+#ifndef MAP_HPP_
+#define MAP_HPP_
 
-#include <string>
-#include <vector>
-#include <memory>
-#include <functional>
-#include <nlohmann/json.hpp>
 #include "common_interfaces_cpp/hal/odometry.hpp"
 #include "common_interfaces_cpp/hal/laser.hpp"
+#include <memory>
+#include <vector>
+#include <string>
+#include <mutex>
+#include <array>
+#include <thread>
+#include <atomic>
+#include <nlohmann/json.hpp>
 
 class Target {
 public:
-    Target(const std::string& id, const Pose3d& pose, bool active = false, bool reached = false);
-    std::string getId() const;
-    Pose3d getPose() const;
-    bool isReached() const;
-    void setReached(bool value);
-
-    std::string id;
-    Pose3d pose;
-    bool reached;
-    bool active;
+    Target(std::string id, Pose3d pose, bool active = false, bool reached = false);
+
+    std::string get_id() const;
+    Pose3d get_pose() const;
+    bool is_reached() const;
+    void set_reached(bool value);
+    bool is_active() const;
+    void set_active(bool value);
+
+private:
+    std::string id_;
+    Pose3d pose_;
+    bool active_;
+    bool reached_;
 };
 
 class Map {
 public:
-    Map(std::function<LaserData()> laser_cb, std::function<Pose3d()> pose_cb);
-
-    void setCar(double x, double y);
-    void setObs(double x, double y);
-    void setAvg(double x, double y);
-    void setTargetPos(double x, double y);
-    
-    std::string get_json_data();
-    std::shared_ptr<Target> getNextTarget();
+    Map(std::shared_ptr<LaserNode> laser, std::shared_ptr<OdometryNode> odom);
+    ~Map();
+
+    void set_car(double newx, double newy);
+    void set_obs(double newx, double newy);
+    void set_avg(double newx, double newy);
+    void set_target_pos(double newx, double newy);
+    void set_target_x(double x);
+    void set_target_y(double y);
+
+    std::array<double, 2> get_next_target();
+    void mark_current_target_reached();
     void reset();
 
-    double targetx, targety;
+    nlohmann::json get_json_data();
 
 private:
-    std::vector<double> setPose(const Pose3d& pose);
-    std::pair<nlohmann::json, double> setLaserValues();
+    void load_targets();
+    void laser_poll_loop();
+
+    double carx_, cary_;
+    double obsx_, obsy_;
+    double avgx_, avgy_;
+    double targetx_, targety_;
 
-    double carx, cary, obsx, obsy, avgx, avgy;
     std::vector<std::shared_ptr<Target>> targets_;
-    nlohmann::json payload_;
+    std::shared_ptr<LaserNode> laser_;
+    std::shared_ptr<OdometryNode> odom_;
+
+    LaserData cached_laser_;
+    std::mutex laser_mtx_;
+    std::thread laser_thread_;
+    std::atomic<bool> laser_running_;
 
-    std::function<LaserData()> laser_callback_;
-    std::function<Pose3d()> pose_callback_;
+    std::mutex map_mtx_;
 };
 
 #endif

exercises/obstacle_avoidance/cpp_lib/include/Map.hpp

@@ -1,58 +1,32 @@
 #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 "rclcpp/rclcpp.hpp"
-#include "geometry_msgs/msg/point.hpp"
-#include "std_msgs/msg/bool.hpp"
-#include "common_interfaces_cpp/hal/odometry.hpp"
-#include "common_interfaces_cpp/hal/laser.hpp"
-#include "Map.hpp"
-#include "Lap.hpp"
+#include <memory>
+#include <thread>
+#include <array>
 
-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 {
+class WebGUI
+{
 public:
-    WebGUI();
-    static void showForces(const std::vector<double>& v1, const std::vector<double>& v2, const std::vector<double>& v3);
-    static void showLocalTarget(const std::vector<double>& v);
-    static std::shared_ptr<Target> getNextTarget();
-    static void setTargetx(double x);
-    static void setTargety(double y);
-};
-
-class WebGUINode : public rclcpp::Node {
-public:
-    WebGUINode();
-    static std::shared_ptr<Map> map_;
-    static std::shared_ptr<Lap> lap_;
-    
-    static std::shared_ptr<OdometryNode> pose3d_node_;
-    static std::shared_ptr<LaserNode> laser_node_;
+    WebGUI() = delete;
 
-    void publish_current_target();
+    static void show_forces(const std::array<double, 2>& vec1, const std::array<double, 2>& vec2, const std::array<double, 2>& vec3);
+    static void show_local_target(const std::array<double, 2>& new_vec);
+    static std::array<double, 2> get_next_target();
+    static void set_target_x(double x);
+    static void set_target_y(double y);
+    static void mark_target_reached();
 
 private:
-    void target_reached_callback(std_msgs::msg::Bool::UniquePtr msg);
-    
-    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_car_;
-    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_obs_;
-    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_avg_;
-    rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_target_;
-    rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_reached_;
-    rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr pub_current_target_;
+    static void init();
+    friend class SystemBootstrapper;
 
-    std::shared_ptr<Target> current_target_obj_;
+    static std::shared_ptr<WebGUINode> gui_node_;
+    static std::shared_ptr<rclcpp::executors::MultiThreadedExecutor> executor_;
+    static std::thread spin_thread_;
 };
 
 #endif