56 lines
2 KiB
C++
56 lines
2 KiB
C++
#include "behavior_tree/BtAction.hpp"
|
|
#include "behaviortree_cpp_v3/behavior_tree.h"
|
|
#include "geometry_msgs/msg/pose_array.hpp"
|
|
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
|
|
#include "rbs_utils/rbs_utils.hpp"
|
|
#include <behaviortree_cpp_v3/bt_factory.h>
|
|
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
|
|
#include <rclcpp/logging.hpp>
|
|
#include <rclcpp/node.hpp>
|
|
|
|
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
|
|
|
|
class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
|
|
public:
|
|
MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
|
|
: BtAction<MoveToPoseArrayAction>(name, config) {
|
|
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
|
|
}
|
|
|
|
MoveToPoseArrayAction::Goal populate_goal() override {
|
|
auto goal = MoveToPoseArrayAction::Goal();
|
|
getInput<std::string>("robot_name", robot_name_);
|
|
getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
|
|
for (auto &point : target_pose_vec_.poses) {
|
|
RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
|
|
point.position.x, point.position.y, point.position.z);
|
|
}
|
|
if (!target_pose_vec_.poses.empty()) {
|
|
goal.robot_name = robot_name_;
|
|
goal.target_pose = target_pose_vec_.poses.at(0);
|
|
|
|
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
|
|
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
|
|
target_pose_vec_);
|
|
} else {
|
|
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
|
|
}
|
|
|
|
return goal;
|
|
}
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts(
|
|
{BT::InputPort<std::string>("robot_name"),
|
|
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
|
|
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
|
|
}
|
|
|
|
private:
|
|
std::string robot_name_;
|
|
geometry_msgs::msg::PoseArray target_pose_vec_;
|
|
};
|
|
|
|
BT_REGISTER_NODES(factory) {
|
|
factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
|
|
}
|