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

46
rbs_bt_executor/src/AddPlanningSceneObject.cpp
View file

@ -1,46 +0,0 @@
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
using namespace BT;
using AddPlanningSceneObjectClient =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AddPlanningSceneObjectClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
auto place_pose_ = getInput<std::vector<double>>("pose").value();
request->object_id = object_name_;
request->object_pose = place_pose_;
return request;
}
BT::NodeStatus handle_response(
AddPlanningSceneObjectClient::Response::SharedPtr response) override {
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
private:
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("AddPlanningSceneObject");
}

61
rbs_bt_executor/src/AssembleProcessState.cpp
View file

@ -1,61 +0,0 @@
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include "rbs_skill_interfaces/srv/assemble_state.hpp"
using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
class AssembleState : public BtService<AssembleStateSrv> {
public:
AssembleState(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
AssembleStateSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<AssembleStateSrv::Request>();
auto state_kind = getInput<std::string>("StateKind").value();
request->req_kind = -1;
if (state_kind == "INITIALIZE")
request->req_kind = 0;
else if (state_kind == "VALIDATE")
request->req_kind = 1;
else if (state_kind == "COMPLETE")
request->req_kind = 2;
auto assemble_name = getInput<std::string>("AssembleName").value();
auto part_name = getInput<std::string>("PartName").value();
auto workspace = getInput<std::string>("WorkspaceName").value();
request->assemble_name = assemble_name;
request->part_name = part_name;
request->workspace = workspace;
return request;
}
BT::NodeStatus
handle_response(AssembleStateSrv::Response::SharedPtr response) {
// TODO: return bad status on validate process return false state
return (response->call_status = true) ? BT::NodeStatus::SUCCESS
: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")});
}
private:
std::map<std::string, int> assemble_states_;
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<AssembleState>("AssembleState");
}

76
rbs_bt_executor/src/EnvManager.cpp
View file

@ -1,59 +1,43 @@
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
using EnvStarterService = env_manager_interfaces::srv::StartEnv;
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp/basic_types.h>
#include "behaviortree_ros2/plugins.hpp"
#include <string>
class EnvManagerStarter : public BtService<EnvStarterService> {
using EnvManagerService = env_manager_interfaces::srv::StartEnv;
using namespace BT;
class EnvManager : public RosServiceNode<EnvManagerService> {
public:
EnvManagerStarter(const std::string &name,
const BT::NodeConfiguration &config)
: BtService<EnvStarterService>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
EnvManager(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<EnvManagerService>(name, conf, params) {}
EnvStarterService::Request::SharedPtr populate_request() override {
auto request = std::make_shared<EnvStarterService::Request>();
std::string env_name = getInput<std::string>("env_name").value();
std::string env_class = getInput<std::string>("env_class").value();
request->name = env_name;
request->type = env_class;
return request;
}
BT::NodeStatus handle_response(
const EnvStarterService::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("env_name"),
BT::InputPort<std::string>("env_class"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
InputPort<std::string>("env_name"),
InputPort<std::string>("env_class"),
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("env_name", request->name);
getInput("env_class", request->type);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
#include "behaviortree_cpp_v3/bt_factory.h"
CreateRosNodePlugin(EnvManager, "EnvManager");
BT_REGISTER_NODES(factory) {
factory.registerNodeType<EnvManagerStarter>("EnvStarter");
}

104
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -1,104 +0,0 @@
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose.hpp"
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
using namespace BT;
using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
grasp_direction_str_ = getInput<std::string>("GraspDirection").value();
place_direction_str_ = getInput<std::string>("PlaceDirection").value();
RCLCPP_INFO_STREAM(_node->get_logger(),
"Starting process for object: " << object_name_);
grasp_direction_ = convert_direction_from_string(grasp_direction_str_);
place_direction_ = convert_direction_from_string(place_direction_str_);
request->object_name = object_name_;
request->grasp_direction = grasp_direction_;
request->place_direction = place_direction_;
return request;
}
BT::NodeStatus handle_response(
GetPickPlacePoseClient::Response::SharedPtr response) override {
// RCLCPP_INFO(_node->get_logger(),
// "\n 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",
// response->grasp[1].position.x, response->grasp[1].position.y,
// response->grasp[1].position.z, response->grasp[1].orientation.x,
// response->grasp[1].orientation.y,
// response->grasp[1].orientation.z,
// response->grasp[1].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::string>("GraspDirection"), // x or y or z
InputPort<std::string>("PlaceDirection"), // x or y or z
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
// TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) {
std::map<std::string, int> directions;
geometry_msgs::msg::Vector3 vector;
directions["x"] = 1;
directions["y"] = 2;
directions["z"] = 3;
switch (directions[str]) {
case 1:
vector.x = 1;
vector.y = 0;
vector.z = 0;
break;
case 2:
vector.x = 0;
vector.y = 1;
vector.z = 0;
break;
case 3:
vector.x = 0;
vector.y = 0;
vector.z = 1;
break;
};
return vector;
}
private:
std::string object_name_{};
std::string grasp_direction_str_{};
std::string place_direction_str_{};
geometry_msgs::msg::Vector3 grasp_direction_{};
geometry_msgs::msg::Vector3 place_direction_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("GetPickPlacePoses");
}

65
rbs_bt_executor/src/GetWorkspace.cpp Normal file
View file

@ -0,0 +1,65 @@
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rbs_utils_interfaces/srv/get_workspace.hpp"
#include <behaviortree_cpp/basic_types.h>
#include "behaviortree_ros2/plugins.hpp"
#include <geometry_msgs/msg/detail/point__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <string>
using GetWorkspaceService = rbs_utils_interfaces::srv::GetWorkspace;
using namespace BT;
class GetWorkspace : public RosServiceNode<GetWorkspaceService> {
public:
GetWorkspace(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<GetWorkspaceService>(name, conf, params) {}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("type"),
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>("workspace")
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("type", request->type);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
auto workspace = response->workspace;
auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
quat->w = 0.0;
quat->x = 0.0;
quat->y = 1.0;
quat->z = 0.0;
workspace_arr->poses.resize(workspace.size());
size_t i = 0;
for (auto& point : workspace) {
point.z += 0.35;
geometry_msgs::msg::Pose pose;
pose.position = point;
pose.orientation = *quat;
workspace_arr->poses[i++] = pose;
}
setOutput("workspace", workspace_arr);
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
CreateRosNodePlugin(GetWorkspace, "GetWorkspace");

51
rbs_bt_executor/src/MoveGripper.cpp Normal file
View file

@ -0,0 +1,51 @@
#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
using namespace BT;
class GripperControl : public RosActionNode<GripperCommand> {
public:
GripperControl(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<GripperCommand>(name, conf, params) {
if (!node_.lock()->has_parameter("open")) {
node_.lock()->declare_parameter("open", position.open);
}
if (!node_.lock()->has_parameter("close")) {
node_.lock()->declare_parameter("close", position.close);
}
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("pose"),
});
}
bool setGoal(RosActionNode::Goal &goal) override {
getInput("pose", pose);
goal.position = node_.lock()->get_parameter(pose).as_double();
return true;
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
struct {
double open = 0.008;
double close = 0.000;
} position;
std::string pose;
};
CreateRosNodePlugin(GripperControl, "MoveGripper");

70
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,56 +1,40 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.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 "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
using namespace BT;
using MoveToJointStateAction =
rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction> {
class MoveToJointState : public RosActionNode<MoveToJointStateAction> {
public:
MoveToJointState(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToJointStateAction>(name, config) {}
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("JointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
robot_name_.c_str());
for (auto joint : joint_values_) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(),
"Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
MoveToJointState(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveToJointStateAction>(name, conf, params) {}
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("JointState")});
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("JointState")});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState
bool setGoal(RosActionNode::Goal& goal) override {
getInput("robot_name", goal.robot_name);
getInput("JointState", goal.joint_values);
return true;
}
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToJointState>("MoveToJointState");
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
};
CreateRosNodePlugin(MoveToJointState, "MoveToJointState");

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");

85
rbs_bt_executor/src/MoveToPoseArray.cpp
View file

@ -1,58 +1,55 @@
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
using namespace BT;
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
class MoveToPoseArray : public RosActionNode<MoveToPoseArrayAction> {
public:
MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseArrayAction>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
MoveToPoseArray(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveToPoseArrayAction>(name, conf, params) {}
MoveToPoseArrayAction::Goal populate_goal() override {
auto goal = MoveToPoseArrayAction::Goal();
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
for (auto &point : target_pose_vec_.poses) {
RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
point.position.x, point.position.y, point.position.z);
}
if (!target_pose_vec_.poses.empty()) {
goal.robot_name = robot_name_;
goal.target_pose = target_pose_vec_.poses.at(0);
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
target_pose_vec_);
} else {
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
}
return goal;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
{InputPort<std::string>("robot_name"),
InputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
"pose_vec_in"),
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
"pose_vec_out")});
}
bool setGoal(RosActionNode::Goal &goal) override {
getInput("robot_name", goal.robot_name);
getInput("pose_vec_in", m_pose_vec);
if (m_pose_vec->poses.empty()) {
return false;
}
goal.target_pose = m_pose_vec->poses[0];
goal.end_effector_velocity = 1.0;
goal.end_effector_acceleration = 1.0;
m_pose_vec->poses.erase(m_pose_vec->poses.begin());
setOutput("pose_vec_out", m_pose_vec);
return true;
}
NodeStatus onResultReceived(const WrappedResult &wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
std::string robot_name_;
geometry_msgs::msg::PoseArray target_pose_vec_;
std::shared_ptr<geometry_msgs::msg::PoseArray> m_pose_vec;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
}
CreateRosNodePlugin(MoveToPoseArray, "MoveToPoseArray");

125
rbs_bt_executor/src/ObjectDetection.cpp
View file

@ -1,69 +1,64 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <memory>
#include <rclcpp/parameter_client.hpp>
using namespace BT;
using namespace std::chrono_literals;
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
class ObjectDetection : public BtService<ObjDetectionSrv> {
class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
public:
ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
: BtService<ObjDetectionSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/object_detection");
while (!_set_params_client->wait_for_service(1s)) {
ObjectDetection(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<ObjDetectionSrv>(name, conf, params) {
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
node_.lock(), "/object_detection");
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(),
RCLCPP_ERROR(logger(),
"Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(),
"service not available, waiting again...");
}
_setting_path = getInput<std::string>("SettingPath").value();
getInput("SettingPath", m_settings_path);
}
ObjDetectionSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<ObjDetectionSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
}
BT::NodeStatus
handle_response(const ObjDetectionSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("SettingPath"),
InputPort<std::string>("ReqKind"),
InputPort<std::string>("SettingPath"),
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("ReqKind", m_req_type);
auto transition = transition_event(m_req_type);
request->set__transition(transition);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
uint8_t transition_id_{};
std::string _setting_path{};
// std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::string m_settings_path{};
// std::string _model_path{};
std::string m_req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
rcl_interfaces::msg::Parameter m_param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
@ -82,38 +77,14 @@ private:
return translation;
}
// inline std::string build_model_path(const std::string &model_name,
// const std::string &package_path) {
// return package_path + "/config/" + model_name + ".ply";
// }
// inline std::string build_model_path(const std::string &model_path) {
// return model_path;
// }
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _setting_path + ">");
RCLCPP_INFO_STREAM(logger(),
"Set string parameter: <" + m_settings_path + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("setting_path", _setting_path)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> t_parameters{
rclcpp::Parameter("setting_path", m_settings_path)};
m_params_client->set_parameters(t_parameters);
}
// void set_all_param() {
// auto _package_name =
// ament_index_cpp::get_package_share_directory("rbs_perception");
// _model_path = build_model_path(_setting_path, _package_name);
// RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
// std::vector<rclcpp::Parameter> _parameters{
// rclcpp::Parameter("setting_path", _setting_path)};
// _set_params_client->set_parameters(_parameters);
// }
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<ObjectDetection>("ObjectDetection");
}
CreateRosNodePlugin(ObjectDetection, "ObjectDetection");

121
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -1,89 +1,94 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include <behaviortree_ros2/plugins.hpp>
#include <memory>
using namespace BT;
using namespace std::chrono_literals;
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public BtService<PoseEstimationSrv> {
class PoseEstimation : public RosServiceNode<PoseEstimationSrv> {
public:
PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
: BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
PoseEstimation(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<PoseEstimationSrv>(name, conf, params) {
RCLCPP_INFO_STREAM(logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/pose_estimation");
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
node_.lock(), "/pose_estimation");
while (!_set_params_client->wait_for_service(1s)) {
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(),
RCLCPP_ERROR(logger(),
"Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(),
"service not available, waiting again...");
RCLCPP_WARN(logger(), "service not available, waiting again...");
}
_model_name = getInput<std::string>("ObjectName").value();
// Get model name paramter from BT ports
getInput("ObjectName", m_model_name);
}
PoseEstimationSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<PoseEstimationSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
bool setRequest(Request::SharedPtr &request) override {
getInput("ReqKind", m_req_type);
request->set__transition(transition_event(m_req_type));
return true;
}
BT::NodeStatus
handle_response(const PoseEstimationSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
return NodeStatus::FAILURE;
}
return BT::NodeStatus::FAILURE;
return NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("ObjectName"),
BT::InputPort<std::string>("ObjectPath"),
InputPort<std::string>("ReqKind"),
InputPort<std::string>("ObjectName"),
InputPort<std::string>("ObjectPath"),
});
}
private:
uint8_t transition_id_{};
std::string _model_name{};
std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
uint8_t m_transition_id{};
std::string m_model_name{};
std::string m_model_type{};
std::string m_req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> m_params;
rcl_interfaces::msg::Parameter m_param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
lifecycle_msgs::msg::Transition translation{};
// ParamSetter param_setter(m_params_client);
if (req_type == "configure") {
set_mesh_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
// auto str_mesh_param = std::make_shared<SetParamShareDirectoryStrategy>("model_name", m_model_name);
// param_setter.setStrategy(str_mesh_param);
// param_setter.setParam()
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "calibrate") {
set_str_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "activate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
} else if (req_type == "deactivate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
} else if (req_type == "cleanup") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
}
// calibrate
translation.set__id(transition_id_);
translation.set__id(m_transition_id);
return translation;
}
@ -97,28 +102,24 @@ private:
}
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _model_name + ">");
RCLCPP_INFO_STREAM(logger(),
"Set string parameter: <" + m_model_name + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> params{
rclcpp::Parameter("model_name", m_model_name)};
m_params_client->set_parameters(params);
}
void set_mesh_param() {
auto _package_name =
auto t_package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
m_model_type = build_model_path(m_model_name, t_package_name);
RCLCPP_INFO_STREAM(logger(), m_model_type);
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> params{
rclcpp::Parameter("mesh_path", m_model_name)};
m_params_client->set_parameters(params);
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<PoseEstimation>("PoseEstimation");
}
CreateRosNodePlugin(PoseEstimation, "PoseEstimation");

43
rbs_bt_executor/src/TreeRunner.cpp Normal file
View file

@ -0,0 +1,43 @@
#include <behaviortree_ros2/tree_execution_server.hpp>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>
// #include <rclcpp/logging.hpp>
// #include <std_msgs/msg/float32.hpp>
class RbsBtExecutor : public BT::TreeExecutionServer {
public:
RbsBtExecutor(const rclcpp::NodeOptions &options)
: TreeExecutionServer(options) {}
void onTreeCreated(BT::Tree &tree) override {
logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
}
std::optional<std::string>
onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override {
// RCLCPP_INFO(logger_cout_, "Tree completed with code: %d", status );
logger_cout_.reset();
return std::nullopt;
}
private:
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
// rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto action_server = std::make_shared<RbsBtExecutor>(options);
rclcpp::executors::MultiThreadedExecutor exec(
rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
exec.add_node(action_server->node());
exec.spin();
exec.remove_node(action_server->node());
rclcpp::shutdown();
}

53
rbs_bt_executor/src/gripper_move.cpp
View file

@ -1,53 +0,0 @@
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rclcpp/parameter.hpp"
using namespace BT;
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
class GripperControl : public BtAction<GripperCommand> {
public:
GripperControl(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<GripperCommand>(name, config) {
if (!_node->has_parameter("open")) {
_node->declare_parameter("open", position.open);
}
if (!_node->has_parameter("close")) {
_node->declare_parameter("close", position.close);
}
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose", pose);
double targetPose = _node->get_parameter(pose).as_double();
RCLCPP_INFO(_node->get_logger(), "Send target pose: %f", targetPose);
auto goal = GripperCommand::Goal();
goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(), "Goal send");
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("pose")});
}
private:
struct {
double open = 0.008;
double close = 0.000;
} position;
std::string pose;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GripperControl>("GripperControl");
}

64
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -1,62 +1,62 @@
#include "behavior_tree/BtService.hpp"
// #include "behavior_tree/BtService.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
using namespace BT;
using namespace std::chrono_literals;
using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
class RbsBtAction : public BtService<RbsBtActionSrv> {
class RbsBtAction : public RosServiceNode<RbsBtActionSrv> {
public:
RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
: BtService<RbsBtActionSrv>(name, config) {
RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
: RosServiceNode<RbsBtActionSrv>(name, conf, params) {
_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
m_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name);
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, NODE_NAME);
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
while (!_set_params_client->wait_for_service(1s)) {
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again...");
RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
}
}
RbsBtActionSrv::Request::SharedPtr populate_request() override {
auto request = std::make_shared<RbsBtActionSrv::Request>();
request->action = _action_name;
request->sid = getInput<std::string>(STR_SID).value();
request->command = getInput<std::string>(STR_COMMAND).value();
return request;
bool setRequest(Request::SharedPtr &request) override {
getInput(STR_SID, request->sid);
getInput(STR_COMMAND, request->command);
return true;
}
BT::NodeStatus handle_response(const RbsBtActionSrv::Response::SharedPtr response) override {
if (response->ok) {
return BT::NodeStatus::SUCCESS;
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
return BT::NodeStatus::FAILURE;
return NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
InputPort<std::string>(STR_SID),
InputPort<std::string>(STR_ACTION),
InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string _action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::string m_action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<RbsBtAction>("RbsBtAction");
}
CreateRosNodePlugin(RbsBtAction, "RbsAction")