Robowflex  v0.1
Making MoveIt Easy
ur5_pool.cpp File Reference

Go to the source code of this file.

Functions

int main (int argc, char **argv)
 

Function Documentation

◆ main()

int main ( int  argc,
char **  argv 
)

Definition at line 17 of file ur5_pool.cpp.

18 {
19  // Startup ROS
20  ROS ros(argc, argv);
21 
22  // Create the default UR5 robot.
23  auto ur5 = std::make_shared<UR5Robot>();
24  ur5->initialize();
25 
26  // Create an empty scene.
27  auto scene = std::make_shared<Scene>(ur5);
28 
29  // Create a pool of default planners for the UR5.
30  auto planner = std::make_shared<PoolPlanner>(ur5);
31  planner->initialize<OMPL::UR5OMPLPipelinePlanner>();
32 
33  // Create a motion planning request with a pose goal.
34  MotionRequestBuilder request(planner, "manipulator");
35  request.setStartConfiguration({0.0677, -0.8235, 0.9860, -0.1624, 0.0678, 0.0});
36 
37  RobotPose pose = RobotPose::Identity();
38  pose.translate(Eigen::Vector3d{-0.268, -0.826, 1.313});
39  Eigen::Quaterniond orn{0, 0, 1, 0};
40 
41  request.setGoalRegion("ee_link", "world", // links
42  pose, Geometry::makeSphere(0.1), // position
43  orn, {0.01, 0.01, 0.01} // orientation
44  );
45 
46  // Submit a blocking call for planning. This plan is executed in a different thread
47  planning_interface::MotionPlanResponse res = planner->plan(scene, request.getRequest());
48  if (res.error_code_.val != moveit_msgs::MoveItErrorCodes::SUCCESS)
49  return 1;
50 
51  // Submit a set of asynchronous planning calls.
52  auto job1 = planner->submit(scene, request.getRequest());
53  auto job2 = planner->submit(scene, request.getRequest());
54  auto job3 = planner->submit(scene, request.getRequest());
55  auto job4 = planner->submit(scene, request.getRequest());
56  auto job5 = planner->submit(scene, request.getRequest());
57  auto job6 = planner->submit(scene, request.getRequest());
58  auto job7 = planner->submit(scene, request.getRequest());
59  auto job8 = planner->submit(scene, request.getRequest());
60 
61  // Cancel a job (if already running, nothing happens, but if canceled before execution the job is skipped)
62  job5->cancel();
63 
64  // Wait for a job to complete
65  job1->wait();
66 
67  // Get results of plans. These block until results are available.
68  planning_interface::MotionPlanResponse res1 = job1->get();
69  planning_interface::MotionPlanResponse res2 = job2->get();
70  planning_interface::MotionPlanResponse res3 = job3->get();
71  planning_interface::MotionPlanResponse res4 = job4->get();
72 
73  // We canceled job5, so no result is guaranteed.
74  // planning_interface::MotionPlanResponse res5 = job5->get();
75 
76  planning_interface::MotionPlanResponse res6 = job6->get();
77  planning_interface::MotionPlanResponse res7 = job7->get();
78  planning_interface::MotionPlanResponse res8 = job8->get();
79 
80  return 0;
81 }
A helper class to build motion planning requests for a robowflex::Planner.
Definition: builder.h:34
Convenience class for the default motion planning pipeline for UR5.
Definition: ur5.h:65
RAII-pattern for starting up ROS.
Definition: util.h:52
std::shared_ptr< dart::dynamics::SphereShape > makeSphere(double radius)
Create a sphere.
Definition: structure.cpp:385
std::decay< decltype(std::declval< moveit::core::Transforms >().getTransform("")) >::type RobotPose
A pose (point in SE(3)) used in various functions. Defined from what MoveIt! uses.
Definition: adapter.h:24
Functions for loading and animating scenes in Blender.
moveit_msgs::MoveItErrorCodes error_code_