60 lines
2.2 KiB
C++
60 lines
2.2 KiB
C++
#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> >);
|
|
};
|
|
|
|
}; |