105 lines
4.5 KiB
C++
105 lines
4.5 KiB
C++
#include <iostream>
|
|
#include <functional>
|
|
#include <thread>
|
|
|
|
#define GLM_ENABLE_EXPERIMENTAL
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "mg_navigation.hpp"
|
|
#include "handlers/dwa.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) {
|
|
DWA<T> dwm = DWA<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);
|
|
|
|
path_buffer_ = std::make_shared<PathBuffer>(this);
|
|
|
|
pub_twist = create_publisher<Geometry::TwistStamped>("diffdrive_controller/cmd_vel", 2);
|
|
|
|
obstacle_manager_ = std::make_shared<ObstacleManager>(this, tf2_buffer);
|
|
|
|
using namespace std::placeholders;
|
|
|
|
sv_move_global = rclcpp_action::create_server<MoveGlobal>(
|
|
this,
|
|
"MoveGlobal",
|
|
std::bind(&MgNavigationServer::handle_goal<MoveGlobal>, this, _1, _2, "MoveGlobal"),
|
|
std::bind(&MgNavigationServer::handle_cancel<MoveGlobal>, this, _1, "MoveGlobal"),
|
|
std::bind(&MgNavigationServer::handle_accepted<MoveGlobal>, this, _1, "MoveGlobal"));
|
|
|
|
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_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"));
|
|
|
|
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_rotate = rclcpp_action::create_server<Rotate>(
|
|
this,
|
|
"Rotate",
|
|
std::bind(&MgNavigationServer::handle_goal<Rotate>, this, _1, _2, "Rotate"),
|
|
std::bind(&MgNavigationServer::handle_cancel<Rotate>, this, _1, "Rotate"),
|
|
std::bind(&MgNavigationServer::handle_accepted<Rotate>, this, _1, "Rotate"));
|
|
}
|
|
|
|
};
|
|
|
|
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;
|
|
} |