Magnezijum-Robotics/magrob
2
0
Fork 0
Code Issues Pull Requests 1 Actions Packages Projects Releases Wiki Activity

DWM -> DWA (Dynamic window approach)

Browse Source
This commit is contained in:
Pimpest
2025-03-23 10:26:47 +01:00
parent 39d1f5e8dc
commit 2f279896a7
8 changed files with 43 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
mg_navigation/include/handlers/dwa.hpp Normal file
View File

@ -0,0 +1,7 @@
#pragma once
#include "handlers/dwa_core.hpp"
#include "handlers/dwa_point.hpp"
#include "handlers/dwa_forward.hpp"
#include "handlers/dwa_lookat.hpp"
#include "handlers/dwa_rotate.hpp"

18
mg_navigation/include/handlers/dwm_core.hpp → mg_navigation/include/handlers/dwa_core.hpp
View File

@ -30,7 +30,7 @@
namespace mg {
template <typename T>
class DWM {
class DWA {
public:
using Goal = typename T::Goal;
using Result = typename T::Result;
@ -62,7 +62,7 @@ namespace mg {
glm::dvec2 pos;
double theta = 0;
DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
DWA(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
void execute();
@ -88,7 +88,7 @@ namespace mg {
};
template <typename T>
DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
DWA<T>::DWA(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
baseNode(mns),
hgoal(g),
pub_twist(mns.pub_twist),
@ -99,7 +99,7 @@ namespace mg {
pos(0, 0) { }
template <typename T>
void DWM<T>::execute() {
void DWA<T>::execute() {
std::vector<int> spacevl(dimx * dimy);
std::vector<int> spacevr(dimx * dimy);
@ -144,7 +144,7 @@ namespace mg {
}
template <typename T>
void DWM<T>::pos_query() {
void DWA<T>::pos_query() {
try {
double x = NAN;
double y = NAN;
@ -164,7 +164,7 @@ namespace mg {
}
template <typename T>
void DWM<T>::send_speed(double vl, double vr) {
void DWA<T>::send_speed(double vl, double vr) {
auto [v, w] = to_vel(vl, vr);
Geometry::TwistStamped twist;
twist.twist.angular.z = w;
@ -174,7 +174,7 @@ namespace mg {
}
template <typename T>
void DWM<T>::abort(int error) {
void DWA<T>::abort(int error) {
baseNode.mtx.lock();
send_speed(0, 0);
auto x = std::make_shared<Result>();
@ -183,7 +183,7 @@ namespace mg {
}
template <typename T>
void DWM<T>::succeed() {
void DWA<T>::succeed() {
baseNode.mtx.lock();
send_speed(0, 0);
auto x = std::make_shared<Result>();
@ -192,7 +192,7 @@ namespace mg {
}
template <typename T>
void DWM<T>::cancel() {
void DWA<T>::cancel() {
baseNode.mtx.lock();
send_speed(0, 0);
auto x = std::make_shared<Result>();

12
mg_navigation/include/handlers/dwm_forward.hpp → mg_navigation/include/handlers/dwa_forward.hpp
View File

@ -1,5 +1,5 @@
#pragma once
#include "handlers/dwm_core.hpp"
#include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
@ -8,22 +8,22 @@
namespace mg {
template <>
inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
inline bool DWA<MgNavigationServer::MoveStraight>::target_check() {
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
}
template <>
inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
inline void DWA<MgNavigationServer::MoveStraight>::target_init() {
target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
}
template <>
inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
return false;
}
template <>
inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
inline double DWA<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
constexpr double delta = 0.8;
auto [v, w] = to_vel(step * vl, step * vr);
@ -37,7 +37,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
inline void DWA<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
std::vector<int>& vr,
int dimx,
int) {

14
mg_navigation/include/handlers/dwm_lookat.hpp → mg_navigation/include/handlers/dwa_lookat.hpp
View File

@ -1,5 +1,5 @@
#pragma once
#include "handlers/dwm_core.hpp"
#include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
@ -9,7 +9,7 @@
namespace mg {
template <>
inline bool DWM<MgNavigationServer::LookAt>::target_check() {
inline bool DWA<MgNavigationServer::LookAt>::target_check() {
const double a = glm::abs(theta - target_ornt);
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
@ -18,7 +18,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::LookAt>::target_init() {
inline void DWA<MgNavigationServer::LookAt>::target_init() {
target_pos = glm::vec2(goal->x, goal->y);
@ -30,7 +30,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::LookAt>::target_update() {
inline void DWA<MgNavigationServer::LookAt>::target_update() {
const double angle = glm::angle(glm::dvec2(1, 0), glm::normalize(target_pos - pos));
const double cross = glm::cross(glm::dvec3(1, 0, 0), glm::dvec3(target_pos - pos, 0)).z;
@ -40,12 +40,12 @@ namespace mg {
}
template <>
inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
inline bool DWA<MgNavigationServer::LookAt>::condition_check() {
return false;
}
template <>
inline double DWM<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
inline double DWA<MgNavigationServer::LookAt>::calc_score(int vl, int vr) {
constexpr double delta = 0.5;
const auto [v, w] = to_vel(step * vl, step * vr);
const double n_theta = theta + w * delta;
@ -62,7 +62,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
inline void DWA<MgNavigationServer::LookAt>::populate_candidates(std::vector<int>& vl,
std::vector<int>& vr,
int dimx,
int) {

12
mg_navigation/include/handlers/dwm_point.hpp → mg_navigation/include/handlers/dwa_point.hpp
View File

@ -1,5 +1,5 @@
#pragma once
#include "handlers/dwm_core.hpp"
#include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
@ -8,22 +8,22 @@
namespace mg {
template <>
inline bool DWM<MgNavigationServer::MovePoint>::target_check() {
inline bool DWA<MgNavigationServer::MovePoint>::target_check() {
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
}
template <>
inline void DWM<MgNavigationServer::MovePoint>::target_update() {
inline void DWA<MgNavigationServer::MovePoint>::target_update() {
target_pos = glm::dvec2(goal->x, goal->y);
}
template <>
inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
return false;
}
template <>
inline double DWM<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
inline double DWA<MgNavigationServer::MovePoint>::calc_score(int vl, int vr) {
auto [v, w] = to_vel(step * vl, step * vr);
const double delta = 0.5;
glm::dvec2 n_pos;
@ -57,7 +57,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
inline void DWA<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
std::vector<int>& vr,
int dimx,
int dimy) {

12
mg_navigation/include/handlers/dwm_rotate.hpp → mg_navigation/include/handlers/dwa_rotate.hpp
View File

@ -1,5 +1,5 @@
#pragma once
#include "handlers/dwm_core.hpp"
#include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp"
@ -9,7 +9,7 @@
namespace mg {
template <>
inline bool DWM<MgNavigationServer::Rotate>::target_check() {
inline bool DWA<MgNavigationServer::Rotate>::target_check() {
const double a = glm::abs(theta - target_ornt);
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
@ -18,17 +18,17 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::Rotate>::target_init() {
inline void DWA<MgNavigationServer::Rotate>::target_init() {
target_ornt = goal->angle;
}
template <>
inline bool DWM<MgNavigationServer::Rotate>::condition_check() {
inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
return false;
}
template <>
inline double DWM<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
inline double DWA<MgNavigationServer::Rotate>::calc_score(int vl, int vr) {
constexpr double delta = 0.5;
const auto [v, w] = to_vel(step * vl, step * vr);
const double n_theta = theta + w * delta;
@ -45,7 +45,7 @@ namespace mg {
}
template <>
inline void DWM<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
inline void DWA<MgNavigationServer::Rotate>::populate_candidates(std::vector<int>& vl,
std::vector<int>& vr,
int dimx,
int) {

7
mg_navigation/include/handlers/dwm.hpp
View File

@ -1,7 +0,0 @@
#pragma once
#include "handlers/dwm_core.hpp"
#include "handlers/dwm_point.hpp"
#include "handlers/dwm_forward.hpp"
#include "handlers/dwm_lookat.hpp"
#include "handlers/dwm_rotate.hpp"

4
mg_navigation/src/mg_navigation.cpp
View File

@ -4,7 +4,7 @@
#include "rclcpp/rclcpp.hpp"
#include "mg_navigation.hpp"
#include "handlers/dwm.hpp"
#include "handlers/dwa.hpp"
namespace mg {
@ -40,7 +40,7 @@ namespace mg {
template <typename T>
void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
DWA<T> dwm = DWA<T>(gh, tf2_buffer, *this);
dwm.execute();
is_processing = false;
mtx.unlock();