BehaviorTree.MTC/examples/bt_pick_place.cpp at master · captain-yoshi/BehaviorTree.MTC · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
#include <ros/ros.h>

#include <behaviortree_mtc/container.h>
#include <behaviortree_mtc/stage.h>
#include <behaviortree_mtc/task.h>

#include <behaviortree_mtc/moveit/planning_scene_interface.h>

#include <behaviortree_mtc/msgs/geometry_msgs.h>
#include <behaviortree_mtc/msgs/moveit_msgs.h>

#include <behaviortree_mtc/solvers/cartesian_path.h>
#include <behaviortree_mtc/solvers/joint_interpolation.h>
#include <behaviortree_mtc/solvers/pipeline_planner.h>

#include <behaviortree_mtc/serialization/std_containers.h>

#include <behaviortree_mtc/stages/compute_ik.h>
#include <behaviortree_mtc/stages/connect.h>
#include <behaviortree_mtc/stages/current_state.h>
#include <behaviortree_mtc/stages/generate_grasp_pose.h>
#include <behaviortree_mtc/stages/generate_place_pose.h>
#include <behaviortree_mtc/stages/modify_planning_scene.h>
#include <behaviortree_mtc/stages/move_relative.h>
#include <behaviortree_mtc/stages/move_to.h>

#include "behaviortree_cpp/bt_factory.h"
#include "behaviortree_cpp/loggers/bt_file_logger_v2.h"
#include "behaviortree_cpp/loggers/groot2_publisher.h"

using namespace BT;
using namespace BT::MTC;

int main(int argc, char** argv)
{
  ros::init(argc, argv, "bt_pick_place");
  ros::NodeHandle pnh("~");

  ros::AsyncSpinner spinner(1);
  spinner.start();

  BehaviorTreeFactory factory;

  // geometry_msgs
  factory.registerNodeType<GeometryMsgsPose>("PoseMsg");
  factory.registerNodeType<GeometryMsgsPoseStamped>("PoseStampedMsg");
  factory.registerNodeType<GeometryMsgsTwistStamped>("TwistStampedMsg");
  factory.registerNodeType<GeometryMsgsVector3Stamped>("Vector3StampedMsg");

  // moveit_msgs
  factory.registerNodeType<MoveItMsgsCollisionObjectBox>("CollisionObjectBoxMsg");
  factory.registerNodeType<MoveItMsgsCollisionObjectCylinder>("CollisionObjectCylinderMsg");

  // moveit
  factory.registerNodeType<PlanningSceneInterface>("PlanningSceneInterface");
  factory.registerNodeType<PlanningSceneInterfaceAddCollisionObject>("AddCollisionObjectToPlanningSceneInterface");

  // mtc stage/containers/task
  factory.registerNodeType<PropertiesSet<moveit::task_constructor::Stage, std::string>>("SetStagePropertiesString");
  factory.registerNodeType<PropertiesSet<moveit::task_constructor::Task, std::string>>("SetTaskPropertiesString");
  factory.registerNodeType<PropertiesExposeTo<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("ExposeContainerPropertiesToContainer");
  factory.registerNodeType<PropertiesExposeTo<moveit::task_constructor::Task, moveit::task_constructor::ContainerBase>>("ExposeTaskPropertiesToContainer");
  factory.registerNodeType<PropertiesConfigureInitFrom<moveit::task_constructor::Stage>>("ConfigureStagePropertiesInitFrom");
  factory.registerNodeType<PropertiesConfigureInitFrom<moveit::task_constructor::ContainerBase>>("ConfigureContainerPropertiesInitFrom");
  factory.registerNodeType<StageGetRawPtr>("GetStageRawPtr");
  factory.registerNodeType<StageMove<moveit::task_constructor::Stage, moveit::task_constructor::ContainerBase>>("MoveStageToContainer");
  factory.registerNodeType<StageMove<moveit::task_constructor::Stage, moveit::task_constructor::Task>>("MoveStageToTask");
  factory.registerNodeType<StageMove<moveit::task_constructor::ContainerBase, moveit::task_constructor::ContainerBase>>("MoveContainerToContainer");
  factory.registerNodeType<StageMove<moveit::task_constructor::ContainerBase, moveit::task_constructor::Task>>("MoveContainerToTask");
  factory.registerNodeType<SerialContainer>("SerialContainer");
  factory.registerNodeType<Task>("Task");
  factory.registerNodeType<TaskLoadRobotModel>("LoadRobotModelToTask");
  factory.registerNodeType<TaskPlan>("PlanTask");
  factory.registerScriptingEnums<moveit::task_constructor::Stage::PropertyInitializerSource>();

  // mtc solvers
  factory.registerNodeType<CartesianPath>("CartesianPath");
  factory.registerNodeType<JointInterpolation>("JointInterpolation");
  factory.registerNodeType<PipelinePlanner>("PipelinePlanner");

  // mtc stages
  factory.registerNodeType<CurrentState>("CurrentState");
  factory.registerNodeType<GenerateGraspPose>("GenerateGraspPose");
  factory.registerNodeType<GeneratePlacePose>("GeneratePlacePose");
  factory.registerNodeType<MoveToNamedJointPose>("MoveToNamedJointPose");
  factory.registerNodeType<MoveToJoint>("MoveToJoint");
  factory.registerNodeType<MoveToPoint>("MoveToPoint");
  factory.registerNodeType<MoveToPose>("MoveToPose");
  factory.registerNodeType<MoveRelativeTranslate>("MoveRelativeTranslate");
  factory.registerNodeType<MoveRelativeJoint>("MoveRelativeJoint");
  factory.registerNodeType<MoveRelativeTwist>("MoveRelativeTwist");
  factory.registerNodeType<Connect>("Connect");
  factory.registerNodeType<ComputeIK>("ComputeIK");
  factory.registerNodeType<ModifyPlanningSceneAttachObjects>("ModifyPlanningSceneAttachObjects");
  factory.registerNodeType<ModifyPlanningSceneDetachObjects>("ModifyPlanningSceneDetachObjects");
  factory.registerNodeType<ModifyPlanningSceneForbidCollisionPairs>("ModifyPlanningSceneForbidCollisionPairs");
  factory.registerNodeType<ModifyPlanningSceneAllowCollisionPairs>("ModifyPlanningSceneAllowCollisionPairs");
  factory.registerNodeType<ModifyPlanningSceneAllowAllCollisions>("ModifyPlanningSceneAllowAllCollisions");
  factory.registerNodeType<ModifyPlanningSceneForbidAllCollisions>("ModifyPlanningSceneForbidAllCollisions");
  factory.registerNodeType<ModifyPlanningSceneAllowCollisionsJointModelGroup>("ModifyPlanningSceneAllowCollisionsJointModelGroup");
  factory.registerNodeType<ModifyPlanningSceneForbidCollisionsJointModelGroup>("ModifyPlanningSceneForbidCollisionsJointModelGroup");

  std::string path = PROJECT_WS;
  Tree tree = factory.createTreeFromFile(path + "/config/bt_pick_place.xml");

  // Connect the Groot2Publisher. This will allow Groot2 to
  // get the tree and poll status updates.
  const unsigned port = 1667;
  BT::Groot2Publisher publisher(tree, port);

  // Add two more loggers, to save the transitions into a file.
  // Both formats are compatible with Groot2

  // Logging with lightweight serialization
  BT::FileLogger2 logger2(tree, "t12_logger2.btlog");

  // Gives the user time to connect to Groot2
  int delay_ms;
  pnh.param<int>("delay_ms", delay_ms, 0.0);
  if(delay_ms)
  {
    std::cout << "Waiting " << delay_ms << " msec for connection with Groot2...\n\n"
              << std::endl;
    std::cout << "======================" << std::endl;
    std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
  }

  // Start ticking the Tree
  std::cout << "Starting Behavior Tree" << std::endl;
  std::cout << "======================" << std::endl;

  NodeStatus status = NodeStatus::IDLE;
  while(ros::ok() && (status == NodeStatus::IDLE || status == NodeStatus::RUNNING))
  {
    status = tree.tickExactlyOnce();
    std::cout << "Status = " << status << std::endl;

    ros::Duration sleep_time(0.01);
    sleep_time.sleep();
  }

  std::cout << "\n\nWaiting for shutdown, press CTRL-C to quit" << std::endl;
  ros::waitForShutdown();

  return 0;
}