Ask Your Question
0

How to add a Mesh to PlanningScene in Python

asked 2015-03-20 13:03:03 -0500

TedK gravatar image

updated 2019-01-02 16:46:01 -0500

jayess gravatar image

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.

edit retag flag offensive close merge delete

Comments

p.is_diff = True

Aren't you also setting the is_diff field?

gvdhoorn gravatar imagegvdhoorn ( 2015-03-21 03:35:00 -0500 )edit

I tried it with and without. Should probably edit my OP.

TedK gravatar imageTedK ( 2015-03-21 13:43:34 -0500 )edit

2 Answers

Sort by ยป oldest newest most voted
0

answered 2019-01-02 10:27:30 -0500

Apart from @gvdhoorn's answer, a couple of things to take care of are:

  • The pose header's frame_id must be a valid TF frame. I've been using robot = moveit_commander.RobotCommander(); pose_Ground.header.frame_id = robot.get_planning_frame().
  • In RViz, add the PlanningScene panel and enable it.
edit flag offensive delete link more
0

answered 2015-03-24 02:28:26 -0500

gvdhoorn gravatar image

Could you describe what the problem is that you are having?


One thing to check is the path to your mesh: cwd for a node is not where there source file is located, but by default is ~/.ros. So relative paths tend to not work so well.

Just to test, I'd replace it with an absolute path.

For a proper solution, you could use rospkg or catkin_pkg to first get the path to the pkg where the resource is located, then concatenate that with the relative path you have now.

Alternatively, you could set the cwd of your node to the pkg dir that contains the resource. See roslaunch/XML/node - Attributes for that.

edit flag offensive delete link more

Comments

The Object I'm trying to add to the scene doesn't appear in Rviz. I've tried absolute and relative paths, but that doesn't solve the problem. Also the add_mesh method checks, whether there actually is a mesh at the declared position.

TedK gravatar imageTedK ( 2015-03-24 16:00:40 -0500 )edit

also add_mesh publishes on the topic '/collision_object' but in Rviz that appears as an unvisualizable topic.

TedK gravatar imageTedK ( 2015-03-24 16:01:55 -0500 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

Stats

Asked: 2015-03-20 13:03:03 -0500

Seen: 2,278 times

Last updated: Jan 02