57 lines
1.4 KiB
C++
57 lines
1.4 KiB
C++
#pragma once
|
|
|
|
#include "angles/angles.h"
|
|
#include "behaviortree_ros2/bt_service_node.hpp"
|
|
#include "tf2/utils.hpp"
|
|
#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
|
|
#include "toid_bt/plugin.hpp"
|
|
#include "toid_msgs/srv/send_double.hpp"
|
|
|
|
namespace toid
|
|
{
|
|
|
|
class EndCalibNode : public BT::RosServiceNode<toid_msgs::srv::SendDouble>
|
|
{
|
|
public:
|
|
EndCalibNode(
|
|
const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params,
|
|
PoseFunc pose)
|
|
: BT::RosServiceNode<toid_msgs::srv::SendDouble>(name, conf, params), get_pose(pose)
|
|
{
|
|
}
|
|
|
|
static BT::PortsList providedPorts()
|
|
{
|
|
return providedBasicPorts({
|
|
BT::InputPort<double>("width"),
|
|
BT::InputPort<int>("count", 1, {}),
|
|
BT::OutputPort<double>("new_width"),
|
|
//BT::InputPort<double>("options"),
|
|
});
|
|
}
|
|
|
|
bool setRequest(typename Request::SharedPtr & request) override
|
|
{
|
|
auto width = getInput<double>("width").value();
|
|
auto count = getInput<double>("count").value();
|
|
|
|
geometry_msgs::msg::PoseStamped pose;
|
|
get_pose(pose);
|
|
|
|
double theta = tf2::getYaw(pose.pose.orientation);
|
|
double new_width = width * (1 + (theta / (2 * M_PI * count)));
|
|
request->data = new_width;
|
|
|
|
setOutput("new_width", new_width);
|
|
return true;
|
|
}
|
|
|
|
BT::NodeStatus onResponseReceived(const typename Response::SharedPtr &) override
|
|
{
|
|
return BT::NodeStatus::SUCCESS;
|
|
}
|
|
|
|
PoseFunc get_pose;
|
|
};
|
|
|
|
} // namespace toid
|