The following code is mainly the same as in this MoveIt tutorial: http://docs.ros.org/hydro/api/pr2_moveit_tutorials/html/planning/src/doc/move_group_interface_tutorial.html
The code should print the name of the added collision object, but it does not print it and the object does not show up in Rviz. However, if collision objects are added by using the GUI in Rviz, they do show up and they do get printed.
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = range_sensor_frame;
collision_object.id = "sensor_mount";
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.05;
primitive.dimensions[1] = 0.8;
primitive.dimensions[2] = 0.11;
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.21;
box_pose.position.y = 0.0;
box_pose.position.z = 0.2;
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
std::vector collision_objects;
collision_objects.push_back(collision_object);
std::cout << "collision_objects.size(): " << collision_objects.size() << std::endl;
std::cout << collision_objects[0] << std::endl;
planning_scene_interface.addCollisionObjects(collision_objects);
ros::Duration(2.0).sleep();
std::vector obj = planning_scene_interface.getKnownObjectNames();
if (obj.size() > 0)
{
std::cout << std::endl << "-- KNOWN COLLISION OBJECTS --" << std::endl;
for (int i = 0; i < obj.size(); ++i)
std::cout << obj[i] << std::endl;
}
This is in **ROS Hydro**. I've also posted this question https://groups.google.com/forum/#!topic/moveit-users/sfXthHuxyx0.
↧