moveit2
The MoveIt Motion Planning Framework for ROS 2.
demo.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2012, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
38 #include <ros/ros.h>
39 
41 {
42  std::vector<moveit_msgs::msg::Grasp> grasps;
43  for (std::size_t i = 0; i < 20; ++i)
44  {
45  geometry_msgs::PoseStamped p = group.getRandomPose();
46  p.pose.orientation.x = 0;
47  p.pose.orientation.y = 0;
48  p.pose.orientation.z = 0;
49  p.pose.orientation.w = 1;
50  moveit_msgs::msg::Grasp g;
51  g.grasp_pose = p;
52  g.pre_grasp_approach.direction.vector.x = 1.0;
53  g.post_grasp_retreat.direction.vector.z = 1.0;
54  g.post_grasp_retreat.direction.header = p.header;
55  g.pre_grasp_approach.min_distance = 0.2;
56  g.pre_grasp_approach.desired_distance = 0.4;
57  g.post_grasp_retreat.min_distance = 0.1;
58  g.post_grasp_retreat.desired_distance = 0.27;
59  g.pre_grasp_posture.joint_names.resize(1, "r_gripper_joint");
60  g.pre_grasp_posture.points.resize(1);
61  g.pre_grasp_posture.points[0].positions.resize(1);
62  g.pre_grasp_posture.points[0].positions[0] = 1;
63 
64  g.grasp_posture.joint_names.resize(1, "r_gripper_joint");
65  g.grasp_posture.points.resize(1);
66  g.grasp_posture.points[0].positions.resize(1);
67  g.grasp_posture.points[0].positions[0] = 0;
68 
69  grasps.push_back(g);
70  }
71  group.pick("bubu", std::move(grasps));
72 }
73 
75 {
76  std::vector<moveit_msgs::action::PlaceLocation> loc;
77  for (std::size_t i = 0; i < 20; ++i)
78  {
79  geometry_msgs::PoseStamped p = group.getRandomPose();
80  p.pose.orientation.x = 0;
81  p.pose.orientation.y = 0;
82  p.pose.orientation.z = 0;
83  p.pose.orientation.w = 1;
84  moveit_msgs::action::PlaceLocation g;
85  g.place_pose = p;
86  g.pre_place_approach.direction.vector.x = 1.0;
87  g.post_place_retreat.direction.vector.z = 1.0;
88  g.post_place_retreat.direction.header = p.header;
89  g.pre_place_approach.min_distance = 0.2;
90  g.pre_place_approach.desired_distance = 0.4;
91  g.post_place_retreat.min_distance = 0.1;
92  g.post_place_retreat.desired_distance = 0.27;
93 
94  g.post_place_posture.joint_names.resize(1, "r_gripper_joint");
95  g.post_place_posture.points.resize(1);
96  g.post_place_posture.points[0].positions.resize(1);
97  g.post_place_posture.points[0].positions[0] = 0;
98 
99  loc.push_back(g);
100  }
101  group.place("bubu", std::move(loc));
102 }
103 
105 {
106 }
107 
108 int main(int argc, char** argv)
109 {
110  ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName);
111 
112  ros::AsyncSpinner spinner(1);
113  spinner.start();
114 
115  moveit::planning_interface::MoveGroupInterface group(argc > 1 ? argv[1] : "right_arm");
116  demoPlace(group);
117 
118  ros::Duration(2).sleep();
119 
120  return 0;
121 }
Client class to conveniently use the ROS interfaces provided by the move_group node.
void demoPlace(moveit::planning_interface::MoveGroupInterface &group)
Definition: demo.cpp:74
void demoPick(moveit::planning_interface::MoveGroupInterface &group)
Definition: demo.cpp:40
int main(int argc, char **argv)
Definition: demo.cpp:108
void attachObject()
Definition: demo.cpp:104
p
Definition: pick.py:62