Skip to content

Commit c0a0854

Browse files
javizqhjavizqh
authored
Merge pull request #3786 from JdeRobot/dynamic_window_approach
Dynamic window approach
2 parents 7fdd0e6 + f6ea734 commit c0a0854

37 files changed

Lines changed: 2150 additions & 0 deletions

‎database/exercises/db.sql‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -137,6 +137,7 @@ COPY public.exercises (id, exercise_id, name, description, tags, status, url) FR
137137
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
138138
25 machine_vision Machine Vision Machine Vision exercise ["ROS2"] PROTOTYPE https://jderobot.github.io/RoboticsAcademy/exercises/IndustrialRobots/machine_vision
139139
26 labyrinth_escape Labyrinth Escape Labyrinth Escape exercise ["ROS2","Drones"] ACTIVE https://jderobot.github.io/RoboticsAcademy/exercises/Drones/labyrinth_escape
140+
27 dynamic_window_approach Local Navigation Dynamic Window Approach exercise ["ROS2","AUTONOMOUS DRIVING"] PROTOTYPE https://jderobot.github.io/RoboticsAcademy/exercises/AutonomousCars/obstacle_avoidance
140141
\.
141142

142143
--
@@ -214,6 +215,7 @@ COPY public.exercises_universes (id, exercise_id, universe_id, is_default) FROM
214215
67 14 63 False
215216
68 14 64 False
216217
69 14 65 False
218+
70 27 40 False
217219
\.
218220

219221
--
@@ -303,6 +305,9 @@ COPY public.exercises_tools (id, exercise_id, tool_id) FROM stdin;
303305
79 15 video
304306
80 18 video
305307
81 19 video
308+
82 27 console
309+
83 27 simulator
310+
84 27 web_gui
306311
\.
307312

308313
--

‎exercises/dynamic_window_approach/cpp_lib/CMakeLists.txt‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
cmake_minimum_required(VERSION 3.10)
2+
project(ObstacleAvoidanceLibs CXX)
3+
4+
set(CMAKE_CXX_STANDARD 17)
5+
set(CMAKE_CXX_STANDARD_REQUIRED ON)
6+
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
7+
8+
find_package(ament_cmake REQUIRED)
9+
10+
set(ROS2_PACKAGES
11+
rclcpp
12+
nav_msgs
13+
geometry_msgs
14+
sensor_msgs
15+
std_msgs
16+
)
17+
18+
foreach(pkg IN LISTS ROS2_PACKAGES)
19+
find_package(${pkg} REQUIRED)
20+
endforeach()
21+
22+
find_package(Boost REQUIRED COMPONENTS system)
23+
find_package(nlohmann_json REQUIRED)
24+
25+
set(COMMON_INTERFACES_INCLUDE "/home/ws/install/common_interfaces_cpp/include")
26+
set(COMMON_INTERFACES_LIB "/home/ws/install/common_interfaces_cpp/lib/libcommon_interfaces_cpp.so")
27+
28+
set(WEBGUI_ROS_DEPS rclcpp geometry_msgs std_msgs nav_msgs sensor_msgs)
29+
set(WEBGUI_SYS_DEPS Frequency Boost::system nlohmann_json::nlohmann_json)
30+
set(HAL_ROS_DEPS rclcpp geometry_msgs sensor_msgs nav_msgs)
31+
32+
33+
# --- libFrequency.so ---
34+
add_library(Frequency SHARED src/Frequency.cpp)
35+
target_include_directories(Frequency PUBLIC include)
36+
37+
38+
# --- libWebGUI.so ---
39+
add_library(WebGUI SHARED
40+
src/WebGUI.cpp
41+
src/Map.cpp
42+
src/Lap.cpp
43+
)
44+
target_include_directories(WebGUI PUBLIC include ${COMMON_INTERFACES_INCLUDE})
45+
ament_target_dependencies(WebGUI ${WEBGUI_ROS_DEPS})
46+
target_link_libraries(WebGUI ${WEBGUI_SYS_DEPS} ${COMMON_INTERFACES_LIB})
47+
48+
49+
# --- libHAL.so ---
50+
add_library(HAL SHARED src/HAL.cpp)
51+
target_include_directories(HAL PUBLIC include ${COMMON_INTERFACES_INCLUDE})
52+
ament_target_dependencies(HAL ${HAL_ROS_DEPS})
53+
target_link_libraries(HAL ${COMMON_INTERFACES_LIB})

‎exercises/dynamic_window_approach/cpp_lib/include/Frequency.hpp‎

Lines changed: 22 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
#ifndef INCLUDE_FREQUENCY_HPP_
2+
#define INCLUDE_FREQUENCY_HPP_
3+
4+
#include <chrono>
5+
#include <cmath>
6+
#include <thread>
7+
8+
class Frequency
9+
{
10+
private:
11+
std::chrono::high_resolution_clock::time_point last_time;
12+
int is_first_iteration;
13+
14+
public:
15+
Frequency();
16+
~Frequency();
17+
18+
static int rate;
19+
void tick(int ideal_cycle = 50);
20+
};
21+
22+
#endif

‎exercises/dynamic_window_approach/cpp_lib/include/HAL.hpp‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
#ifndef INCLUDE_HAL_HPP_
2+
#define INCLUDE_HAL_HPP_
3+
4+
#include <vector>
5+
#include <memory>
6+
#include <thread>
7+
#include <chrono>
8+
#include "rclcpp/rclcpp.hpp"
9+
#include "common_interfaces_cpp/hal/motors.hpp"
10+
#include "common_interfaces_cpp/hal/laser.hpp"
11+
#include "common_interfaces_cpp/hal/odometry.hpp"
12+
13+
class HAL : public rclcpp::Node
14+
{
15+
public:
16+
HAL();
17+
static void set_v(const double velocity);
18+
static void set_w(const double velocity);
19+
static Pose3d get_pose3d();
20+
static const LaserData *get_laser_data();
21+
22+
private:
23+
static std::shared_ptr<OdometryNode> pose3d_node_;
24+
static std::shared_ptr<MotorsNode> motors_node_;
25+
static std::shared_ptr<LaserNode> laser_node_;
26+
std::thread spin_thread_;
27+
};
28+
29+
#endif

‎exercises/dynamic_window_approach/cpp_lib/include/Lap.hpp‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
#ifndef INCLUDE_LAP_HPP_
2+
#define INCLUDE_LAP_HPP_
3+
4+
#include <string>
5+
#include <chrono>
6+
#include <memory>
7+
#include "Map.hpp"
8+
9+
class Lap {
10+
public:
11+
Lap(std::shared_ptr<Map> map_object);
12+
13+
double check_threshold();
14+
double return_lap_time() const;
15+
void reset();
16+
void pause();
17+
void unpause();
18+
19+
private:
20+
std::shared_ptr<Map> map_;
21+
std::string target_start_;
22+
std::string target_end_;
23+
24+
std::chrono::time_point<std::chrono::system_clock> start_time_;
25+
std::chrono::duration<double> lap_time_;
26+
bool buffer_;
27+
bool pause_condition_;
28+
};
29+
30+
#endif

‎exercises/dynamic_window_approach/cpp_lib/include/Map.hpp‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
#ifndef INCLUDE_MAP_HPP_
2+
#define INCLUDE_MAP_HPP_
3+
4+
#include <string>
5+
#include <vector>
6+
#include <memory>
7+
#include <functional>
8+
#include <nlohmann/json.hpp>
9+
#include "common_interfaces_cpp/hal/odometry.hpp"
10+
#include "common_interfaces_cpp/hal/laser.hpp"
11+
12+
class Target {
13+
public:
14+
Target(const std::string& id, const Pose3d& pose, bool active = false, bool reached = false);
15+
std::string getId() const;
16+
Pose3d getPose() const;
17+
bool isReached() const;
18+
void setReached(bool value);
19+
20+
std::string id;
21+
Pose3d pose;
22+
bool reached;
23+
bool active;
24+
};
25+
26+
class Map {
27+
public:
28+
Map(std::function<LaserData()> laser_cb, std::function<Pose3d()> pose_cb);
29+
30+
void setCar(double x, double y);
31+
void setObs(double x, double y);
32+
void setAvg(double x, double y);
33+
void setTargetPos(double x, double y);
34+
35+
std::string get_json_data();
36+
std::shared_ptr<Target> getNextTarget();
37+
void reset();
38+
39+
double targetx, targety;
40+
41+
private:
42+
std::vector<double> setPose(const Pose3d& pose);
43+
std::pair<nlohmann::json, double> setLaserValues();
44+
45+
double carx, cary, obsx, obsy, avgx, avgy;
46+
std::vector<std::shared_ptr<Target>> targets_;
47+
nlohmann::json payload_;
48+
49+
std::function<LaserData()> laser_callback_;
50+
std::function<Pose3d()> pose_callback_;
51+
};
52+
53+
#endif

‎exercises/dynamic_window_approach/cpp_lib/include/WebGUI.hpp‎

Lines changed: 58 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,58 @@
1+
#ifndef INCLUDE_WEBGUI_HPP_
2+
#define INCLUDE_WEBGUI_HPP_
3+
4+
#include <boost/beast/core.hpp>
5+
#include <boost/beast/websocket.hpp>
6+
#include <boost/asio/connect.hpp>
7+
#include <boost/asio/ip/tcp.hpp>
8+
#include <boost/asio/strand.hpp>
9+
#include <nlohmann/json.hpp>
10+
#include "rclcpp/rclcpp.hpp"
11+
#include "geometry_msgs/msg/point.hpp"
12+
#include "std_msgs/msg/bool.hpp"
13+
#include "common_interfaces_cpp/hal/odometry.hpp"
14+
#include "common_interfaces_cpp/hal/laser.hpp"
15+
#include "Map.hpp"
16+
#include "Lap.hpp"
17+
18+
namespace beast = boost::beast;
19+
namespace websocket = beast::websocket;
20+
namespace net = boost::asio;
21+
using tcp = net::ip::tcp;
22+
using json = nlohmann::json;
23+
24+
class WebGUI {
25+
public:
26+
WebGUI();
27+
static void showForces(const std::vector<double>& v1, const std::vector<double>& v2, const std::vector<double>& v3);
28+
static void showLocalTarget(const std::vector<double>& v);
29+
static std::shared_ptr<Target> getNextTarget();
30+
static void setTargetx(double x);
31+
static void setTargety(double y);
32+
};
33+
34+
class WebGUINode : public rclcpp::Node {
35+
public:
36+
WebGUINode();
37+
static std::shared_ptr<Map> map_;
38+
static std::shared_ptr<Lap> lap_;
39+
40+
static std::shared_ptr<OdometryNode> pose3d_node_;
41+
static std::shared_ptr<LaserNode> laser_node_;
42+
43+
void publish_current_target();
44+
45+
private:
46+
void target_reached_callback(std_msgs::msg::Bool::UniquePtr msg);
47+
48+
rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_car_;
49+
rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_obs_;
50+
rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_avg_;
51+
rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr sub_target_;
52+
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr sub_reached_;
53+
rclcpp::Publisher<geometry_msgs::msg::Point>::SharedPtr pub_current_target_;
54+
55+
std::shared_ptr<Target> current_target_obj_;
56+
};
57+
58+
#endif

‎exercises/dynamic_window_approach/cpp_lib/src/Frequency.cpp‎

Lines changed: 34 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,34 @@
1+
#include "Frequency.hpp"
2+
3+
int Frequency::rate = 0;
4+
5+
Frequency::Frequency() : is_first_iteration(1) {}
6+
7+
Frequency::~Frequency() {}
8+
9+
void Frequency::tick(int ideal_cycle)
10+
{
11+
const auto ideal_ms = std::chrono::milliseconds(1000 / ideal_cycle);
12+
const auto now = std::chrono::high_resolution_clock::now();
13+
14+
if (is_first_iteration == 1)
15+
{
16+
last_time = now;
17+
is_first_iteration = 0;
18+
return;
19+
}
20+
21+
const auto iter_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now - last_time);
22+
23+
if (iter_ms < ideal_ms)
24+
{
25+
rate = std::round(1000.0 / ideal_ms.count());
26+
std::this_thread::sleep_for(ideal_ms - iter_ms);
27+
}
28+
else
29+
{
30+
rate = std::round(1000.0 / iter_ms.count());
31+
}
32+
33+
last_time = now;
34+
}

‎exercises/dynamic_window_approach/cpp_lib/src/HAL.cpp‎

Lines changed: 53 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,53 @@
1+
#include "HAL.hpp"
2+
3+
using namespace std::chrono_literals;
4+
5+
std::shared_ptr<OdometryNode> HAL::pose3d_node_ = nullptr;
6+
std::shared_ptr<MotorsNode> HAL::motors_node_ = nullptr;
7+
std::shared_ptr<LaserNode> HAL::laser_node_ = nullptr;
8+
9+
HAL::HAL() : Node("hal_node")
10+
{
11+
pose3d_node_ = std::make_shared<OdometryNode>("/odom");
12+
motors_node_ = std::make_shared<MotorsNode>("/cmd_vel", 4.0, 0.3);
13+
laser_node_ = std::make_shared<LaserNode>("/f1/laser/scan");
14+
15+
spin_thread_ = std::thread([]() {
16+
rclcpp::executors::MultiThreadedExecutor executor;
17+
executor.add_node(HAL::pose3d_node_);
18+
executor.add_node(HAL::motors_node_);
19+
executor.add_node(HAL::laser_node_);
20+
21+
while (rclcpp::ok()) {
22+
executor.spin_some();
23+
std::this_thread::sleep_for(std::chrono::milliseconds(10));
24+
}
25+
});
26+
spin_thread_.detach();
27+
}
28+
29+
void HAL::set_v(const double velocity)
30+
{
31+
if (motors_node_) motors_node_->sendV(velocity);
32+
}
33+
34+
void HAL::set_w(const double velocity)
35+
{
36+
if (motors_node_) motors_node_->sendW(velocity);
37+
}
38+
39+
Pose3d HAL::get_pose3d()
40+
{
41+
if (pose3d_node_) return pose3d_node_->getPose3d();
42+
return Pose3d();
43+
}
44+
45+
const LaserData *HAL::get_laser_data()
46+
{
47+
if (!laser_node_) return nullptr;
48+
LaserData data = laser_node_->getLaserData();
49+
while (data.values.empty() && rclcpp::ok()) {
50+
data = laser_node_->getLaserData();
51+
}
52+
return new LaserData(data);
53+
}

0 commit comments

Comments
 (0)