83 lines
3 KiB
C++
83 lines
3 KiB
C++
#include "behaviortree_ros2/bt_action_node.hpp"
|
|
#include "rbs_skill_interfaces/action/moveit_send_pose.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>
|
|
#include <rclcpp/logging.hpp>
|
|
|
|
using namespace BT;
|
|
using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
|
|
|
|
class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
|
|
public:
|
|
MoveToPose(const std::string &name, const NodeConfig &conf,
|
|
const RosNodeParams ¶ms)
|
|
: RosActionNode<MoveitSendPoseAction>(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>("duration"),
|
|
BT::InputPort<bool>("try_plan_untill_success", false, "Replan if failure to plan"),
|
|
BT::InputPort<double>("allowed_planning_time", 5.0, "Maximal time to plan"),
|
|
});
|
|
}
|
|
|
|
bool setGoal(RosActionNode::Goal &goal) override {
|
|
RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(),
|
|
this->action_name_.c_str());
|
|
if (!getInput("robot_name", goal.robot_name)) {
|
|
RCLCPP_FATAL(this->logger(),
|
|
"[%s] Could not get robot_name from input port",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
if (!getInput("pose", m_target_pose)) {
|
|
RCLCPP_FATAL(this->logger(),
|
|
"[%s] Could not get target_pose from input port",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
if (!getInput("duration", goal.duration)) {
|
|
RCLCPP_FATAL(this->logger(),
|
|
"[%s] Could not get duration from input port",
|
|
name().c_str());
|
|
return false;
|
|
}
|
|
|
|
RCLCPP_INFO_STREAM(
|
|
this->logger(),
|
|
"[" << name().c_str() << "]" << "Got target_pose: \nPosition [ "
|
|
<< m_target_pose->position.x << ", " << m_target_pose->position.y
|
|
<< ", " << m_target_pose->position.z << " ];\n"
|
|
<< "Orientation: [ " << m_target_pose->orientation.x << ", "
|
|
<< m_target_pose->orientation.y << ", "
|
|
<< m_target_pose->orientation.z << ", "
|
|
<< m_target_pose->orientation.w << "] ");
|
|
|
|
goal.target_pose = *m_target_pose;
|
|
getInput("try_plan_untill_success", goal.try_plan_untill_success);
|
|
getInput("allowed_planning_time", goal.allowed_planning_time);
|
|
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(MoveToPose, "MoveToPose");
|