62 lines
1.7 KiB
C++
62 lines
1.7 KiB
C++
#pragma once
|
|
|
|
#include "angles/angles.h"
|
|
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "tf2/utils.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
#include "toid_bt/plugin.hpp"
|
|
#include "toid_msgs/action/simple_rotate.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
class RotateNode : public BT::RosActionNode<toid_msgs::action::SimpleRotate>
|
|
{
|
|
public:
|
|
RotateNode(
|
|
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
|
|
PoseFunc pose)
|
|
: BT::RosActionNode<toid_msgs::action::SimpleRotate>(name, conf, params), get_pose(pose)
|
|
{
|
|
}
|
|
|
|
static BT::PortsList providedPorts()
|
|
{
|
|
return providedBasicPorts({
|
|
BT::InputPort<double>("angle", {}),
|
|
BT::InputPort<double>("min_angle", 0, {}),
|
|
BT::InputPort<double>("max_speed", 0, {}),
|
|
//BT::InputPort<double>("options"),
|
|
});
|
|
}
|
|
|
|
bool setGoal(Goal & goal) override
|
|
{
|
|
auto angle = getInput<double>("angle");
|
|
auto min_angle = getInput<double>("min_angle");
|
|
auto max_speed = getInput<double>("max_speed");
|
|
goal.angle = angle.value();
|
|
goal.min_angle = min_angle.value();
|
|
goal.max_speed = max_speed.value();
|
|
goal.angle = angles::from_degrees(goal.angle);
|
|
goal.min_angle = angles::from_degrees(goal.min_angle);
|
|
return true;
|
|
}
|
|
|
|
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
|
|
|
BT::NodeStatus onResultReceived(const WrappedResult & wr) override
|
|
{
|
|
return (wr.result->error_code == 0) ? BT::NodeStatus::SUCCESS : BT::NodeStatus::FAILURE;
|
|
}
|
|
|
|
BT::NodeStatus onFailure(BT::ActionNodeErrorCode error) override
|
|
{
|
|
RCLCPP_ERROR(logger(), "%s: onFailure with error: %s", name().c_str(), toStr(error));
|
|
return BT::NodeStatus::FAILURE;
|
|
}
|
|
|
|
PoseFunc get_pose;
|
|
};
|
|
|
|
} // namespace toid
|