bt-node clang-format
This commit is contained in:
parent
4decc40c88
commit
034e172f62
7 changed files with 253 additions and 266 deletions
|
@ -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) {
|
||||
|
|
|
@ -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");
|
||||
}
|
||||
|
|
|
@ -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) {
|
||||
|
|
|
@ -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");
|
||||
}
|
|
@ -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");
|
||||
}
|
|
@ -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"
|
||||
|
|
|
@ -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");
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue