How to add a Mesh to PlanningScene in Python

edit

Hi,

I'm having trouble adding a Mesh I've created in Blender (.stl file) to my Planning Scene, which is visualized in Rviz. It's supposed to be the floor the robot is standing on. I'm using Python.

Here is the relevant part in my code:

def addPlanningScene():
    # Use the planning scene object to add or remove objects //Interface
        scene = moveit_commander.PlanningSceneInterface()

    REFERENCE_FRAME = '/world'
    p = PlanningScene()
    p.is_diff = True    

    # Create a scene publisher to push changes to the scene //PlanningScene
        scene_pub = rospy.Publisher('/move_group/monitored_planning_scene', PlanningScene)

    # Give each of the scene objects a unique name        
        Ground_id = 'ground'
        Object1_id = 'box1'
        Object2_id = 'box2'

    # Remove leftover objects from a previous run
        scene.remove_world_object(Ground_id)
        scene.remove_world_object(Object1_id)
        scene.remove_world_object(Object2_id)
        scene.remove_world_object(target_id)

    pose_Ground = geometry_msgs.msg.PoseStamped()
    pose_Ground.header.frame_id = REFERENCE_FRAME
    pose_Ground.pose.position.x = 0.0
    pose_Ground.pose.position.y = 0.0
    pose_Ground.pose.position.z = -0.05


    scene.add_mesh(Ground_id,pose_Ground,'./Scene_Mesh/Ground_Plane.stl')

    scene_pub.publish(PlanningScene)

I'm trying to use the the add_mesh function from the planning_scene_interface.py It should be an easy problem to solve but I'm stuck right now.

Appreciate any Help! Thanks.