changed files for DWA exercise

Author: jmjimenez2020

database/exercises/db.sql

@@ -137,6 +137,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
 24	car_junction	Car Junction	Autonomous Navigation through traffic at road junction.	["AUTONOMOUS DRIVING","ROS2"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/car_junction
 25	machine_vision	Machine Vision	Machine Vision exercise	["ROS2"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/machine_vision
 26	labyrinth_escape	Labyrinth Escape	Labyrinth Escape exercise	["ROS2","Drones"]	ACTIVE	https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
+27	dynamic_window_approach	Local Navigation	Dynamic Window Approach exercise	["ROS2","AUTONOMOUS DRIVING"]	PROTOTYPE	https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/obstacle_avoidance
 \.
 
 --
@@ -214,6 +215,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id, is_default) FROM
 67	14	63	False
 68	14	64	False
 69	14	65	False
+70	27	68	False
 \.
 
 --
@@ -303,6 +305,9 @@ COPY public.exercises_tools (id, exercise_id, tool_id) FROM stdin;
 79	15	video
 80	18	video
 81	19	video
+82	27	console
+83	27	simulator
+84	27	web_gui
 \.
 
 --

exercises/dynamic_window_approach/cpp_lib/CMakeLists.txt

@@ -0,0 +1,53 @@
+cmake_minimum_required(VERSION 3.10)
+project(ObstacleAvoidanceLibs 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
+    geometry_msgs
+    sensor_msgs
+    std_msgs
+)
+
+foreach(pkg IN LISTS ROS2_PACKAGES)
+    find_package(${pkg} REQUIRED)
+endforeach()
+
+find_package(Boost REQUIRED COMPONENTS system)
+find_package(nlohmann_json 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(HAL_ROS_DEPS     rclcpp geometry_msgs sensor_msgs nav_msgs)
+
+
+# --- 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})
+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})
+ament_target_dependencies(HAL ${HAL_ROS_DEPS})
+target_link_libraries(HAL ${COMMON_INTERFACES_LIB})

exercises/dynamic_window_approach/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/dynamic_window_approach/cpp_lib/include/HAL.hpp

@@ -0,0 +1,29 @@
+#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"
+
+class HAL : public rclcpp::Node
+{
+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();
+
+private:
+    static std::shared_ptr<OdometryNode> pose3d_node_;
+    static std::shared_ptr<MotorsNode> motors_node_;
+    static std::shared_ptr<LaserNode> laser_node_;
+    std::thread spin_thread_;
+};
+
+#endif

exercises/dynamic_window_approach/cpp_lib/include/Lap.hpp

@@ -0,0 +1,30 @@
+#ifndef INCLUDE_LAP_HPP_
+#define INCLUDE_LAP_HPP_
+
+#include <string>
+#include <chrono>
+#include <memory>
+#include "Map.hpp"
+
+class Lap {
+public:
+    Lap(std::shared_ptr<Map> map_object);
+
+    double check_threshold();
+    double return_lap_time() const;
+    void reset();
+    void pause();
+    void unpause();
+
+private:
+    std::shared_ptr<Map> map_;
+    std::string target_start_;
+    std::string target_end_;
+
+    std::chrono::time_point<std::chrono::system_clock> start_time_;
+    std::chrono::duration<double> lap_time_;
+    bool buffer_;
+    bool pause_condition_;
+};
+
+#endif

exercises/dynamic_window_approach/cpp_lib/include/Map.hpp

@@ -0,0 +1,53 @@
+#ifndef INCLUDE_MAP_HPP_
+#define INCLUDE_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"
+
+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;
+};
+
+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();
+    void reset();
+
+    double targetx, targety;
+
+private:
+    std::vector<double> setPose(const Pose3d& pose);
+    std::pair<nlohmann::json, double> setLaserValues();
+
+    double carx, cary, obsx, obsy, avgx, avgy;
+    std::vector<std::shared_ptr<Target>> targets_;
+    nlohmann::json payload_;
+
+    std::function<LaserData()> laser_callback_;
+    std::function<Pose3d()> pose_callback_;
+};
+
+#endif

exercises/dynamic_window_approach/cpp_lib/include/WebGUI.hpp

@@ -0,0 +1,58 @@
+#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"
+
+namespace beast = boost::beast;
+namespace websocket = beast::websocket;
+namespace net = boost::asio;
+using tcp = net::ip::tcp;
+using json = nlohmann::json;
+
+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_;
+
+    void publish_current_target();
+
+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_;
+
+    std::shared_ptr<Target> current_target_obj_;
+};
+
+#endif

exercises/dynamic_window_approach/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/dynamic_window_approach/cpp_lib/src/HAL.cpp

@@ -0,0 +1,53 @@
+#include "HAL.hpp"
+
+using namespace std::chrono_literals;
+
+std::shared_ptr<OdometryNode> HAL::pose3d_node_ = nullptr;
+std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
+std::shared_ptr<LaserNode> HAL::laser_node_ = nullptr;
+
+HAL::HAL() : Node("hal_node")
+{
+    pose3d_node_ = std::make_shared<OdometryNode>("/odom");
+    motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 4.0, 0.3);
+    laser_node_ = std::make_shared<LaserNode>("/f1/laser/scan");
+
+    spin_thread_ = std::thread([]() {
+        rclcpp::executors::MultiThreadedExecutor executor;
+        executor.add_node(HAL::pose3d_node_);
+        executor.add_node(HAL::motors_node_);
+        executor.add_node(HAL::laser_node_);
+        
+        while (rclcpp::ok()) {
+            executor.spin_some();
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    });
+    spin_thread_.detach();
+}
+
+void HAL::set_v(const double velocity)
+{
+    if (motors_node_) motors_node_->sendV(velocity);
+}
+
+void HAL::set_w(const double velocity)
+{
+    if (motors_node_) motors_node_->sendW(velocity);
+}
+
+Pose3d HAL::get_pose3d()
+{
+    if (pose3d_node_) return pose3d_node_->getPose3d();
+    return Pose3d();
+}
+
+const LaserData *HAL::get_laser_data()
+{
+    if (!laser_node_) return nullptr;
+    LaserData data = laser_node_->getLaserData();
+    while (data.values.empty() && rclcpp::ok()) {
+        data = laser_node_->getLaserData();
+    }
+    return new LaserData(data);
+}