(MoveIt) PlanningSceneInterface does not add collision object [closed]
The following code is mainly the same as in this MoveIt tutorial: http://docs.ros.org/hydro/api/pr2_mov...
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/#!top... .
Did you end up figuring this out? I'd prefer this interface over using the planning_scene directly, though at least that works.
Please consider using the
applyCollisionObject(s)methods available starting from indigo.In the case reported above an additional sleep before the
addCollisionObjectscall could help. It's likely that the topic connection to move_group was not ready yet.Both of those solutions worked, thanks!
I've closed this since v4hn's solutions worked for rkeatin3.
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...