75 lines
No EOL
2.8 KiB
C++
75 lines
No EOL
2.8 KiB
C++
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
|
|
|
|
#include "behaviortree_cpp_v3/bt_factory.h"
|
|
#include "behavior_tree/BtAction.hpp"
|
|
#include "geometry_msgs/msg/pose_stamped.hpp"
|
|
#include "moveit_msgs/action/move_group.h"
|
|
// #include "Eigen/Geometry"
|
|
#include "rclcpp/parameter.hpp"
|
|
|
|
using namespace BT;
|
|
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
|
|
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient");
|
|
|
|
class MoveToPose : public BtAction<MoveToPoseAction>
|
|
{
|
|
public:
|
|
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
|
|
: BtAction<MoveToPoseAction>(name, config)
|
|
{
|
|
//auto params = _node->declare_parameters("Poses")
|
|
RCLCPP_INFO(_node->get_logger(), "Start the node");
|
|
pose_des.position.x = 0.11724797630931184;
|
|
pose_des.position.y = 0.46766635768602544;
|
|
pose_des.position.z = 0.5793862343094913;
|
|
pose_des.orientation.x = 0.9987999001314066;
|
|
pose_des.orientation.y = 0.041553846820940925;
|
|
pose_des.orientation.z = -0.004693314677006583;
|
|
pose_des.orientation.w = -0.025495295825239704;
|
|
}
|
|
|
|
MoveToPoseAction::Goal populate_goal() override
|
|
{
|
|
getInput<std::string>("robot_name", robot_name_);
|
|
getInput<std::string>("pose", pose_);
|
|
|
|
//auto pose = _node->get_parameter(pose_).get_parameter_value().get<geometry_msgs::msg::Pose>();
|
|
|
|
RCLCPP_INFO(_node->get_logger(), "GrasPose pose.x: %f pose.y: %f pose.z: \n%f opientation.x: %f orientation.y: %f orientation.z: %f 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 PortsList providedPorts()
|
|
{
|
|
return providedBasicPorts({
|
|
InputPort<std::string>("robot_name"),
|
|
InputPort<std::string>("pose")
|
|
});
|
|
}
|
|
|
|
private:
|
|
std::string robot_name_;
|
|
std::string pose_;
|
|
geometry_msgs::msg::Pose pose_des;
|
|
std::map<std::string, geometry_msgs::msg::Pose> Poses;
|
|
|
|
|
|
}; // class MoveToPose
|
|
|
|
BT_REGISTER_NODES(factory)
|
|
{
|
|
factory.registerNodeType<MoveToPose>("MoveToPose");
|
|
} |