robossembler/runtime
7
0
Fork 0
Code Issues 5 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

21
rbs_bt_executor/src/AddPlanningSceneObject.cpp
View file

@ -1,21 +1,19 @@
#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();
@ -26,8 +24,8 @@ public:
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;
} }
@ -38,6 +36,7 @@ public:
InputPort<std::vector<double>>("pose"), InputPort<std::vector<double>>("pose"),
}); });
} }
private: private:
std::string object_name_{}; std::string object_name_{};
}; };

32
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>("StateKind"),
BT::InputPort<std::string>("AssembleName"), BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"), BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName") 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");
} }

43
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -1,27 +1,26 @@
#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(), "Starting process for object: " << object_name_); RCLCPP_INFO_STREAM(_node->get_logger(),
"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_;
@ -30,16 +29,23 @@ public:
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 "
"orientation.z: %f \n\t orientation.w: %f",
response->grasp[2].position.x, response->grasp[2].position.y,
response->grasp[2].position.z, response->grasp[2].orientation.x,
response->grasp[2].orientation.y,
response->grasp[2].orientation.z,
response->grasp[2].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()"); RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]); setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]); setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]); setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[3]);
setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose", response->grasp[2]); setOutput<geometry_msgs::msg::Pose>("PostPostGraspPose",
response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]); setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]); setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]); setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[1]);
@ -54,21 +60,21 @@ public:
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>(
"PostPostGraspPose"), // TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PlacePose"), OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"), OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"), OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
}); });
} }
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) {
{
std::map<std::string, int> directions; std::map<std::string, int> directions;
geometry_msgs::msg::Vector3 vector; geometry_msgs::msg::Vector3 vector;
directions["x"] = 1; directions["x"] = 1;
directions["y"] = 2; directions["y"] = 2;
directions["z"] = 3; directions["z"] = 3;
switch(directions[str]){ switch (directions[str]) {
case 1: case 1:
vector.x = 1; vector.x = 1;
vector.y = 0; vector.y = 0;
@ -87,6 +93,7 @@ public:
}; };
return vector; return vector;
} }
private: private:
std::string object_name_{}; std::string object_name_{};
std::string grasp_direction_str_{}; std::string grasp_direction_str_{};

48
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,38 +1,36 @@
#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(), "Joint value size " << joint_values_.size()); RCLCPP_INFO_STREAM(_node->get_logger(),
"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_;
@ -41,22 +39,18 @@ class MoveToJointState : public BtAction<MoveToJointStateAction>
return goal; return goal;
} }
static PortsList providedPorts() static PortsList providedPorts() {
{ return providedBasicPorts(
return providedBasicPorts({ {InputPort<std::string>("robot_name"),
InputPort<std::string>("robot_name"), InputPort<std::vector<double>>("PrePlaceJointState")});
InputPort<std::vector<double>>("PrePlaceJointState")
});
} }
private: private:
std::string robot_name_{}; std::string robot_name_{};
std::vector<double> joint_values_; 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");
} }

50
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,27 +9,28 @@
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(), "\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", 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.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); 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.robot_name = robot_name_;
goal.target_pose = pose_des; goal.target_pose = pose_des;
@ -41,28 +42,23 @@ class MoveToPose : public BtAction<MoveToPoseAction>
return goal; return goal;
} }
static PortsList providedPorts() static PortsList providedPorts() {
{ return providedBasicPorts({InputPort<std::string>("robot_name"),
return providedBasicPorts({ InputPort<geometry_msgs::msg::Pose>("pose")});
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
} }
private: private:
std::string robot_name_; std::string robot_name_;
//std::vector<double> pose_; // std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des; geometry_msgs::msg::Pose pose_des;
//std::map<std::string, geometry_msgs::msg::Pose> Poses; // std::map<std::string, geometry_msgs::msg::Pose> Poses;
// geometry_msgs::msg::Pose array_to_pose(std::vector<double> pose_array){ // 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"

35
rbs_bt_executor/src/gripper_move.cpp
View file

@ -1,42 +1,39 @@
#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)
public: : BtAction<GripperCommand>(name, config) {
GripperControl(const std::string & name, const BT::NodeConfiguration & config): BtAction<GripperCommand>(name, config) { if (!_node->has_parameter("open")) {
if(!_node->has_parameter("open")) { _node->declare_parameter("open", position.open);
_node->declare_parameter("open",position.open);
} }
if(!_node->has_parameter("close")) { if (!_node->has_parameter("close")) {
_node->declare_parameter("close",position.close); _node->declare_parameter("close", position.close);
} }
if(!_node->has_parameter("midPosition")) { if (!_node->has_parameter("midPosition")) {
_node->declare_parameter("midPosition",position.closeFull); _node->declare_parameter("midPosition", position.closeFull);
} }
RCLCPP_INFO(_node->get_logger(),"Init the GripperControl Bt node");
} }
GripperCommand::Goal populate_goal() override { GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose",pose); getInput<std::string>("pose", pose);
double targetPose = _node->get_parameter(pose).as_double(); double targetPose = _node->get_parameter(pose).as_double();
RCLCPP_INFO(_node->get_logger(), "Send target pose: %f",targetPose); RCLCPP_INFO(_node->get_logger(), "Send target pose: %f", targetPose);
auto goal = GripperCommand::Goal(); auto goal = GripperCommand::Goal();
goal.position = targetPose; goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(),"Goal send"); RCLCPP_INFO(_node->get_logger(), "Goal send");
return goal; return goal;
} }
@ -45,8 +42,7 @@ class GripperControl: public BtAction<GripperCommand> {
return providedBasicPorts({InputPort<std::string>("pose")}); return providedBasicPorts({InputPort<std::string>("pose")});
} }
private:
private:
struct { struct {
double open = 0.2; double open = 0.2;
double closeFull = 0.8; double closeFull = 0.8;
@ -54,7 +50,6 @@ class GripperControl: public BtAction<GripperCommand> {
} position; } position;
std::string pose; std::string pose;
}; };
BT_REGISTER_NODES(factory) { BT_REGISTER_NODES(factory) {