57 lines
1.8 KiB
C++
57 lines
1.8 KiB
C++
#pragma once
|
|
#include "handlers/dwm_core.hpp"
|
|
|
|
#include "glm/gtx/norm.hpp"
|
|
#include "glm/gtx/rotate_vector.hpp"
|
|
#include "glm/gtx/vector_angle.hpp"
|
|
|
|
namespace mg {
|
|
|
|
template <>
|
|
inline bool DWM<MgNavigationServer::MoveStraight>::target_check() {
|
|
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::MoveStraight>::target_init() {
|
|
target_pos = glm::rotate(glm::dvec2(goal->distance, 0), theta) + pos;
|
|
}
|
|
|
|
template <>
|
|
inline bool DWM<MgNavigationServer::MoveStraight>::condition_check() {
|
|
return false;
|
|
}
|
|
|
|
template <>
|
|
inline double DWM<MgNavigationServer::MoveStraight>::calc_score(int vl, int vr) {
|
|
constexpr double delta = 0.8;
|
|
|
|
auto [v, w] = to_vel(step * vl, step * vr);
|
|
|
|
const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
|
|
const glm::dvec2 n_pos = dp + pos;
|
|
|
|
const double score = goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
|
|
|
return score;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::MoveStraight>::populate_candidates(std::vector<int>& vl,
|
|
std::vector<int>& vr,
|
|
int dimx,
|
|
int) {
|
|
vl.clear();
|
|
vr.clear();
|
|
|
|
for (int i = -dimx / 2; i <= dimx / 2; i++) {
|
|
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + i));
|
|
|
|
if (step * abs(cvl + i) <= goal->max_wheel_speed && glm::abs(v) < goal->max_vel) {
|
|
vl.push_back(cvl + i);
|
|
vr.push_back(cvr + i);
|
|
}
|
|
}
|
|
}
|
|
|
|
} |