74 lines
2.8 KiB
C++
74 lines
2.8 KiB
C++
/** Copyright 2025 The Magrob Authors
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
#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> >);
|
|
};
|
|
|
|
}; |