Revision history [back]

After the fruitful discussion with FSound I came to a partial solution to add padding to the peripheral elements of the robot cell.

The first assumption is that the whole model of the cell is available in the parameter server under /robot_description. This should anyhow be the case. Next, I prepared the padding values as described by FSound and loaded them into their appropriate namespace.

I then created a Python script that goes through the URDF and adds the objects to the Scene with the add_mesh() method.

Here are a few slices of the code.

First the imports that help you parse the URDF and the path to the mesh:

from urdf_parser_py.urdf import URDF
import rospkg
 

I then created a helper function that returns the full path of the mesh from the path available in the URDF:

get_full_mesh_path(mesh_path_from_urdf):
    relative_path = mesh_path_from_urdf.replace("package://","")    
    package_name = relative_path.split('/')[0]
    package_path = rospkg.RosPack().get_path(package_name)
    file_path = package_path + relative_path.replace(package_name, "")
 

Finally, I loop through the list of objects to add and add them to the scene:

urdf = URDF.from_parameter_server('/robot_description')
scene = moveit_commander.PlanningSceneInterface()
for object in objects_to_add:
    link = urdf.link_map[object]
    mesh_path = get_full_mesh_path(link.visual.geometry.filename)
    link_pose = PoseStamped()
    link_pose.pose.orientation.w = 1
    link_pose.header.frame_id = link.name
    scene.add_mesh(link.name, link_pose, mesh_path)
 

After the fruitful discussion with FSound I came to a partial solution to add padding to the peripheral elements of the robot cell.

The first assumption is that the whole model of the cell is available in the parameter server under /robot_description. This should anyhow be the case. Next, I prepared the padding values as described by FSound and loaded them into their appropriate namespace.

I then created a Python script that goes through the URDF and adds the objects to the Scene with the add_mesh() method.

Here are a few slices of the code.

First the imports that help you parse the URDF and the path to the mesh:

from urdf_parser_py.urdf import URDF
import rospkg
rospkg 

I then created a helper function that returns the full path of the mesh from the path available in the URDF:

get_full_mesh_path(mesh_path_from_urdf):
    relative_path = mesh_path_from_urdf.replace("package://","")    
    package_name = relative_path.split('/')[0]
    package_path = rospkg.RosPack().get_path(package_name)
    file_path = package_path + relative_path.replace(package_name, "")
"") 

Finally, I loop through the list of objects to add and add them to the scene:

 urdf = URDF.from_parameter_server('/robot_description')
scene = moveit_commander.PlanningSceneInterface()
for object in objects_to_add:
    link = urdf.link_map[object]
    mesh_path = get_full_mesh_path(link.visual.geometry.filename)
    link_pose = PoseStamped()
    link_pose.pose.orientation.w = 1
    link_pose.header.frame_id = link.name
    scene.add_mesh(link.name, link_pose, mesh_path)
mesh_path) 

After the fruitful discussion with FSound I came to a partial solution to add padding to the peripheral elements of the robot cell.

The first assumption is that the whole model of the cell is available in the parameter server under /robot_description. This should anyhow be the case. Next, I prepared the padding values as described by FSound and loaded them into their appropriate namespace.

I then created a Python script that goes through the URDF and adds the objects to the Scene with the add_mesh() method.

Here are a few slices of the code.

First the imports that help you parse the URDF and the path to the mesh:

 from urdf_parser_py.urdf import URDF
import rospkg 

I then created a helper function that returns the full path of the mesh from the path available in the URDF:

 get_full_mesh_path(mesh_path_from_urdf):
    relative_path = mesh_path_from_urdf.replace("package://","")    
    package_name = relative_path.split('/')[0]
    package_path = rospkg.RosPack().get_path(package_name)
    file_path = package_path + relative_path.replace(package_name, "") 

Finally, I loop through the list of objects to add and add them to the scene:

urdf = URDF.from_parameter_server('/robot_description')
scene = moveit_commander.PlanningSceneInterface()
for object in objects_to_add:
    link = urdf.link_map[object]
    mesh_path = get_full_mesh_path(link.visual.geometry.filename)
    link_pose = PoseStamped()
    link_pose.pose.orientation.w = 1
    link_pose.header.frame_id = link.name
    scene.add_mesh(link.name, link_pose, mesh_path) 

After the fruitful discussion with FSound FSound I came to a partial solution to add padding to the peripheral elements of the robot cell.

The first assumption is that the whole model of the cell is available in the parameter server under /robot_description. This should anyhow be the case. Next, I prepared the padding values as described by FSound and loaded them into their appropriate namespace.

I then created a Python script that goes through the URDF and adds the objects to the Scene with the add_mesh() method.

Here are a few slices of the code.

First the imports that help you parse the URDF and the path to the mesh:

from urdf_parser_py.urdf import URDF
import rospkg 

I then created a helper function that returns the full path of the mesh from the path available in the URDF:

get_full_mesh_path(mesh_path_from_urdf):
    relative_path = mesh_path_from_urdf.replace("package://","")    
    package_name = relative_path.split('/')[0]
    package_path = rospkg.RosPack().get_path(package_name)
    file_path = package_path + relative_path.replace(package_name, "") 

Finally, I loop through the list of objects to add and add them to the scene:

urdf = URDF.from_parameter_server('/robot_description')
scene = moveit_commander.PlanningSceneInterface()
for object in objects_to_add:
    link = urdf.link_map[object]
    mesh_path = get_full_mesh_path(link.visual.geometry.filename)
    link_pose = PoseStamped()
    link_pose.pose.orientation.w = 1
    link_pose.header.frame_id = link.name
    scene.add_mesh(link.name, link_pose, mesh_path)