(MoveIt) PlanningSceneInterface does not add collision object
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<moveit_msgs::CollisionObject> 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<std::string> 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.
Asked by atp on 2015-05-15 12:14:55 UTC
Comments
Did you end up figuring this out? I'd prefer this interface over using the planning_scene directly, though at least that works.
Asked by rkeatin3 on 2017-08-04 19:32:45 UTC
Please consider using the
applyCollisionObject(s)
methods available starting from indigo.In the case reported above an additional sleep before the
addCollisionObjects
call could help. It's likely that the topic connection to move_group was not ready yet.Asked by v4hn on 2017-08-05 06:00:04 UTC
Both of those solutions worked, thanks!
Asked by rkeatin3 on 2017-08-05 17:53:49 UTC
I've closed this since v4hn's solutions worked for rkeatin3.
Asked by atp on 2017-08-05 22:25:51 UTC
Hi, is there a function that allows to remove the collision object immediately? Because the planning_scene_interface.removeCollisionObjects(object_ids) function requires at least 0.5 seconds delay to be effective on Rviz...
Asked by enrico on 2018-08-26 09:03:08 UTC