47 lines
1.9 KiB
C++
47 lines
1.9 KiB
C++
#pragma once
|
|
|
|
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "mg_msgs/action/rotate.hpp"
|
|
|
|
namespace mg {
|
|
|
|
class RotateNode : public BT::RosActionNode<mg_msgs::action::Rotate> {
|
|
public:
|
|
RotateNode(const std::string& name, const BT::NodeConfig& conf, const BT::RosNodeParams& params) :
|
|
BT::RosActionNode<mg_msgs::action::Rotate>(name, conf, params) { }
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts({
|
|
BT::InputPort<double>("angle"),
|
|
BT::InputPort<double>("tolerance", 0.001, {}),
|
|
BT::InputPort<double>("max_wheel_speed", 3, {}),
|
|
BT::InputPort<double>("max_angular", 3.14, {}),
|
|
});
|
|
}
|
|
|
|
bool setGoal(Goal& goal) override {
|
|
auto angle = getInput<double>("angle");
|
|
auto tolerance = getInput<double>("tolerance");
|
|
auto max_wheel_speed = getInput<double>("max_wheel_speed");
|
|
auto max_angular = getInput<double>("max_angular");
|
|
goal.angle = angle.value();
|
|
goal.tolerance = tolerance.value_or(goal.tolerance);
|
|
goal.max_wheel_speed = max_wheel_speed.value_or(goal.max_wheel_speed);
|
|
goal.max_angular = max_angular.value_or(goal.max_angular);
|
|
return true;
|
|
}
|
|
|
|
void onHalt() override { RCLCPP_INFO(logger(), "%s: onHalt", name().c_str()); }
|
|
|
|
BT::NodeStatus onResultReceived(const WrappedResult& wr) override {
|
|
return (wr.result->error == ActionType::Result::SUCCESS) ? 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;
|
|
}
|
|
};
|
|
|
|
} |