How to add obstacles to MoveIt with Python in ROS Indigo? This is what I tried, but it does not add anything:
scene_pub = rospy.Publisher('/planning_scene', PlanningScene)
rospy.wait_for_service('/get_planning_scene', 10.0)
get_planning_scene = rospy.ServiceProxy('/get_planning_scene', GetPlanningScene)
request = PlanningSceneComponents(components=PlanningSceneComponents.ALLOWED_COLLISION_MATRIX)
response = get_planning_scene(request)
coll_mat = response.scene.allowed_collision_matrix
print "response.scene.world.collision_objects:", response.scene.world.collision_objects
pose_box = Pose(Point(0,0,0), Quaternion(0,0,0,1))
box = SolidPrimitive()
box.type = SolidPrimitive.BOX
box.dimensions = [1,1,1]
coll_obj = CollisionObject()
coll_obj.header.stamp = rospy.Time(0)
coll_obj.header.frame_id = "/base"
coll_obj.id = "sensor_platform"
coll_obj.operation = CollisionObject.ADD
coll_obj.primitives.append(box)
coll_obj.primitive_poses.append(pose_box)
print "===== Coll Obj"
print coll_obj
scene_diff = PlanningScene(is_diff=True, allowed_collision_matrix = coll_mat)
scene_diff.world.collision_objects.append(coll_obj)
print "===== scene_diff"
print scene_diff
scene_pub.publish(scene_diff)
print "Publishing msg"
rospy.sleep(3)
request = PlanningSceneComponents(components=16)
response = get_planning_scene(request)
coll_mat = response.scene.allowed_collision_matrix
print "response.scene.world.collision_objects:", response.scene.world.collision_objects
response.scene.world.collision_objects is always an empty list.
↧