75 lines
2.8 KiB
C++
75 lines
2.8 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 "mg_msgs/action/rotate.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"
|
|
|
|
#include "mg_obstacles/mg_obstacles.hpp"
|
|
|
|
#include "path_buffer.hpp"
|
|
|
|
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 MoveGlobal = mg_msgs::action::MoveGlobal;
|
|
using MoveStraight = mg_msgs::action::MoveStraight;
|
|
using LookAt = mg_msgs::action::LookAt;
|
|
using Rotate = mg_msgs::action::Rotate;
|
|
|
|
rclcpp::Publisher<Geometry::TwistStamped>::SharedPtr pub_twist;
|
|
|
|
rclcpp_action::Server<MoveGlobal>::SharedPtr sv_move_global;
|
|
rclcpp_action::Server<MovePoint>::SharedPtr sv_move_point;
|
|
rclcpp_action::Server<MoveStraight>::SharedPtr sv_move_straight;
|
|
rclcpp_action::Server<LookAt>::SharedPtr sv_look_at;
|
|
rclcpp_action::Server<Rotate>::SharedPtr sv_rotate;
|
|
|
|
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);
|
|
|
|
std::shared_ptr<PathBuffer> path_buffer() { return path_buffer_; }
|
|
std::shared_ptr<ObstacleManager>& obstacle_manager() { return obstacle_manager_; }
|
|
|
|
private:
|
|
bool is_processing = false;
|
|
|
|
std::shared_ptr<PathBuffer> path_buffer_;
|
|
std::shared_ptr<ObstacleManager> obstacle_manager_;
|
|
|
|
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> >);
|
|
};
|
|
|
|
}; |