62 lines
2.4 KiB
C++
62 lines
2.4 KiB
C++
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "rbs_skill_interfaces/action/moveit_send_pose_with_constraints.hpp"
|
|
#include <behaviortree_cpp/basic_types.h>
|
|
#include <behaviortree_cpp/tree_node.h>
|
|
#include <behaviortree_ros2/plugins.hpp>
|
|
#include <behaviortree_ros2/ros_node_params.hpp>
|
|
|
|
using namespace BT;
|
|
using MoveitSendPoseWithConstraintAction =
|
|
rbs_skill_interfaces::action::MoveitSendPoseWithConstraints;
|
|
|
|
class MoveToPoseOrientationConstraint
|
|
: public RosActionNode<MoveitSendPoseWithConstraintAction> {
|
|
public:
|
|
MoveToPoseOrientationConstraint(const std::string &name,
|
|
const NodeConfig &conf,
|
|
const RosNodeParams ¶ms)
|
|
: RosActionNode<MoveitSendPoseWithConstraintAction>(name, conf, params) {
|
|
|
|
RCLCPP_INFO(this->logger(), "Starting MoveToPose");
|
|
}
|
|
|
|
static BT::PortsList providedPorts() {
|
|
return providedBasicPorts(
|
|
{BT::InputPort<std::string>("robot_name"),
|
|
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose"),
|
|
BT::InputPort<double>("x_tol"), BT::InputPort<double>("y_tol"),
|
|
BT::InputPort<double>("z_tol"), BT::InputPort<double>("weight"),
|
|
BT::InputPort<double>("duration")});
|
|
}
|
|
|
|
bool setGoal(RosActionNode::Goal &goal) override {
|
|
RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
|
|
this->action_name_.c_str());
|
|
|
|
getInput("x_tol", goal.orientation_constraint.absolute_x_axis_tolerance);
|
|
getInput("y_tol", goal.orientation_constraint.absolute_y_axis_tolerance);
|
|
getInput("z_tol", goal.orientation_constraint.absolute_z_axis_tolerance);
|
|
getInput("weight", goal.orientation_constraint.weight);
|
|
getInput("robot_name", goal.robot_name);
|
|
getInput("pose", m_target_pose);
|
|
getInput("duration", goal.duration);
|
|
goal.target_pose = *m_target_pose;
|
|
return true;
|
|
}
|
|
|
|
NodeStatus onResultReceived(const WrappedResult &wr) override {
|
|
|
|
RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b",
|
|
name().c_str(), this->action_name_.c_str(), wr.result->success);
|
|
if (!wr.result->success) {
|
|
return NodeStatus::FAILURE;
|
|
}
|
|
return NodeStatus::SUCCESS;
|
|
}
|
|
|
|
private:
|
|
std::shared_ptr<geometry_msgs::msg::Pose> m_target_pose;
|
|
};
|
|
|
|
CreateRosNodePlugin(MoveToPoseOrientationConstraint,
|
|
"MtpOrientationConstraint");
|