105 lines
3.0 KiB
C++
105 lines
3.0 KiB
C++
#pragma once
|
|
#include "nav2_behaviors/timed_behavior.hpp"
|
|
#include "nav2_core/behavior.hpp"
|
|
#include "nav2_util/node_utils.hpp"
|
|
#include "nav2_util/twist_publisher.hpp"
|
|
#include "nav_msgs/msg/odometry.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "toid_msgs/action/simple_rotate.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
template <typename ActionT>
|
|
class SimpleMove : public nav2_behaviors::TimedBehavior<ActionT>
|
|
{
|
|
public:
|
|
virtual void configureCB() {}
|
|
virtual void cleanupCB() {}
|
|
|
|
virtual void activateCB() {}
|
|
virtual void deactivateCB() {}
|
|
|
|
virtual nav2_behaviors::ResultStatus onStart(
|
|
const std::shared_ptr<const typename ActionT::Goal> command,
|
|
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel) = 0;
|
|
|
|
virtual nav2_behaviors::ResultStatus updateVel(
|
|
const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & vel,
|
|
geometry_msgs::msg::Twist & out_vel) = 0;
|
|
|
|
void onConfigure() override
|
|
{
|
|
rclcpp_lifecycle::LifecycleNode::SharedPtr node = this->node_.lock();
|
|
nav2_util::declare_parameter_if_not_declared(
|
|
node, "odom_topic", rclcpp::ParameterValue("/odom"));
|
|
std::string odom_topic_name = node->get_parameter("odom_topic").as_string();
|
|
odom_sub_ = node->create_subscription<nav_msgs::msg::Odometry>(
|
|
odom_topic_name, 1, [&](nav_msgs::msg::Odometry msg) {
|
|
std::lock_guard lock(mut_);
|
|
current_pose_ = msg.pose.pose;
|
|
current_vel_ = msg.twist.twist;
|
|
});
|
|
control_duration_ = 1.0 / this->cycle_frequency_;
|
|
configureCB();
|
|
}
|
|
|
|
void onCleanup() override
|
|
{
|
|
odom_sub_.reset();
|
|
cleanupCB();
|
|
}
|
|
|
|
void activate() override
|
|
{
|
|
nav2_behaviors::TimedBehavior<ActionT>::activate();
|
|
activateCB();
|
|
}
|
|
|
|
void deactivate() override
|
|
{
|
|
nav2_behaviors::TimedBehavior<ActionT>::deactivate();
|
|
deactivateCB();
|
|
}
|
|
|
|
nav2_behaviors::ResultStatus onRun(
|
|
const std::shared_ptr<const typename ActionT::Goal> command) override
|
|
{
|
|
geometry_msgs::msg::Pose pose;
|
|
geometry_msgs::msg::Twist vel;
|
|
{
|
|
std::lock_guard lock(mut_);
|
|
pose = current_pose_;
|
|
vel = current_vel_;
|
|
}
|
|
return onStart(command, pose, vel);
|
|
}
|
|
|
|
nav2_behaviors::ResultStatus onCycleUpdate() override
|
|
{
|
|
geometry_msgs::msg::Pose pose;
|
|
geometry_msgs::msg::Twist vel;
|
|
geometry_msgs::msg::Twist out_vel;
|
|
auto vel_p = std::make_unique<geometry_msgs::msg::TwistStamped>();
|
|
{
|
|
std::lock_guard lock(mut_);
|
|
pose = current_pose_;
|
|
vel = current_vel_;
|
|
}
|
|
nav2_behaviors::ResultStatus r = updateVel(pose, vel, out_vel);
|
|
vel_p->twist = out_vel;
|
|
vel_p->header.stamp = this->clock_->now();
|
|
vel_p->header.frame_id = this->robot_base_frame_;
|
|
this->vel_pub_->publish(std::move(vel_p));
|
|
return r;
|
|
}
|
|
|
|
protected:
|
|
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
|
|
geometry_msgs::msg::Pose current_pose_;
|
|
geometry_msgs::msg::Twist current_vel_;
|
|
std::recursive_mutex mut_;
|
|
double control_duration_;
|
|
};
|
|
|
|
} // namespace toid
|