Implented point to point DWM action server
This commit is contained in:
53
mg_navigation/CMakeLists.txt
Normal file
53
mg_navigation/CMakeLists.txt
Normal file
@ -0,0 +1,53 @@
|
||||
cmake_minimum_required(VERSION 3.8)
|
||||
project(mg_navigation)
|
||||
|
||||
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
|
||||
add_compile_options(-Wall -Wextra -Wpedantic)
|
||||
endif()
|
||||
|
||||
# find dependencies
|
||||
find_package(ament_cmake REQUIRED)
|
||||
find_package(rclcpp REQUIRED)
|
||||
find_package(mg_msgs REQUIRED)
|
||||
find_package(geometry_msgs REQUIRED)
|
||||
find_package(tf2 REQUIRED)
|
||||
find_package(tf2_ros REQUIRED)
|
||||
find_package(tf2_geometry_msgs REQUIRED)
|
||||
|
||||
include(FindPkgConfig)
|
||||
pkg_search_module(LIBGLM REQUIRED glm)
|
||||
|
||||
|
||||
|
||||
set(PACKAGE_DEPS
|
||||
rclcpp
|
||||
mg_msgs
|
||||
geometry_msgs
|
||||
tf2
|
||||
tf2_ros
|
||||
tf2_geometry_msgs
|
||||
)
|
||||
|
||||
add_executable(mg_nav_server
|
||||
src/mg_navigation.cpp
|
||||
)
|
||||
ament_target_dependencies(mg_nav_server ${PACKAGE_DEPS})
|
||||
|
||||
target_include_directories(
|
||||
mg_nav_server
|
||||
PRIVATE
|
||||
${LIBGLM_INCLUDE_DIRS}
|
||||
include
|
||||
)
|
||||
|
||||
target_link_libraries(
|
||||
mg_nav_server
|
||||
${LIBGLM_LIBRARIES}
|
||||
)
|
||||
|
||||
install( TARGETS
|
||||
mg_nav_server
|
||||
DESTINATION lib/${PROJECT_NAME}
|
||||
)
|
||||
|
||||
ament_package()
|
||||
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> >);
|
||||
};
|
||||
|
||||
};
|
||||
27
mg_navigation/package.xml
Normal file
27
mg_navigation/package.xml
Normal file
@ -0,0 +1,27 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>mg_navigation</name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="82343504+Pimpest@users.noreply.github.com">Pimpest</maintainer>
|
||||
<license>Apache 2.0</license>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<depend>rclcpp</depend>
|
||||
<depend>rclcpp_action</depend>
|
||||
<depend>rclcpp_components</depend>
|
||||
<depend>mg_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>tf2</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
<depend>tf2_geometry_msgs</depend>
|
||||
|
||||
|
||||
<build_depend>libglm-dev</build_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_cmake</build_type>
|
||||
</export>
|
||||
</package>
|
||||
86
mg_navigation/src/mg_navigation.cpp
Normal file
86
mg_navigation/src/mg_navigation.cpp
Normal file
@ -0,0 +1,86 @@
|
||||
#include <iostream>
|
||||
#include <functional>
|
||||
#include <thread>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "mg_navigation.hpp"
|
||||
#include "handlers/dwm.hpp"
|
||||
|
||||
namespace mg {
|
||||
|
||||
template <typename T>
|
||||
rclcpp_action::GoalResponse MgNavigationServer::handle_goal(const rclcpp_action::GoalUUID& /*unused*/,
|
||||
typename T::Goal::ConstSharedPtr /*unused*/,
|
||||
const char* N) {
|
||||
RCLCPP_INFO(get_logger(), "Recieved goal for action: [%s]", N);
|
||||
const std::lock_guard<std::mutex> lock(mtx);
|
||||
if (is_processing) {
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
} else {
|
||||
is_processing = true;
|
||||
return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
|
||||
}
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
rclcpp_action::CancelResponse MgNavigationServer::handle_cancel(
|
||||
const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> >, const char* N) {
|
||||
RCLCPP_INFO(this->get_logger(), "Recieved cancel request for action: [%s]", N);
|
||||
return rclcpp_action::CancelResponse::ACCEPT;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void MgNavigationServer::handle_accepted(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh,
|
||||
const char* N) {
|
||||
RCLCPP_INFO(this->get_logger(), "Goal accepted for action: [%s]", N);
|
||||
using namespace std::placeholders;
|
||||
auto t = std::bind(&MgNavigationServer::execute<T>, this, _1);
|
||||
std::thread{ t, gh }.detach();
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
void MgNavigationServer::execute(const std::shared_ptr<rclcpp_action::ServerGoalHandle<T> > gh) {
|
||||
DWM<T> dwm = DWM<T>(gh, tf2_buffer, *this);
|
||||
dwm.execute();
|
||||
is_processing = false;
|
||||
mtx.unlock();
|
||||
}
|
||||
|
||||
MgNavigationServer::MgNavigationServer(rclcpp::NodeOptions& opts) : rclcpp::Node("MgNavigationServer", opts) {
|
||||
tf2_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
|
||||
tf2_listener = std::make_shared<tf2_ros::TransformListener>(*tf2_buffer);
|
||||
|
||||
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
|
||||
|
||||
using namespace std::placeholders;
|
||||
sv_move_point = rclcpp_action::create_server<MovePoint>(
|
||||
this,
|
||||
"MovePoint",
|
||||
std::bind(&MgNavigationServer::handle_goal<MovePoint>, this, _1, _2, "MovePoint"),
|
||||
std::bind(&MgNavigationServer::handle_cancel<MovePoint>, this, _1, "MovePoint"),
|
||||
std::bind(&MgNavigationServer::handle_accepted<MovePoint>, this, _1, "MovePoint"));
|
||||
|
||||
sv_look_at = rclcpp_action::create_server<LookAt>(
|
||||
this,
|
||||
"LookAt",
|
||||
std::bind(&MgNavigationServer::handle_goal<LookAt>, this, _1, _2, "LookAt"),
|
||||
std::bind(&MgNavigationServer::handle_cancel<LookAt>, this, _1, "LookAt"),
|
||||
std::bind(&MgNavigationServer::handle_accepted<LookAt>, this, _1, "LookAt"));
|
||||
|
||||
sv_move_straight = rclcpp_action::create_server<MoveStraight>(
|
||||
this,
|
||||
"MoveStraight",
|
||||
std::bind(&MgNavigationServer::handle_goal<MoveStraight>, this, _1, _2, "MoveStraight"),
|
||||
std::bind(&MgNavigationServer::handle_cancel<MoveStraight>, this, _1, "MoveStraight"),
|
||||
std::bind(&MgNavigationServer::handle_accepted<MoveStraight>, this, _1, "MoveStraight"));
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::NodeOptions opts;
|
||||
rclcpp::spin(std::make_shared<mg::MgNavigationServer>(opts));
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user