94 lines
3.4 KiB
C++
94 lines
3.4 KiB
C++
/** Copyright 2025 The Magrob Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#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::MovePoint>::target_check() {
|
|
return glm::length2(pos - target_pos) > goal->tolerance * goal->tolerance;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::MovePoint>::target_update() {
|
|
target_pos = glm::dvec2(goal->x, goal->y);
|
|
}
|
|
|
|
template <>
|
|
inline bool DWM<MgNavigationServer::MovePoint>::condition_check() {
|
|
return false;
|
|
}
|
|
|
|
template <>
|
|
inline double DWM<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;
|
|
double n_theta = NAN;
|
|
double score = 0;
|
|
|
|
n_theta = w * delta;
|
|
|
|
if (n_theta <= 1e-6) { //NOLINT
|
|
n_theta += theta;
|
|
const glm::dvec2 dp = glm::rotate(glm::dvec2(1, 0), theta) * delta * v;
|
|
n_pos = dp + pos;
|
|
} else {
|
|
n_theta += theta;
|
|
const double r = v / w;
|
|
n_pos.x = r * (glm::sin(n_theta) - glm::sin(theta));
|
|
n_pos.y = -r * (glm::cos(n_theta) - glm::cos(theta));
|
|
n_pos += pos;
|
|
}
|
|
|
|
const glm::dvec2 n_face = glm::rotate(glm::dvec2(1, 0), n_theta);
|
|
const glm::dvec2 face = glm::rotate(glm::dvec2(1, 0), theta);
|
|
|
|
const double angl = glm::angle(face, glm::normalize(target_pos - pos));
|
|
const double n_angl = glm::angle(n_face, glm::normalize(target_pos - n_pos));
|
|
|
|
score += goal->pos_mult * (glm::distance(target_pos, pos) - glm::distance(target_pos, n_pos));
|
|
score += goal->ornt_mult * (angl - n_angl);
|
|
|
|
return score;
|
|
}
|
|
|
|
template <>
|
|
inline void DWM<MgNavigationServer::MovePoint>::populate_candidates(std::vector<int>& vl,
|
|
std::vector<int>& vr,
|
|
int dimx,
|
|
int dimy) {
|
|
vl.clear();
|
|
vr.clear();
|
|
|
|
for (int i = -dimx / 2; i <= dimx / 2; i++) {
|
|
for (int j = -dimy / 2; j <= dimy / 2; j++) {
|
|
auto [v, w] = to_vel(step * (cvl + i), step * (cvr + j));
|
|
|
|
if (step * abs(cvl + i) <= goal->max_wheel_speed && step * abs(cvr + j) <= goal->max_wheel_speed
|
|
&& glm::abs(v) < goal->max_vel && glm::abs(w) < goal->max_angular) {
|
|
vl.push_back(cvl + i);
|
|
vr.push_back(cvr + j);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
} |