Implented point to point DWM action server
This commit is contained in:
6
mg_navigation/include/handlers/dwm.hpp
Normal file
6
mg_navigation/include/handlers/dwm.hpp
Normal file
@ -0,0 +1,6 @@
|
||||
#pragma once
|
||||
|
||||
#include "handlers/dwm_core.hpp"
|
||||
#include "handlers/dwm_point.hpp"
|
||||
#include "handlers/dwm_forward.hpp"
|
||||
#include "handlers/dwm_lookat.hpp"
|
||||
203
mg_navigation/include/handlers/dwm_core.hpp
Normal file
203
mg_navigation/include/handlers/dwm_core.hpp
Normal file
@ -0,0 +1,203 @@
|
||||
#pragma once
|
||||
|
||||
#include "glm/glm.hpp"
|
||||
#include "mg_navigation.hpp"
|
||||
|
||||
#include "tf2/LinearMath/Transform.h"
|
||||
#include "tf2/convert.h"
|
||||
#include <sys/types.h>
|
||||
|
||||
#ifndef WHEEL_SEP
|
||||
#define WHEEL_SEP 0.258
|
||||
#endif
|
||||
|
||||
#ifndef WHEEL_RAD
|
||||
#define WHEEL_RAD ((0.075 / 2))
|
||||
#endif
|
||||
|
||||
#ifndef UPDATE_RATE
|
||||
#define UPDATE_RATE 100
|
||||
#endif
|
||||
|
||||
#ifndef DIM_X
|
||||
#define DIM_X 8
|
||||
#endif
|
||||
|
||||
#ifndef DIM_Y
|
||||
#define DIM_Y 8
|
||||
#endif
|
||||
|
||||
namespace mg {
|
||||
|
||||
template <typename T>
|
||||
class DWM {
|
||||
public:
|
||||
using Goal = typename T::Goal;
|
||||
using Result = typename T::Result;
|
||||
using Feedback = typename T::Feedback;
|
||||
using GoalHandle = typename rclcpp_action::ServerGoalHandle<T>;
|
||||
using TwistPub = rclcpp::Publisher<Geometry::TwistStamped>;
|
||||
|
||||
enum DwmError {
|
||||
SUCCESS,
|
||||
BLOCKED,
|
||||
TIMEOUT,
|
||||
MISALIGNED,
|
||||
|
||||
UNKNOWN = 254
|
||||
};
|
||||
|
||||
MgNavigationServer& baseNode; // NOLINT
|
||||
std::shared_ptr<GoalHandle> hgoal;
|
||||
TwistPub::SharedPtr pub_twist;
|
||||
typename Goal::ConstSharedPtr goal;
|
||||
tf2_ros::Buffer::SharedPtr tfbuf;
|
||||
ulong dimx = DIM_X, dimy = DIM_Y;
|
||||
int cvl = 0, cvr = 0;
|
||||
double step;
|
||||
|
||||
glm::dvec2 target_pos;
|
||||
double target_ornt = 0;
|
||||
|
||||
glm::dvec2 pos;
|
||||
double theta = 0;
|
||||
|
||||
DWM(std::shared_ptr<GoalHandle>, tf2_ros::Buffer::SharedPtr&, MgNavigationServer&);
|
||||
|
||||
void execute();
|
||||
|
||||
protected:
|
||||
void target_init() { target_update(); }
|
||||
bool target_check() { return false; }
|
||||
void target_update() { }
|
||||
bool condition_check();
|
||||
void populate_candidates(std::vector<int>& vl, std::vector<int>& vr, int dimx, int dimy);
|
||||
double calc_score(int vl, int vr);
|
||||
|
||||
void abort(int error);
|
||||
void succeed();
|
||||
void cancel();
|
||||
|
||||
void pos_query();
|
||||
void send_speed(double vl, double vr);
|
||||
|
||||
static std::pair<double, double> to_vel(double vl, double vr) {
|
||||
return std::make_pair(WHEEL_RAD * (vl + vr) / 2.0, // NOLINT
|
||||
WHEEL_RAD * (vr - vl) / WHEEL_SEP);
|
||||
}
|
||||
};
|
||||
|
||||
template <typename T>
|
||||
DWM<T>::DWM(std::shared_ptr<GoalHandle> g, tf2_ros::Buffer::SharedPtr& buf, MgNavigationServer& mns) :
|
||||
baseNode(mns),
|
||||
hgoal(g),
|
||||
pub_twist(mns.pub_twist),
|
||||
goal(g->get_goal()),
|
||||
tfbuf(buf),
|
||||
step(g->get_goal()->step),
|
||||
target_pos(0, 0),
|
||||
pos(0, 0) { }
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::execute() {
|
||||
std::vector<int> spacevl(dimx * dimy);
|
||||
std::vector<int> spacevr(dimx * dimy);
|
||||
|
||||
pos_query();
|
||||
target_init();
|
||||
|
||||
rclcpp::Rate rate(UPDATE_RATE);
|
||||
|
||||
while (target_check() && rclcpp::ok()) {
|
||||
if (hgoal->is_canceling()) {
|
||||
cancel();
|
||||
return;
|
||||
}
|
||||
|
||||
if (condition_check()) {
|
||||
return;
|
||||
}
|
||||
|
||||
pos_query();
|
||||
populate_candidates(spacevl, spacevr, dimx, dimy);
|
||||
|
||||
double b_score = calc_score(spacevl[0], spacevr[0]);
|
||||
uint b_ind = 0;
|
||||
|
||||
for (uint i = 1; i < spacevl.size(); i++) {
|
||||
const double score = calc_score(spacevl[i], spacevr[i]);
|
||||
if (score > b_score) {
|
||||
b_score = score;
|
||||
b_ind = i;
|
||||
}
|
||||
}
|
||||
|
||||
calc_score(spacevl[b_ind], spacevr[b_ind]); // Called here again so it sticks out in logs
|
||||
|
||||
cvl = spacevl[b_ind];
|
||||
cvr = spacevr[b_ind];
|
||||
send_speed(step * cvl, step * cvr);
|
||||
|
||||
rate.sleep();
|
||||
}
|
||||
succeed();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::pos_query() {
|
||||
try {
|
||||
double x = NAN;
|
||||
double y = NAN;
|
||||
|
||||
auto tmsg = tfbuf->lookupTransform("odom", "base-link", tf2::TimePointZero);
|
||||
|
||||
tf2::Transform t;
|
||||
tf2::convert(tmsg.transform, t);
|
||||
t.getBasis().getRPY(x, y, theta);
|
||||
auto vec3 = tmsg.transform.translation;
|
||||
|
||||
pos = glm::dvec2(vec3.x, vec3.y);
|
||||
|
||||
} catch (const tf2::TransformException& e) {
|
||||
RCLCPP_ERROR(baseNode.get_logger(), "Got following error when looking up transform:\n\t%s", e.what());
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::send_speed(double vl, double vr) {
|
||||
auto [v, w] = to_vel(vl, vr);
|
||||
Geometry::TwistStamped twist;
|
||||
twist.twist.angular.z = w;
|
||||
twist.twist.linear.x = v;
|
||||
|
||||
pub_twist->publish(twist);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::abort(int error) {
|
||||
baseNode.mtx.lock();
|
||||
send_speed(0, 0);
|
||||
auto x = std::make_shared<Result>();
|
||||
x->error = error;
|
||||
hgoal->abort(x);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::succeed() {
|
||||
baseNode.mtx.lock();
|
||||
send_speed(0, 0);
|
||||
auto x = std::make_shared<Result>();
|
||||
x->error = SUCCESS;
|
||||
hgoal->succeed(x);
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void DWM<T>::cancel() {
|
||||
baseNode.mtx.lock();
|
||||
send_speed(0, 0);
|
||||
auto x = std::make_shared<Result>();
|
||||
x->error = UNKNOWN;
|
||||
hgoal->canceled(x);
|
||||
}
|
||||
|
||||
}; // namespace mg
|
||||
57
mg_navigation/include/handlers/dwm_forward.hpp
Normal file
57
mg_navigation/include/handlers/dwm_forward.hpp
Normal file
@ -0,0 +1,57 @@
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
83
mg_navigation/include/handlers/dwm_lookat.hpp
Normal file
83
mg_navigation/include/handlers/dwm_lookat.hpp
Normal file
@ -0,0 +1,83 @@
|
||||
#pragma once
|
||||
#include "handlers/dwm_core.hpp"
|
||||
|
||||
#include "glm/gtx/norm.hpp"
|
||||
#include "glm/gtx/rotate_vector.hpp"
|
||||
#include "glm/gtx/vector_angle.hpp"
|
||||
#include <glm/gtc/constants.hpp>
|
||||
|
||||
namespace mg {
|
||||
|
||||
template <>
|
||||
inline bool DWM<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;
|
||||
|
||||
return b > goal->tolerance;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline void DWM<MgNavigationServer::LookAt>::target_init() {
|
||||
|
||||
target_pos = glm::vec2(goal->x, goal->y);
|
||||
|
||||
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 sign = (cross < 0) ? -1 : 1;
|
||||
|
||||
target_ornt = angle * sign;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline void DWM<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;
|
||||
const double sign = (cross < 0) ? -1 : 1;
|
||||
|
||||
target_ornt = angle * sign;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline bool DWM<MgNavigationServer::LookAt>::condition_check() {
|
||||
return false;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline double DWM<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;
|
||||
|
||||
double dist_old = glm::abs(target_ornt - theta);
|
||||
double dist_new = glm::abs(target_ornt - n_theta);
|
||||
|
||||
dist_old = (dist_old > glm::pi<double>()) ? glm::two_pi<double>() - dist_old : dist_old;
|
||||
dist_new = (dist_new > glm::pi<double>()) ? glm::two_pi<double>() - dist_new : dist_new;
|
||||
|
||||
const double score = goal->ornt_mult * (dist_old - dist_new);
|
||||
|
||||
return score;
|
||||
}
|
||||
|
||||
template <>
|
||||
inline void DWM<MgNavigationServer::LookAt>::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 && step * abs(cvr + i) <= goal->max_wheel_speed
|
||||
&& glm::abs(w) < goal->max_angular) {
|
||||
vl.push_back(cvl - i);
|
||||
vr.push_back(cvr + i);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
80
mg_navigation/include/handlers/dwm_point.hpp
Normal file
80
mg_navigation/include/handlers/dwm_point.hpp
Normal file
@ -0,0 +1,80 @@
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
60
mg_navigation/include/mg_navigation.hpp
Normal file
60
mg_navigation/include/mg_navigation.hpp
Normal file
@ -0,0 +1,60 @@
|
||||
#pragma once
|
||||
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "rclcpp_action/rclcpp_action.hpp"
|
||||
|
||||
#include "mg_msgs/action/move_point.hpp"
|
||||
#include "mg_msgs/action/move_straight.hpp"
|
||||
#include "mg_msgs/action/look_at.hpp"
|
||||
|
||||
#include "geometry_msgs/msg/twist_stamped.hpp"
|
||||
#include "geometry_msgs/msg/transform_stamped.hpp"
|
||||
|
||||
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
||||
#include "tf2_ros/transform_listener.h"
|
||||
#include "tf2_ros/buffer.h"
|
||||
|
||||
namespace mg {
|
||||
|
||||
namespace Geometry = geometry_msgs::msg;
|
||||
|
||||
class MgNavigationServer : public rclcpp::Node {
|
||||
public:
|
||||
RCLCPP_SMART_PTR_ALIASES_ONLY(MgNavigationServer)
|
||||
using MovePoint = mg_msgs::action::MovePoint;
|
||||
using MoveStraight = mg_msgs::action::MoveStraight;
|
||||
using LookAt = mg_msgs::action::LookAt;
|
||||
|
||||
rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
|
||||
|
||||
rclcpp_action::Server<LookAt>::SharedPtr sv_look_at;
|
||||
rclcpp_action::Server<MovePoint>::SharedPtr sv_move_point;
|
||||
rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
|
||||
|
||||
rclcpp::Subscription<Geometry::TransformStamped>::SharedPtr tf2_subscription_;
|
||||
tf2_ros::Buffer::SharedPtr tf2_buffer;
|
||||
std::shared_ptr<tf2_ros::TransformListener> tf2_listener;
|
||||
std::mutex mtx;
|
||||
|
||||
MgNavigationServer(rclcpp::NodeOptions& opts);
|
||||
|
||||
private:
|
||||
bool is_processing = false;
|
||||
|
||||
template <typename T>
|
||||
rclcpp_action::GoalResponse handle_goal(const rclcpp_action::GoalUUID&,
|
||||
typename T::Goal::ConstSharedPtr,
|
||||
const char*);
|
||||
template <typename T>
|
||||
rclcpp_action::CancelResponse handle_cancel(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >,
|
||||
const char*);
|
||||
template <typename T>
|
||||
void handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char*);
|
||||
|
||||
template <typename T>
|
||||
void execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >);
|
||||
};
|
||||
|
||||
};
|
||||
Reference in New Issue
Block a user