53 lines
1.8 KiB
C++
53 lines
1.8 KiB
C++
// Copyright [2023] bill-finger
|
|
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
|
|
|
|
#include "behavior_tree/BtAction.hpp"
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
|
// #include "Eigen/Geometry"
|
|
|
|
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
|
|
|
|
class MoveToPose : public BtAction<MoveToPoseAction> {
|
|
public:
|
|
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
|
|
: BtAction<MoveToPoseAction>(name, config) {}
|
|
|
|
MoveToPoseAction::Goal populate_goal() override {
|
|
getInput<std::string>("robot_name", robot_name_);
|
|
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
|
|
RCLCPP_INFO(_node->get_logger(),
|
|
"\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
|
|
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
|
|
"orientation.z: %f \n\t orientation.w: %f",
|
|
pose_des.position.x, pose_des.position.y, pose_des.position.z,
|
|
pose_des.orientation.x, pose_des.orientation.y,
|
|
pose_des.orientation.z, pose_des.orientation.w);
|
|
|
|
auto goal = MoveToPoseAction::Goal();
|
|
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
|
|
robot_name_.c_str());
|
|
|
|
goal.robot_name = robot_name_;
|
|
goal.target_pose = pose_des;
|
|
goal.end_effector_acceleration = 1.0;
|
|
goal.end_effector_velocity = 1.0;
|
|
|
|
RCLCPP_INFO(_node->get_logger(), "Goal populated");
|
|
|
|
return goal;
|
|
}
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts(
|
|
{BT::InputPort<std::string>("robot_name"),
|
|
BT::InputPort<geometry_msgs::msg::Pose>("pose")});
|
|
}
|
|
|
|
private:
|
|
std::string robot_name_;
|
|
geometry_msgs::msg::Pose pose_des;
|
|
}; // class MoveToPose
|
|
|
|
BT_REGISTER_NODES(factory) {
|
|
factory.registerNodeType<MoveToPose>("MoveToPose");
|
|
}
|