78 lines
No EOL
2.7 KiB
C++
78 lines
No EOL
2.7 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)
|
|
{
|
|
RCLCPP_INFO(_node->get_logger(), "Start the node");
|
|
}
|
|
|
|
MoveToPoseAction::Goal populate_goal() override
|
|
{
|
|
getInput<std::string>("robot_name", robot_name_);
|
|
getInput<std::vector<double>>("pose", pose_);
|
|
|
|
pose_des.position.x = pose_[0];
|
|
pose_des.position.y = pose_[1];
|
|
pose_des.position.z = pose_[2];
|
|
pose_des.orientation.x = pose_[3];
|
|
pose_des.orientation.y = pose_[4];
|
|
pose_des.orientation.z = pose_[5];
|
|
pose_des.orientation.w = pose_[6];
|
|
|
|
|
|
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 = 0.5;
|
|
goal.end_effector_velocity = 0.5;
|
|
|
|
RCLCPP_INFO(_node->get_logger(), "Goal populated");
|
|
|
|
return goal;
|
|
}
|
|
|
|
static PortsList providedPorts()
|
|
{
|
|
return providedBasicPorts({
|
|
InputPort<std::string>("robot_name"),
|
|
InputPort<std::vector<double>>("pose")
|
|
});
|
|
}
|
|
|
|
private:
|
|
std::string robot_name_;
|
|
std::vector<double> pose_;
|
|
geometry_msgs::msg::Pose pose_des;
|
|
std::map<std::string, geometry_msgs::msg::Pose> Poses;
|
|
|
|
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){
|
|
|
|
// }
|
|
|
|
|
|
}; // class MoveToPose
|
|
|
|
BT_REGISTER_NODES(factory)
|
|
{
|
|
factory.registerNodeType<MoveToPose>("MoveToPose");
|
|
} |