71 lines
2.4 KiB
C++
71 lines
2.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::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);
|
|
}
|
|
}
|
|
}
|
|
|
|
} |