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

Add minimal pick-and-place case with world embedded

Browse source
This commit is contained in:
Igor Brylyov 2023-03-31 20:24:56 +00:00
parent 209e99a4b3
commit a87fb8a7ec
64 changed files with 2419 additions and 275 deletions
Download patch file Download diff file Expand all files Collapse all files

47
rbs_bt_executor/src/AddPlanningSceneObject.cpp Normal file
View file

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

63
rbs_bt_executor/src/GetPickPlacePoses.cpp Normal file
View file

@ -0,0 +1,63 @@
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtService.hpp"
using namespace BT;
using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient>
{
public:
GetPickPlacePose(const std::string & name, const BT::NodeConfiguration & config)
: BtService<GetPickPlacePoseClient>(name, config) {
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();
RCLCPP_INFO_STREAM(_node->get_logger(), "Starting process for object: " << object_name_);
request->object_name = object_name_;
return request;
}
BT::NodeStatus handle_response(GetPickPlacePoseClient::Response::SharedPtr response) override
{
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<std::vector<double>>("GraspPose", response->grasp_pose);
setOutput<std::vector<double>>("PreGraspPose", response->pre_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose", response->post_grasp_pose);
setOutput<std::vector<double>>("PostGraspPose2", response->post_grasp_pose_d);
setOutput<std::vector<double>>("PlacePose", response->place_pose);
setOutput<std::vector<double>>("PrePlacePose", response->pre_place_pose);
setOutput<std::vector<double>>("PostPlacePose", response->post_place_pose);
setOutput<std::vector<double>>("PrePlaceJointState", response->pre_place_joint_state);
setOutput<std::string>("NextObject", response->next_object);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
OutputPort<std::string>("ObjectName"),
OutputPort<std::vector<double>>("GraspPose"),
OutputPort<std::vector<double>>("PreGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose"),
OutputPort<std::vector<double>>("PostGraspPose2"),
OutputPort<std::vector<double>>("PlacePose"),
OutputPort<std::vector<double>>("PrePlacePose"),
OutputPort<std::vector<double>>("PostPlacePose"),
OutputPort<std::vector<double>>("PrePlaceJointState"),
OutputPort<std::string>("NextObject")
});
}
private:
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("GetPickPlacePoses");
}

62
rbs_bt_executor/src/MoveToJointStates.cpp Normal file
View file

@ -0,0 +1,62 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behavior_tree/BtAction.hpp"
#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");
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");
}
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());
for (auto joint : joint_values_)
{
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
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");
}

65
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -17,62 +17,21 @@ class MoveToPose : public BtAction<MoveToPoseAction>
MoveToPose(const std::string & name, const BT::NodeConfiguration & config)
: BtAction<MoveToPoseAction>(name, config)
{
if (!_node->has_parameter("pose1"))
{
_node->declare_parameter("pose1",std::vector<double>{ 0.11724797630931184,
0.46766635768602544,
0.5793862343094913,
0.9987999001314066,
0.041553846820940925,
-0.004693314677006583,
-0.025495295825239704
}
);
}
if (!_node->has_parameter("pose2"))
{
_node->declare_parameter("pose2",std::vector<double>{ -0.11661364861606047,
0.4492600889665261,
0.5591700913807053,
0.9962273179258467,
0.04057380155886888,
0.009225849745372298,
0.07615629548377048
}
);
}
if (!_node->has_parameter("pose3"))
{
_node->declare_parameter("pose3",std::vector<double>{ -0.07133612047767886,
0.41038906578748613,
0.2844649846989331,
0.999078481789772,
0.04109234110437432,
0.006539754292177074,
0.010527978961032304
}
);
}
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
MoveToPoseAction::Goal populate_goal() override
{
getInput<std::string>("robot_name", robot_name_);
getInput<std::string>("pose", pose_);
rclcpp::Parameter arr = _node->get_parameter(pose_);
std::vector<double> possd = arr.as_double_array();
getInput<std::vector<double>>("pose", pose_);
pose_des.position.x = possd[0];
pose_des.position.y = possd[1];
pose_des.position.z = possd[2];
pose_des.orientation.x = possd[3];
pose_des.orientation.y = possd[4];
pose_des.orientation.z = possd[5];
pose_des.orientation.w = possd[6];
pose_des.position.x = pose_[0];
pose_des.position.y = pose_[1];
pose_des.position.z = pose_[2];
pose_des.orientation.x = pose_[3];
pose_des.orientation.y = pose_[4];
pose_des.orientation.z = pose_[5];
pose_des.orientation.w = pose_[6];
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",
@ -84,8 +43,8 @@ class MoveToPose : public BtAction<MoveToPoseAction>
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
goal.end_effector_acceleration = 0.5;
goal.end_effector_velocity = 0.5;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
@ -96,13 +55,13 @@ class MoveToPose : public BtAction<MoveToPoseAction>
{
return providedBasicPorts({
InputPort<std::string>("robot_name"),
InputPort<std::string>("pose")
InputPort<std::vector<double>>("pose")
});
}
private:
std::string robot_name_;
std::string pose_;
std::vector<double> pose_;
geometry_msgs::msg::Pose pose_des;
std::map<std::string, geometry_msgs::msg::Pose> Poses;

62
rbs_bt_executor/src/gripper_move.cpp Normal file
View file

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