19 #include <geometry_msgs/Pose.h> 20 #include <moveit_msgs/CollisionObject.h> 21 #include <shape_msgs/SolidPrimitive.h> 25 #include <boost/variant.hpp> 26 #include <boost/shared_ptr.hpp> 29 int main(
int argc,
char** argv)
31 ros::init(argc, argv,
"test_obstacle_publisher_node");
37 moveit_msgs::CollisionObject col_sphere_msg;
38 col_sphere_msg.id =
"sphere_primitive";
39 col_sphere_msg.header.frame_id =
"odom_combined";
40 col_sphere_msg.operation = moveit_msgs::CollisionObject::ADD;
42 shape_msgs::SolidPrimitive
sphere;
43 sphere.type = shape_msgs::SolidPrimitive::SPHERE;
44 sphere.dimensions.push_back(0.1);
46 geometry_msgs::Pose
pose;
47 pose.position.x = 1.0;
48 pose.position.y = 0.0;
49 pose.position.z = 0.0;
50 pose.orientation.w = 1.0;
52 col_sphere_msg.primitives.push_back(sphere);
53 col_sphere_msg.primitive_poses.push_back(pose);
59 moveit_msgs::CollisionObject col_sphere_mesh_msg;
60 col_sphere_mesh_msg.id =
"sphere_mesh";
61 col_sphere_mesh_msg.header.frame_id =
"odom_combined";
62 col_sphere_mesh_msg.operation = moveit_msgs::CollisionObject::ADD;
67 shape_msgs::Mesh sphere_mesh = boost::get<shape_msgs::Mesh>(sphere_mesh_msg);
69 pose.position.x = 1.0;
71 pose.position.z = 0.0;
72 pose.orientation.w = 1.0;
74 col_sphere_mesh_msg.meshes.push_back(sphere_mesh);
75 col_sphere_mesh_msg.mesh_poses.push_back(pose);
77 pub.
publish(col_sphere_mesh_msg);
81 moveit_msgs::CollisionObject col_box_msg;
82 col_box_msg.id =
"box_primitive";
83 col_box_msg.header.frame_id =
"odom_combined";
84 col_box_msg.operation = moveit_msgs::CollisionObject::ADD;
86 shape_msgs::SolidPrimitive box;
87 box.type = shape_msgs::SolidPrimitive::BOX;
88 box.dimensions.push_back(0.1);
89 box.dimensions.push_back(0.1);
90 box.dimensions.push_back(0.1);
92 pose.position.x = 1.0;
93 pose.position.y = 0.0;
94 pose.position.z = 1.0;
95 pose.orientation.w = 1.0;
97 col_box_msg.primitives.push_back(box);
98 col_box_msg.primitive_poses.push_back(pose);
104 moveit_msgs::CollisionObject col_box_mesh_msg;
105 col_box_mesh_msg.id =
"box_mesh";
106 col_box_mesh_msg.header.frame_id =
"odom_combined";
107 col_box_mesh_msg.operation = moveit_msgs::CollisionObject::ADD;
112 shape_msgs::Mesh box_mesh = boost::get<shape_msgs::Mesh>(box_mesh_msg);
114 pose.position.x = 1.0;
116 pose.position.z = 1.0;
117 pose.orientation.w = 1.0;
119 col_box_mesh_msg.meshes.push_back(box_mesh);
120 col_box_mesh_msg.mesh_poses.push_back(pose);
126 moveit_msgs::CollisionObject col_cylinder_msg;
127 col_cylinder_msg.id =
"cylinder_primitive";
128 col_cylinder_msg.header.frame_id =
"odom_combined";
129 col_cylinder_msg.operation = moveit_msgs::CollisionObject::ADD;
131 shape_msgs::SolidPrimitive cylinder;
132 cylinder.type = shape_msgs::SolidPrimitive::CYLINDER;
133 cylinder.dimensions.push_back(0.1);
134 cylinder.dimensions.push_back(0.1);
136 pose.position.x = 1.0;
137 pose.position.y = 0.0;
138 pose.position.z = 2.0;
139 pose.orientation.w = 1.0;
141 col_cylinder_msg.primitives.push_back(cylinder);
142 col_cylinder_msg.primitive_poses.push_back(pose);
148 moveit_msgs::CollisionObject col_cylinder_mesh_msg;
149 col_cylinder_mesh_msg.id =
"cylinder_mesh";
150 col_cylinder_mesh_msg.header.frame_id =
"odom_combined";
151 col_cylinder_mesh_msg.operation = moveit_msgs::CollisionObject::ADD;
156 shape_msgs::Mesh cylinder_mesh = boost::get<shape_msgs::Mesh>(cylinder_mesh_msg);
158 pose.position.x = 1.0;
160 pose.position.z = 2.0;
161 pose.orientation.w = 1.0;
163 col_cylinder_mesh_msg.meshes.push_back(cylinder_mesh);
164 col_cylinder_mesh_msg.mesh_poses.push_back(pose);
166 pub.
publish(col_cylinder_mesh_msg);
176 moveit_msgs::CollisionObject test_col_prim_msg;
177 test_col_prim_msg.id =
"test_primitive";
178 test_col_prim_msg.header.frame_id =
"odom_combined";
179 test_col_prim_msg.operation = moveit_msgs::CollisionObject::ADD;
181 shape_msgs::SolidPrimitive test_prim;
184 test_prim.type = shape_msgs::SolidPrimitive::CYLINDER;
185 test_prim.dimensions.push_back(0.1);
186 test_prim.dimensions.push_back(0.1);
189 geometry_msgs::Pose test_pose;
190 test_pose.position.x = 2.0;
191 test_pose.position.y = 0.0;
192 test_pose.position.z = 1.0;
193 test_pose.orientation.w = 1.0;
195 test_col_prim_msg.primitives.push_back(test_prim);
196 test_col_prim_msg.primitive_poses.push_back(test_pose);
198 pub.
publish(test_col_prim_msg);
202 moveit_msgs::CollisionObject test_col_mesh_msg;
203 test_col_mesh_msg.id =
"test_mesh";
204 test_col_mesh_msg.header.frame_id =
"odom_combined";
205 test_col_mesh_msg.operation = moveit_msgs::CollisionObject::ADD;
212 shape_msgs::Mesh test_mesh = boost::get<shape_msgs::Mesh>(test_mesh_msg);
214 test_pose.position.x = 2.0;
216 test_pose.position.z = 1.0;
217 test_pose.orientation.w = 1.0;
219 test_col_mesh_msg.meshes.push_back(test_mesh);
220 test_col_mesh_msg.mesh_poses.push_back(test_pose);
222 pub.
publish(test_col_mesh_msg);
void publish(const boost::shared_ptr< M > &message) const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
int main(int argc, char **argv)
bool constructMsgFromShape(const Shape *shape, ShapeMsg &shape_msg)
Mesh * createMeshFromResource(const std::string &resource)
boost::variant< shape_msgs::SolidPrimitive, shape_msgs::Mesh, shape_msgs::Plane > ShapeMsg