robossembler/runtime
7
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

Migration to BT.cpp v4 & new BT executor

Browse source
This commit is contained in:
Ilya Uraev 2024-07-04 12:11:07 +00:00 • committed by Igor Brylyov
parent b58307dea1
commit 2de37b027b
69 changed files with 843 additions and 2065 deletions
Download patch file Download diff file Expand all files Collapse all files

70
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,53 +1,37 @@
// Copyright [2023] bill-finger
#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
// #include "Eigen/Geometry"
using namespace BT;
using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
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;
}
class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
public:
MoveToPose(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveitSendPoseAction>(name, conf, params) {}
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::Pose>("pose")});
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")});
}
private:
std::string robot_name_;
geometry_msgs::msg::Pose pose_des;
}; // class MoveToPose
bool setGoal(RosActionNode::Goal& goal) override {
getInput("robot_name", goal.robot_name);
getInput("pose", goal.target_pose);
return true;
}
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
};
CreateRosNodePlugin(MoveToPose, "MoveToPose");