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

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 "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;
using AddPlanningSceneObjectClient =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient>
{
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");
}
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_;
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;
}
return request;
}
BT::NodeStatus handle_response(AddPlanningSceneObjectClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
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"),
});
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
private:
std::string object_name_{};
std::string object_name_{};
};
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 <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>
{
class AssembleState : public BtService<AssembleStateSrv> {
public:
AssembleState(const std::string & name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config)
{
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()
{
AssembleStateSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<AssembleStateSrv::Request>();
auto state_kind = getInput<std::string>("StateKind").value();
request->req_kind = -1;
@ -39,20 +36,18 @@ public:
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
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()
{
return providedBasicPorts({
BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")
});
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:
@ -61,7 +56,6 @@ private:
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory)
{
BT_REGISTER_NODES(factory) {
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 "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>
{
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
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;
}
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[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()");
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[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;
}
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[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()");
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[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() {
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"),
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"), //TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
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"),
OutputPort<geometry_msgs::msg::Pose>(
"PostPostGraspPose"), // TODO: change to std::vector<>
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;
}
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_{};
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) {

76
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,62 +1,56 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#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 MoveToJointStateAction = rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER = rclcpp::get_logger("MoveToJointStateActionClient");
using MoveToJointStateAction =
rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction>
{
public:
MoveToJointState(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToJointStateAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
class MoveToJointState : public BtAction<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>>("PrePlaceJointState", joint_values_);
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("PrePlaceJointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]", robot_name_.c_str());
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");
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_;
return goal;
}
RCLCPP_INFO(_node->get_logger(), "Goal populated");
static PortsList providedPorts()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")
});
}
return goal;
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("PrePlaceJointState")});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToJointState>("MoveToJointState");
BT_REGISTER_NODES(factory) {
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 "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
@ -9,60 +9,56 @@
using namespace BT;
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>
{
public:
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
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);
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 = 0.5;
goal.end_effector_velocity = 0.5;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
robot_name_.c_str());
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()
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<geometry_msgs::msg::Pose>("pose")
});
}
RCLCPP_INFO(_node->get_logger(), "Goal populated");
private:
std::string robot_name_;
//std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
//std::map<std::string, geometry_msgs::msg::Pose> Poses;
return goal;
}
// 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
BT_REGISTER_NODES(factory)
{
factory.registerNodeType<MoveToPose>("MoveToPose");
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}

10
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -18,7 +18,8 @@ public:
: BtService<PoseEstimationSrv>(name, config) {
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)) {
if (!rclcpp::ok()) {
@ -92,14 +93,15 @@ private:
}
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);
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);
}
};
#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 "behaviortree_cpp_v3/bt_factory.h"
#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> {
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:
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(), "Send target pose: %f", targetPose);
RCLCPP_INFO(_node->get_logger(),"Init the GripperControl Bt node");
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose",pose);
double targetPose = _node->get_parameter(pose).as_double();
auto goal = GripperCommand::Goal();
RCLCPP_INFO(_node->get_logger(), "Send target pose: %f",targetPose);
goal.position = targetPose;
auto goal = GripperCommand::Goal();
goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(), "Goal send");
RCLCPP_INFO(_node->get_logger(),"Goal send");
return goal;
}
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("pose")});
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("pose")});
}
private:
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
} position;
std::string pose;
private:
struct {
double open = 0.2;
double closeFull = 0.8;
double close = 0.4;
} position;
std::string pose;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GripperControl>("GripperControl");
factory.registerNodeType<GripperControl>("GripperControl");
}