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

bt-node clang-format

Browse source
This commit is contained in:
Ilya Uraev 2023-11-22 14:43:30 +03:00
parent 4decc40c88
commit 034e172f62
7 changed files with 253 additions and 266 deletions
Download patch file Download diff file Expand all files Collapse all files

63
rbs_bt_executor/src/AddPlanningSceneObject.cpp
View file

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

36
rbs_bt_executor/src/AssembleProcessState.cpp
View file

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

173
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

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

76
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,62 +1,56 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp" #include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp" #include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h" #include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry" // #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp" #include "rclcpp/parameter.hpp"
using namespace BT; using namespace BT;
using MoveToJointStateAction = rbs_skill_interfaces::action::MoveitSendJointStates; using MoveToJointStateAction =
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToJointStateActionClient"); rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction> class MoveToJointState : public BtAction<MoveToJointStateAction> {
{ public:
public: MoveToJointState(const std::string &name, const BT::NodeConfiguration &config)
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config) : BtAction<MoveToJointStateAction>(name, config) {}
: BtAction<MoveToJointStateAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
MoveToJointStateAction::Goal populate_goal() override MoveToJointStateAction::Goal populate_goal() override {
{ getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("robot_name", robot_name_); getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
auto goal = MoveToJointStateAction::Goal(); auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]", robot_name_.c_str()); RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
robot_name_.c_str());
for (auto joint : joint_values_) for (auto joint : joint_values_) {
{ RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint); }
} RCLCPP_INFO_STREAM(_node->get_logger(),
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value size " << joint_values_.size()); "Joint value size " << joint_values_.size());
goal.robot_name = robot_name_; goal.robot_name = robot_name_;
goal.joint_values = joint_values_; goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal; RCLCPP_INFO(_node->get_logger(), "Goal populated");
}
static PortsList providedPorts() return goal;
{ }
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")
});
}
private: static PortsList providedPorts() {
std::string robot_name_{}; return providedBasicPorts(
std::vector<double> joint_values_; {InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState }; // class MoveToJointState
BT_REGISTER_NODES(factory) BT_REGISTER_NODES(factory) {
{ factory.registerNodeType<MoveToJointState>("MoveToJointState");
factory.registerNodeType<MoveToJointState>("MoveToJointState");
} }

86
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,7 +1,7 @@
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp" #include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp" #include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h" #include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry" // #include "Eigen/Geometry"
@ -9,60 +9,56 @@
using namespace BT; using namespace BT;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose; using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToPoseActionClient"); static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToPoseActionClient");
class MoveToPose : public BtAction<MoveToPoseAction> class MoveToPose : public BtAction<MoveToPoseAction> {
{ public:
public: MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
MoveToPose(const std::string & name, const BT::NodeConfiguration & config) : BtAction<MoveToPoseAction>(name, config) {}
: BtAction<MoveToPoseAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
MoveToPoseAction::Goal populate_goal() override MoveToPoseAction::Goal populate_goal() override {
{ getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("robot_name", robot_name_); getInput<geometry_msgs::msg::Pose>("pose", pose_des);
getInput<geometry_msgs::msg::Pose>("pose", pose_des); RCLCPP_INFO(_node->get_logger(),
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", "\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
pose_des.position.x, pose_des.position.y, pose_des.position.z, "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
pose_des.orientation.x, pose_des.orientation.y, pose_des.orientation.z, pose_des.orientation.w); "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(); auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]", robot_name_.c_str()); 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; goal.robot_name = robot_name_;
} goal.target_pose = pose_des;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
static PortsList providedPorts() RCLCPP_INFO(_node->get_logger(), "Goal populated");
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
private: return goal;
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){ static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("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 }; // class MoveToPose
BT_REGISTER_NODES(factory) BT_REGISTER_NODES(factory) {
{ factory.registerNodeType<MoveToPose>("MoveToPose");
factory.registerNodeType<MoveToPose>("MoveToPose");
} }

10
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -18,7 +18,8 @@ public:
: BtService<PoseEstimationSrv>(name, config) { : BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node."); RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, "/pose_estimation"); _set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/pose_estimation");
while (!_set_params_client->wait_for_service(1s)) { while (!_set_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) { if (!rclcpp::ok()) {
@ -92,14 +93,15 @@ private:
} }
void set_mesh_param() { void set_mesh_param() {
auto _package_name = ament_index_cpp::get_package_share_directory("rbs_perception"); auto _package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name); _model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path); RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
std::vector<rclcpp::Parameter> _parameters{rclcpp::Parameter("mesh_path", _model_path)}; std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_path)};
_set_params_client->set_parameters(_parameters); _set_params_client->set_parameters(_parameters);
} }
}; };
#include "behaviortree_cpp_v3/bt_factory.h" #include "behaviortree_cpp_v3/bt_factory.h"

75
rbs_bt_executor/src/gripper_move.cpp
View file

@ -1,62 +1,57 @@
#include "rbs_skill_interfaces/action/gripper_command.hpp" #include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp" #include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rclcpp/parameter.hpp" #include "rclcpp/parameter.hpp"
using namespace BT; using namespace BT;
using GripperCommand = rbs_skill_interfaces::action::GripperCommand; using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
class GripperControl : public BtAction<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);
}
if (!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition", position.closeFull);
}
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose", pose);
double targetPose = _node->get_parameter(pose).as_double();
public: RCLCPP_INFO(_node->get_logger(), "Send target pose: %f", targetPose);
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);
}
if(!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition",position.closeFull);
}
RCLCPP_INFO(_node->get_logger(),"Init the GripperControl Bt node"); auto goal = GripperCommand::Goal();
}
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); goal.position = targetPose;
auto goal = GripperCommand::Goal(); RCLCPP_INFO(_node->get_logger(), "Goal send");
goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(),"Goal send"); return goal;
}
return goal; static PortsList providedPorts() {
} return providedBasicPorts({InputPort<std::string>("pose")});
}
static PortsList providedPorts() { private:
return providedBasicPorts({InputPort<std::string>("pose")}); struct {
} double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
private: } position;
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
} position;
std::string pose;
std::string pose;
}; };
BT_REGISTER_NODES(factory) { BT_REGISTER_NODES(factory) {
factory.registerNodeType<GripperControl>("GripperControl"); factory.registerNodeType<GripperControl>("GripperControl");
} }