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

Fix: Rename DWM to DWA #7

Merged
Pimpest merged 1 commits from dwa-typofix into main 2025-03-23 09:31:00 +00:00
Conversation 0 Commits 1 Files Changed 8 +43 -43
8 changed files with 43 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Showing only changes of commit 2f279896a7 - Show all commits

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 { namespace mg {
template <typename T> template <typename T>
class DWM { class DWA {
public: public:
using Goal = typename T::Goal; using Goal = typename T::Goal;
using Result = typename T::Result; using Result = typename T::Result;
@ -62,7 +62,7 @@ namespace mg {
glm::dvec2 pos; glm::dvec2 pos;
double theta = 0; 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(); void execute();
@ -88,7 +88,7 @@ namespace mg {
}; };
template <typename T> 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), baseNode(mns),
hgoal(g), hgoal(g),
pub_twist(mns.pub_twist), pub_twist(mns.pub_twist),
@ -99,7 +99,7 @@ namespace mg {
pos(0, 0) { } pos(0, 0) { }
template <typename T> template <typename T>
void DWM<T>::execute() { void DWA<T>::execute() {
std::vector<int> spacevl(dimx * dimy); std::vector<int> spacevl(dimx * dimy);
std::vector<int> spacevr(dimx * dimy); std::vector<int> spacevr(dimx * dimy);
@ -144,7 +144,7 @@ namespace mg {
} }
template <typename T> template <typename T>
void DWM<T>::pos_query() { void DWA<T>::pos_query() {
try { try {
double x = NAN; double x = NAN;
double y = NAN; double y = NAN;
@ -164,7 +164,7 @@ namespace mg {
} }
template <typename T> 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); auto [v, w] = to_vel(vl, vr);
Geometry::TwistStamped twist; Geometry::TwistStamped twist;
twist.twist.angular.z = w; twist.twist.angular.z = w;
@ -174,7 +174,7 @@ namespace mg {
} }
template <typename T> template <typename T>
void DWM<T>::abort(int error) { void DWA<T>::abort(int error) {
baseNode.mtx.lock(); baseNode.mtx.lock();
send_speed(0, 0); send_speed(0, 0);
auto x = std::make_shared<Result>(); auto x = std::make_shared<Result>();
@ -183,7 +183,7 @@ namespace mg {
} }
template <typename T> template <typename T>
void DWM<T>::succeed() { void DWA<T>::succeed() {
baseNode.mtx.lock(); baseNode.mtx.lock();
send_speed(0, 0); send_speed(0, 0);
auto x = std::make_shared<Result>(); auto x = std::make_shared<Result>();
@ -192,7 +192,7 @@ namespace mg {
} }
template <typename T> template <typename T>
void DWM<T>::cancel() { void DWA<T>::cancel() {
baseNode.mtx.lock(); baseNode.mtx.lock();
send_speed(0, 0); send_speed(0, 0);
auto x = std::make_shared<Result>(); 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 #pragma once
#include "handlers/dwm_core.hpp" #include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp" #include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp" #include "glm/gtx/rotate_vector.hpp"
@ -8,22 +8,22 @@
namespace mg { namespace mg {
template <> 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; return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
} }
template <> 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; target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
} }
template <> template <>
inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() { inline bool DWA<MgNavigationServer::MoveStraight>::condition_check() {
return false; return false;
} }
template <> 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; constexpr double delta = 0.8;
auto [v, w] = to_vel(step * vl, step * vr); auto [v, w] = to_vel(step * vl, step * vr);
@ -37,7 +37,7 @@ namespace mg {
} }
template <> 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, std::vector<int>& vr,
int dimx, int dimx,
int) { int) {

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

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

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

@ -1,5 +1,5 @@
#pragma once #pragma once
#include "handlers/dwm_core.hpp" #include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp" #include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp" #include "glm/gtx/rotate_vector.hpp"
@ -8,22 +8,22 @@
namespace mg { namespace mg {
template <> 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; return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
} }
template <> template <>
inline void DWM<MgNavigationServer::MovePoint>::target_update() { inline void DWA<MgNavigationServer::MovePoint>::target_update() {
target_pos = glm::dvec2(goal->x, goal->y); target_pos = glm::dvec2(goal->x, goal->y);
} }
template <> template <>
inline bool DWM<MgNavigationServer::MovePoint>::condition_check() { inline bool DWA<MgNavigationServer::MovePoint>::condition_check() {
return false; return false;
} }
template <> 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); auto [v, w] = to_vel(step * vl, step * vr);
const double delta = 0.5; const double delta = 0.5;
glm::dvec2 n_pos; glm::dvec2 n_pos;
@ -57,7 +57,7 @@ namespace mg {
} }
template <> 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, std::vector<int>& vr,
int dimx, int dimx,
int dimy) { 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 #pragma once
#include "handlers/dwm_core.hpp" #include "handlers/dwa_core.hpp"
#include "glm/gtx/norm.hpp" #include "glm/gtx/norm.hpp"
#include "glm/gtx/rotate_vector.hpp" #include "glm/gtx/rotate_vector.hpp"
@ -9,7 +9,7 @@
namespace mg { namespace mg {
template <> 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 a = glm::abs(theta - target_ornt);
const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a; const double b = (a > glm::pi<double>()) ? glm::two_pi<double>() - a : a;
@ -18,17 +18,17 @@ namespace mg {
} }
template <> template <>
inline void DWM<MgNavigationServer::Rotate>::target_init() { inline void DWA<MgNavigationServer::Rotate>::target_init() {
target_ornt = goal->angle; target_ornt = goal->angle;
} }
template <> template <>
inline bool DWM<MgNavigationServer::Rotate>::condition_check() { inline bool DWA<MgNavigationServer::Rotate>::condition_check() {
return false; return false;
} }
template <> 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; constexpr double delta = 0.5;
const auto [v, w] = to_vel(step * vl, step * vr); const auto [v, w] = to_vel(step * vl, step * vr);
const double n_theta = theta + w * delta; const double n_theta = theta + w * delta;
@ -45,7 +45,7 @@ namespace mg {
} }
template <> 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, std::vector<int>& vr,
int dimx, int dimx,
int) { 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 "rclcpp/rclcpp.hpp"
#include "mg_navigation.hpp" #include "mg_navigation.hpp"
#include "handlers/dwm.hpp" #include "handlers/dwa.hpp"
namespace mg { namespace mg {
@ -40,7 +40,7 @@ namespace mg {
template <typename T> template <typename T>
void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) { 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(); dwm.execute();
is_processing = false; is_processing = false;
mtx.unlock(); mtx.unlock();