How to add a ground plane to MoveIt scene?
I want to add an XY plane to the PlanningScene so the arm of my robot doesn't hit the ground. Unfortunately I can't work out if I'm doing it right. I don't see anything in RViz. Also, I'm not sure how to instantiate Plane coefficients correctly. Please have a look on this code:
void insertGroundPlane()
{
moveit_msgs::CollisionObject object;
object.header.frame_id = "base_footprint";
shape_msgs::Plane plane;
plane.coef = {{0, 0, 1, 0}};
geometry_msgs::Pose pose;
pose.orientation.w = 1.0;
object.planes.push_back(plane);
object.plane_poses.push_back(pose);
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(object);
planning_scene.is_diff = true;
moveit_msgs::ApplyPlanningScene srv;
srv.request.scene = planning_scene;
planning_scene_diff_client.call(srv);
}