├── .gitignore
├── src
    └── gazebo2rviz
    │   ├── __init__.py
    │   └── conversions.py
├── .gitremotes
├── setup.py
├── launch
    ├── gazebo2tf.launch
    ├── sdf2rviz.launch
    ├── gazebo2rviz.launch
    └── gazebo2marker.launch
├── package.xml
├── CMakeLists.txt
└── scripts
    ├── sdf2extract_tfstatic_node.py
    ├── sdf2marker_node.py
    ├── sdf2tfstatic_node.py
    ├── gazebo2marker_node.py
    ├── gazebo2tf_node.py
    └── sdf2moveit_collision.py


/.gitignore:
--------------------------------------------------------------------------------
1 | *.swp
2 | *.pyc
3 | 


--------------------------------------------------------------------------------
/src/gazebo2rviz/__init__.py:
--------------------------------------------------------------------------------
1 | from gazebo2rviz.conversions import link2marker_msg
2 | 


--------------------------------------------------------------------------------
/.gitremotes:
--------------------------------------------------------------------------------
1 | i60p414 git@i60p414.ira.uka.de:ahb/gazebo2rviz.git
2 | github git@github.com:andreasBihlmaier/gazebo2rviz.git
3 | ipr git@gitlab.ira.uka.de:ipr/gazebo2rviz.git
4 | 


--------------------------------------------------------------------------------
/setup.py:
--------------------------------------------------------------------------------
 1 | from distutils.core import setup
 2 | from catkin_pkg.python_setup import generate_distutils_setup
 3 | 
 4 | d = generate_distutils_setup(
 5 |   packages=['gazebo2rviz'],
 6 |   package_dir={'': 'src'}
 7 | )
 8 | 
 9 | setup(**d)
10 | 


--------------------------------------------------------------------------------
/launch/gazebo2tf.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <node name="static_tf_pub_world_to_gazebo_world" pkg="tf2_ros" type="static_transform_publisher" args="0 0 0  0 0 0 1 world gazebo_world" />
3 |   <node name="gazebo2tf" pkg="gazebo2rviz" type="gazebo2tf_node.py" output="screen">
4 |     <param name="ignore_submodels_of" value="" type="str" />
5 |   </node>
6 | </launch>
7 | 


--------------------------------------------------------------------------------
/launch/sdf2rviz.launch:
--------------------------------------------------------------------------------
1 | <launch>
2 |   <arg name="model" />
3 |   <arg name="collision" default="false"/>
4 | 
5 |   <node name="sdf2tfstatic" pkg="gazebo2rviz" type="sdf2tfstatic_node.py" args="$(arg model)" output="screen" />
6 |   <node name="sdf2marker" pkg="gazebo2rviz" type="sdf2marker_node.py" args="$(arg model)" output="screen" unless="$(arg collision)" />
7 |   <node name="sdf2marker" pkg="gazebo2rviz" type="sdf2marker_node.py" args="-c $(arg model)" output="screen" if="$(arg collision)" />
8 | </launch>
9 | 


--------------------------------------------------------------------------------
/launch/gazebo2rviz.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="marker_frequency" default="2" />
 3 |   <arg name="marker_collision" default="false" />
 4 |   <arg name="worldfile" default="" />
 5 | 
 6 |   <include file="$(find gazebo2rviz)/launch/gazebo2tf.launch" />
 7 |   <include file="$(find gazebo2rviz)/launch/gazebo2marker.launch">
 8 |     <arg name="frequency" value="$(arg marker_frequency)" />
 9 |     <arg name="collision" value="$(arg marker_collision)" />
10 |     <arg name="worldfile" value="$(arg worldfile)" />
11 |   </include>
12 | </launch>
13 | 


--------------------------------------------------------------------------------
/launch/gazebo2marker.launch:
--------------------------------------------------------------------------------
 1 | <launch>
 2 |   <arg name="frequency" default="2" />
 3 |   <arg name="collision" default="false" />
 4 |   <arg name="worldfile" default="" />
 5 | 
 6 |   <arg name="collision_arg" value="-c" if="$(arg collision)" />
 7 |   <arg name="collision_arg" value="" unless="$(arg collision)" />
 8 |   <arg name="worldfile_arg" value="$(eval '' if (worldfile == '') else ('-w ' + worldfile))" />
 9 | 
10 | 
11 |   <node name="gazebo2marker" pkg="gazebo2rviz" type="gazebo2marker_node.py" args="-f $(arg frequency) $(arg collision_arg) $(arg worldfile_arg)" output="screen">
12 |     <param name="ignore_submodels_of" value="" type="str" />
13 |   </node>
14 | </launch>
15 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>gazebo2rviz</name>
 4 |   <version>0.1.0</version>
 5 |   <description>This package contains nodes to automatically import all entities simulated in a ROS-enabled Gazebo instance into rviz through the TF and Marker plugin.</description>
 6 |   <maintainer email="andreas.bihlmaier@gmx.de">Andreas Bihlmaier</maintainer>
 7 |   <license>MIT</license>
 8 |   <url type="website">http://wiki.ros.org/gazebo2rviz</url>
 9 |   <author email="andreas.bihlmaier@gmx.de">Andreas Bihlmaier</author>
10 | 
11 |   <buildtool_depend>catkin</buildtool_depend>
12 |   <build_depend>rospy</build_depend>
13 |   <build_depend>pysdf</build_depend>
14 |   <run_depend>pysdf</run_depend>
15 |   <run_depend>rospy</run_depend>
16 |   <!-- <run_depend>python-rospkg</run_depend> -->
17 | 
18 |   <export>
19 |   </export>
20 | </package>
21 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 2.8.3)
 2 | project(gazebo2rviz)
 3 | 
 4 | find_package(catkin REQUIRED COMPONENTS
 5 |   rospy
 6 |   pysdf
 7 | )
 8 | 
 9 | catkin_python_setup()
10 | 
11 | catkin_package(
12 | #  INCLUDE_DIRS include
13 | #  LIBRARIES gazebo2rviz
14 |   CATKIN_DEPENDS rospy pysdf
15 |   #  DEPENDS python-rospkg
16 | )
17 | 
18 | 
19 | ###########
20 | ## Build ##
21 | ###########
22 | include_directories(
23 |   ${catkin_INCLUDE_DIRS}
24 | )
25 | 
26 | 
27 | #############
28 | ## Install ##
29 | #############
30 | install(PROGRAMS
31 |   scripts/gazebo2tf_node.py
32 |   scripts/gazebo2marker_node.py
33 |   scripts/sdf2marker_node.py
34 |   scripts/sdf2tfstatic_node.py
35 |   scripts/sdf2extract_tfstatic_node.py
36 |   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
37 | )
38 | 
39 | install(DIRECTORY launch/
40 | 	DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch
41 | )
42 | 


--------------------------------------------------------------------------------
/scripts/sdf2extract_tfstatic_node.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | """
 3 | Publish specific transform between two links within a SDF file as 'static' tf transformation (with different name)
 4 | """
 5 | 
 6 | import argparse
 7 | 
 8 | import rospy
 9 | import tf
10 | import tf_conversions.posemath as pm
11 | from tf.transformations import *
12 | 
13 | import pysdf
14 | 
15 | 
16 | 
17 | def main():
18 |   parser = argparse.ArgumentParser()
19 |   parser.add_argument('-f', '--freq', type=float, default=10, help='Frequency TFs are published (default: 10 Hz)')
20 |   parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
21 |   parser.add_argument('source_from_tf', help='Base Frame from SDF')
22 |   parser.add_argument('source_to_tf', help='Target Frame from SDF')
23 |   parser.add_argument('target_from_tf', nargs='?', help='Published Base Frame')
24 |   parser.add_argument('target_to_tf', nargs='?', help='Published Target Frame')
25 |   args = parser.parse_args(rospy.myargv()[1:])
26 |   source_from_tf, source_to_tf = args.source_from_tf, args.source_to_tf
27 |   target_from_tf = args.target_from_tf if args.target_from_tf else source_from_tf
28 |   target_to_tf = args.target_to_tf if args.target_to_tf else source_to_tf
29 | 
30 |   rospy.init_node('sdf2extract_tfstatic')
31 | 
32 |   sdf = pysdf.SDF(model=args.sdf)
33 |   world = sdf.world
34 |   from_link = world.get_link(source_from_tf)
35 |   if not from_link:
36 |     rospy.logerr('SDF %s does not contain source_from_tf %s' % (args.sdf, source_from_tf))
37 |     return 1
38 |   to_link = world.get_link(source_to_tf)
39 |   if not to_link:
40 |     rospy.logerr('SDF %s does not contain source_to_tf %s' % (args.sdf, source_to_tf))
41 |     return 2
42 | 
43 |   from_to_tf = concatenate_matrices(inverse_matrix(from_link.pose_world), to_link.pose_world)
44 |   translation, quaternion = pysdf.homogeneous2translation_quaternion(from_to_tf)
45 |   
46 |   tfBroadcaster = tf.TransformBroadcaster()
47 | 
48 |   rospy.loginfo('Publishing TF %s -> %s from SDF %s as TF %s -> %s: t=%s q=%s' % (source_from_tf, source_to_tf, args.sdf, target_from_tf, target_to_tf, translation, quaternion))
49 |   rospy.loginfo('Spinning')
50 |   r = rospy.Rate(args.freq)
51 |   while not rospy.is_shutdown():
52 |     tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), target_to_tf, target_from_tf)
53 |     r.sleep()
54 | 
55 | 
56 | if __name__ == '__main__':
57 |   main()
58 | 


--------------------------------------------------------------------------------
/scripts/sdf2marker_node.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | """
 3 | Publish all visuals within a SDF file as rviz Markers
 4 | """
 5 | 
 6 | import argparse
 7 | 
 8 | import rospy
 9 | from visualization_msgs.msg import Marker
10 | from tf.transformations import *
11 | 
12 | import pysdf
13 | from gazebo2rviz import *
14 | 
15 | 
16 | updatePeriod = 0.5
17 | use_collision = False
18 | submodelsToBeIgnored = []
19 | markerPub = None
20 | world = None
21 | markers = []
22 | 
23 | 
24 | 
25 | def prepare_link_marker(link, full_linkname):
26 |   marker_msg = link2marker_msg(link, full_linkname, use_collision, rospy.Duration(2 * updatePeriod))
27 |   if marker_msg:
28 |     markers.append(marker_msg)
29 | 
30 | 
31 | def prepare_markers(prefix):
32 |   world.for_all_links(prepare_link_marker)
33 |   for marker in markers:
34 |     marker.header.frame_id = prefix + pysdf.sdf2tfname(marker.header.frame_id)
35 |     marker.ns = prefix + pysdf.sdf2tfname(marker.ns)
36 | 
37 | 
38 | def publishMarkers():
39 |   for marker in markers:
40 |     #print(marker)
41 |     markerPub.publish(marker)
42 | 
43 | 
44 | 
45 | def main():
46 |   parser = argparse.ArgumentParser()
47 |   parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)')
48 |   parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
49 |   parser.add_argument('-c', '--collision', action='store_true', help='Publish collision instead of visual elements')
50 |   parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
51 |   args = parser.parse_args(rospy.myargv()[1:])
52 | 
53 |   rospy.init_node('sdf2marker')
54 | 
55 |   global submodelsToBeIgnored
56 |   submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of', '').split(';')
57 |   rospy.loginfo('Ignoring submodels of: ' + str(submodelsToBeIgnored))
58 | 
59 |   global updatePeriod
60 |   updatePeriod = 1. / args.freq
61 | 
62 |   global use_collision
63 |   use_collision = args.collision
64 | 
65 |   global markerPub
66 |   markerPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
67 | 
68 |   global world
69 |   sdf = pysdf.SDF(model=args.sdf)
70 |   world = sdf.world
71 | 
72 |   prepare_markers(args.prefix)
73 | 
74 |   rospy.loginfo('Spinning')
75 |   r = rospy.Rate(args.freq)
76 |   while not rospy.is_shutdown():
77 |     publishMarkers();
78 |     r.sleep()
79 | 
80 | if __name__ == '__main__':
81 |   main()
82 | 


--------------------------------------------------------------------------------
/scripts/sdf2tfstatic_node.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | """
 3 | Publish all links within a SDF file as 'static' tf transformation
 4 | """
 5 | 
 6 | import argparse
 7 | 
 8 | import rospy
 9 | import tf
10 | import tf_conversions.posemath as pm
11 | from tf.transformations import *
12 | 
13 | import pysdf
14 | 
15 | ignored_submodels = []
16 | tfBroadcaster = None
17 | world = None
18 | tfs = []
19 | 
20 | 
21 | 
22 | def is_ignored(model):
23 |   model_full_name = model.get_full_name()
24 |   for ignored_submodel in ignored_submodels:
25 |     if model_full_name == ignored_submodel or model_full_name.endswith('::' + ignored_submodel):
26 |       return True
27 |   return False
28 | 
29 | 
30 | def calculate_tfs(prefix):
31 |   world.for_all_joints(calculate_joint_tf)
32 |   for tf in tfs:
33 |     tf[0] = prefix + pysdf.sdf2tfname(tf[0])
34 |     tf[1] = prefix + pysdf.sdf2tfname(tf[1])
35 | 
36 | 
37 | def calculate_joint_tf(joint, full_jointname):
38 |   full_prefix = full_jointname.replace(joint.name, '')
39 |   if is_ignored(joint.parent_model):
40 |     print("Ignoring TF %s -> %s" % (full_prefix + joint.parent, full_prefix + joint.child))
41 |     return
42 |   rel_tf = concatenate_matrices(inverse_matrix(joint.tree_parent_link.pose_world), joint.tree_child_link.pose_world)
43 |   translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf)
44 |   tfs.append([full_prefix + joint.parent, full_prefix + joint.child, translation, quaternion])
45 | 
46 | 
47 | def publish_tf():
48 |   for tf in tfs:
49 |     #print(tf)
50 |     tfBroadcaster.sendTransform(tf[2], tf[3], rospy.get_rostime(), tf[1], tf[0])
51 | 
52 | 
53 | def main():
54 |   parser = argparse.ArgumentParser()
55 |   parser.add_argument('-f', '--freq', type=float, default=10, help='Frequency TFs are published (default: 10 Hz)')
56 |   parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
57 |   parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
58 |   args = parser.parse_args(rospy.myargv()[1:])
59 | 
60 |   rospy.init_node('sdf2tfstatic')
61 | 
62 |   global ignored_submodels
63 |   ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';')
64 |   rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels)
65 | 
66 |   global tfBroadcaster
67 |   tfBroadcaster = tf.TransformBroadcaster()
68 | 
69 |   global world
70 |   sdf = pysdf.SDF(model=args.sdf)
71 |   world = sdf.world
72 | 
73 |   calculate_tfs(args.prefix)
74 | 
75 |   rospy.loginfo('Spinning')
76 |   r = rospy.Rate(args.freq)
77 |   while not rospy.is_shutdown():
78 |     publish_tf();
79 |     r.sleep()
80 | 
81 | 
82 | if __name__ == '__main__':
83 |   main()
84 | 


--------------------------------------------------------------------------------
/scripts/gazebo2marker_node.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | 
  3 | import argparse
  4 | 
  5 | import rospy
  6 | import tf
  7 | from gazebo_msgs.msg import ModelStates
  8 | from visualization_msgs.msg import Marker
  9 | 
 10 | import pysdf
 11 | from gazebo2rviz import *
 12 | 
 13 | 
 14 | updatePeriod = 0.5
 15 | use_collision = False
 16 | submodelsToBeIgnored = []
 17 | markerPub = None
 18 | world = None
 19 | model_cache = {}
 20 | lastUpdateTime = None
 21 | worldsdf = None
 22 | 
 23 | 
 24 | 
 25 | def publish_link_marker(link, full_linkname, **kwargs):
 26 |   full_linkinstancename = full_linkname
 27 |   if 'model_name' in kwargs and 'instance_name' in kwargs:
 28 |     full_linkinstancename = full_linkinstancename.replace(kwargs['model_name'], kwargs['instance_name'], 1)
 29 |   marker_msgs = link2marker_msg(link, full_linkinstancename, use_collision, rospy.Duration(2 * updatePeriod))
 30 |   if len(marker_msgs) > 0:
 31 |     for marker_msg in marker_msgs:
 32 |       markerPub.publish(marker_msg)
 33 | 
 34 | 
 35 | def on_model_states_msg(model_states_msg):
 36 |   global lastUpdateTime
 37 |   sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
 38 |   if sinceLastUpdateDuration.to_sec() < updatePeriod:
 39 |     return
 40 |   lastUpdateTime = rospy.get_rostime()
 41 | 
 42 |   for (model_idx, modelinstance_name) in enumerate(model_states_msg.name):
 43 |     #print(model_idx, modelinstance_name)
 44 |     model_name = pysdf.name2modelname(modelinstance_name)
 45 |     #print('model_name:', model_name)
 46 |     if not model_name in model_cache:
 47 |       model_cache[model_name] = None
 48 |       if worldsdf:
 49 |         for model in worldsdf.world.models:
 50 |           if model.name == model_name:
 51 |             model_cache[model_name] = model
 52 |             break
 53 |       else:
 54 |         sdf = pysdf.SDF(model=model_name)
 55 |         if len(sdf.world.models) >= 1:
 56 |           model_cache[model_name] = sdf.world.models[0]
 57 |       if model_cache[model_name]:
 58 |         rospy.loginfo('Loaded model: %s' % model_cache[model_name].name)
 59 |       else:
 60 |         rospy.loginfo('Unable to load model: %s' % model_name)
 61 |     model = model_cache[model_name]
 62 |     if not model: # Not an SDF model
 63 |       continue
 64 |     #print('model:', model)
 65 |     model.for_all_links(publish_link_marker, model_name=model_name, instance_name=modelinstance_name)
 66 | 
 67 | 
 68 | def main():
 69 |   parser = argparse.ArgumentParser()
 70 |   parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)')
 71 |   parser.add_argument('-c', '--collision', action='store_true', help='Publish collision instead of visual elements')
 72 |   parser.add_argument('-w', '--worldfile', type=str, help='Read models from this world file')
 73 |   args = parser.parse_args(rospy.myargv()[1:])
 74 | 
 75 |   rospy.init_node('gazebo2marker')
 76 | 
 77 |   global submodelsToBeIgnored
 78 |   submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of', '').split(';')
 79 |   rospy.loginfo('Ignoring submodels of: ' + str(submodelsToBeIgnored))
 80 |   if submodelsToBeIgnored:
 81 |     rospy.logerr('ignore_submodels_of is currently not supported and will thus have no effect')
 82 | 
 83 | 
 84 |   global updatePeriod
 85 |   updatePeriod = 1. / args.freq
 86 | 
 87 |   global use_collision
 88 |   use_collision = args.collision
 89 | 
 90 |   if args.worldfile:
 91 |     global worldsdf
 92 |     worldsdf = pysdf.SDF(file=args.worldfile)
 93 | 
 94 |   global markerPub
 95 |   markerPub = rospy.Publisher('/visualization_marker', Marker, queue_size=10)
 96 |   rospy.sleep(rospy.Duration(0, 100 * 1000))
 97 | 
 98 |   global lastUpdateTime
 99 |   lastUpdateTime = rospy.get_rostime()
100 |   modelStatesSub = rospy.Subscriber('gazebo/model_states', ModelStates, on_model_states_msg)
101 | 
102 |   rospy.loginfo('Spinning')
103 |   rospy.spin()
104 | 
105 | if __name__ == '__main__':
106 |   main()
107 | 


--------------------------------------------------------------------------------
/scripts/gazebo2tf_node.py:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env python3
 2 | 
 3 | import rospy
 4 | import tf
 5 | from geometry_msgs.msg import Pose
 6 | from gazebo_msgs.msg import LinkStates
 7 | import tf_conversions.posemath as pm
 8 | from tf.transformations import *
 9 | 
10 | import pysdf
11 | 
12 | tfBroadcaster = None
13 | submodelsToBeIgnored = []
14 | lastUpdateTime = None
15 | updatePeriod = 0.05
16 | model_cache = {}
17 | 
18 | 
19 | def is_ignored(link_name):
20 |   for ignored_submodel in submodelsToBeIgnored:
21 |     if link_name.startswith(ignored_submodel + '::'):
22 |       return True
23 |   return False
24 | 
25 | 
26 | def on_link_states_msg(link_states_msg):
27 |   """
28 |   Publish tf for each model in current Gazebo world
29 |   """
30 |   global lastUpdateTime
31 |   sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime
32 |   if sinceLastUpdateDuration.to_sec() < updatePeriod:
33 |     return
34 |   lastUpdateTime = rospy.get_rostime()
35 | 
36 |   poses = {'gazebo_world': identity_matrix()}
37 |   for (link_idx, link_name) in enumerate(link_states_msg.name):
38 |     poses[link_name] = pysdf.pose_msg2homogeneous(link_states_msg.pose[link_idx])
39 |     #print('%s:\n%s' % (link_name, poses[link_name]))
40 | 
41 |   for (link_idx, link_name) in enumerate(link_states_msg.name):
42 |     #print(link_idx, link_name)
43 |     modelinstance_name = link_name.split('::')[0]
44 |     #print('modelinstance_name:', modelinstance_name)
45 |     model_name = pysdf.name2modelname(modelinstance_name)
46 |     #print('model_name:', model_name)
47 |     if not model_name in model_cache:
48 |       sdf = pysdf.SDF(model=model_name)
49 |       model_cache[model_name] = sdf.world.models[0] if len(sdf.world.models) >= 1 else None
50 |       if model_cache[model_name]:
51 |         rospy.loginfo('Loaded model: %s' % model_cache[model_name].name)
52 |       else:
53 |         rospy.loginfo('Unable to load model: %s' % model_name)
54 |     model = model_cache[model_name]
55 |     link_name_in_model = link_name.replace(modelinstance_name + '::', '')
56 |     if model:
57 |       link = model.get_link(link_name_in_model)
58 |       if link.tree_parent_joint:
59 |         parent_link = link.tree_parent_joint.tree_parent_link
60 |         parent_link_name = parent_link.get_full_name()
61 |         #print('parent:', parent_link_name)
62 |         parentinstance_link_name = parent_link_name.replace(model_name, modelinstance_name, 1)
63 |       else: # direct child of world
64 |         parentinstance_link_name = 'gazebo_world'
65 |     else: # Not an SDF model
66 |         parentinstance_link_name = 'gazebo_world'
67 |     #print('parentinstance:', parentinstance_link_name)
68 |     if is_ignored(parentinstance_link_name):
69 |       rospy.loginfo("Ignoring TF %s -> %s" % (parentinstance_link_name, link_name))
70 |       continue
71 |     pose = poses[link_name]
72 |     parent_pose = poses[parentinstance_link_name]
73 |     rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose)
74 |     translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf)
75 |     #print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion))
76 |     tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
77 | 
78 | 
79 | 
80 | def main():
81 |   rospy.init_node('gazebo2tf')
82 | 
83 |   global submodelsToBeIgnored
84 |   submodelsToBeIgnored = rospy.get_param('~ignore_submodels_of', '').split(';')
85 |   rospy.loginfo('Ignoring submodels of: ' + str(submodelsToBeIgnored))
86 | 
87 |   global tfBroadcaster
88 |   tfBroadcaster = tf.TransformBroadcaster()
89 | 
90 |   global lastUpdateTime
91 |   lastUpdateTime = rospy.get_rostime()
92 |   linkStatesSub = rospy.Subscriber('gazebo/link_states', LinkStates, on_link_states_msg)
93 | 
94 |   rospy.loginfo('Spinning')
95 |   rospy.spin()
96 | 
97 | if __name__ == '__main__':
98 |   main()
99 | 


--------------------------------------------------------------------------------
/src/gazebo2rviz/conversions.py:
--------------------------------------------------------------------------------
 1 | import copy
 2 | 
 3 | import rospy
 4 | from rospkg import RosPack, ResourceNotFound
 5 | from visualization_msgs.msg import Marker
 6 | 
 7 | import pysdf
 8 | import os.path
 9 | 
10 | protoMarkerMsg = Marker()
11 | protoMarkerMsg.frame_locked = True
12 | protoMarkerMsg.id = 0
13 | protoMarkerMsg.action = Marker.ADD
14 | protoMarkerMsg.mesh_use_embedded_materials = True
15 | protoMarkerMsg.color.a = 0.0
16 | protoMarkerMsg.color.r = 0.0
17 | protoMarkerMsg.color.g = 0.0
18 | protoMarkerMsg.color.b = 0.0
19 | supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
20 | 
21 | gazebo_rospack = RosPack()
22 | 
23 | 
24 | def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)):
25 |   marker_msg = None
26 |   linkpart = None
27 |   if use_collision:
28 |     linkparts = getattr(link, 'collisions')
29 |   else: # visual
30 |     linkparts = getattr(link, 'visuals')
31 | 
32 |   msgs = []
33 | 
34 |   for linkpart in linkparts:
35 |     if not linkpart.geometry_type in supported_geometry_types:
36 |       if linkpart.geometry_type:
37 |         print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type))
38 |         return None
39 | 
40 |     marker_msg = copy.deepcopy(protoMarkerMsg)
41 |     marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname)
42 |     marker_msg.header.stamp = rospy.get_rostime()
43 |     marker_msg.lifetime = lifetime
44 |     marker_msg.ns = pysdf.sdf2tfname(full_linkname + "::" + linkpart.name)
45 |     marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose)
46 | 
47 |     if linkpart.geometry_type == 'mesh':
48 |       marker_msg.type = Marker.MESH_RESOURCE
49 |       #print('linkpart.geometry_data: %s' % linkpart.geometry_data['uri'])
50 |       for models_path in pysdf.models_paths:
51 |         resource = linkpart.geometry_data['uri'].replace('model://', models_path + '/')
52 |         #print('resource: %s' % resource)
53 |         if os.path.isfile(resource):
54 |           marker_msg.mesh_resource = 'file://' + resource
55 |           #print('found resource %s at %s' % (linkpart.geometry_data['uri'], resource))
56 |           break
57 |       # support URDF-like resource paths starting with model://
58 |       if not marker_msg.mesh_resource and linkpart.geometry_data['uri'].startswith('model://'):
59 |         stripped_uri = linkpart.geometry_data['uri'].replace('model://', '')
60 |         uri_parts = stripped_uri.split('/', 1)
61 | 
62 |         if len(uri_parts) == 2:
63 |           package_name = uri_parts[0]
64 |           try:
65 |             package_path = gazebo_rospack.get_path(package_name)
66 |             mesh_path = os.path.join(package_path, uri_parts[1])
67 |             if os.path.isfile(mesh_path):
68 |               marker_msg.mesh_resource = 'file://' + mesh_path
69 |           except ResourceNotFound as e:
70 |             pass
71 | 
72 |       if not marker_msg.mesh_resource:
73 |         print('ERROR! could not find resource: %s' % linkpart.geometry_data['uri'])
74 |         return None
75 | 
76 |       scale = (float(val) for val in linkpart.geometry_data['scale'].split())
77 |       marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
78 |     else:
79 |       marker_msg.color.a = 1
80 |       marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5
81 | 
82 |     if linkpart.geometry_type == 'box':
83 |       marker_msg.type = Marker.CUBE
84 |       scale = (float(val) for val in linkpart.geometry_data['size'].split())
85 |       marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale
86 |     elif linkpart.geometry_type == 'sphere':
87 |       marker_msg.type = Marker.SPHERE
88 |       marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius'])
89 |     elif linkpart.geometry_type == 'cylinder':
90 |       marker_msg.type = Marker.CYLINDER
91 |       marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius'])
92 |       marker_msg.scale.z = float(linkpart.geometry_data['length'])
93 | 
94 |     #print(marker_msg)
95 |     msgs.append(marker_msg)
96 |   return msgs
97 | 


--------------------------------------------------------------------------------
/scripts/sdf2moveit_collision.py:
--------------------------------------------------------------------------------
  1 | #!/usr/bin/env python3
  2 | """
  3 | Publish all collision elements within a SDF file as MoveIt CollisionObjects
  4 | """
  5 | 
  6 | import argparse
  7 | import os
  8 | try:
  9 |   from pyassimp import pyassimp
 10 | except:
 11 |   # support pyassimp > 3.0
 12 |   import pyassimp
 13 | 
 14 | import rospy
 15 | #from visualization_msgs.msg import Marker
 16 | from geometry_msgs.msg import Pose, PoseStamped, Point
 17 | from moveit_msgs.msg import CollisionObject, PlanningScene
 18 | from shape_msgs.msg import SolidPrimitive, Plane, Mesh, MeshTriangle
 19 | from tf.transformations import *
 20 | 
 21 | import pysdf
 22 | from gazebo2rviz import *
 23 | 
 24 | 
 25 | ignored_submodels = []
 26 | collision_objects = {}
 27 | 
 28 | 
 29 | 
 30 | # Slightly modified PlanningSceneInterface.__make_mesh from moveit_commander/src/moveit_commander/planning_scene_interface.py
 31 | def make_mesh(co, name, pose, filename, scale = (1, 1, 1)):
 32 |     #print("make_mesh(name=%s filename=%s)" % (name, filename))
 33 |     scene = pyassimp.load(filename)
 34 |     if not scene.meshes or len(scene.meshes) == 0:
 35 |         raise MoveItCommanderException("There are no meshes in the file")
 36 |     if len(scene.meshes[0].faces) == 0:
 37 |         raise MoveItCommanderException("There are no faces in the mesh")
 38 |     co.operation = CollisionObject.ADD
 39 |     co.id = name
 40 |     co.header = pose.header
 41 | 
 42 |     mesh = Mesh()
 43 |     first_face = scene.meshes[0].faces[0]
 44 |     if hasattr(first_face, '__len__'):
 45 |         for face in scene.meshes[0].faces:
 46 |             if len(face) == 3:
 47 |                 triangle = MeshTriangle()
 48 |                 triangle.vertex_indices = [face[0], face[1], face[2]]
 49 |                 mesh.triangles.append(triangle)
 50 |     elif hasattr(first_face, 'indices'):
 51 |         for face in scene.meshes[0].faces:
 52 |             if len(face.indices) == 3:
 53 |                 triangle = MeshTriangle()
 54 |                 triangle.vertex_indices = [face.indices[0],
 55 |                                            face.indices[1],
 56 |                                            face.indices[2]]
 57 |                 mesh.triangles.append(triangle)
 58 |     else:
 59 |         raise MoveItCommanderException("Unable to build triangles from mesh due to mesh object structure")
 60 |     for vertex in scene.meshes[0].vertices:
 61 |         point = Point()
 62 |         point.x = vertex[0]*scale[0]
 63 |         point.y = vertex[1]*scale[1]
 64 |         point.z = vertex[2]*scale[2]
 65 |         mesh.vertices.append(point)
 66 |     if not isinstance(co.meshes, list):
 67 |         co.meshes = []
 68 |     co.meshes += [mesh]
 69 | 
 70 |     if not isinstance(co.mesh_poses, list):
 71 |         co.mesh_poses = []
 72 |     co.mesh_poses += [pose.pose]
 73 | 
 74 |     pyassimp.release(scene)
 75 |     return co
 76 | 
 77 | 
 78 | def append_to_collision_object(sink_collision_object, source_collision_object):
 79 |   sink_collision_object.primitives.extend(source_collision_object.primitives)
 80 |   sink_collision_object.primitive_poses.extend(source_collision_object.primitive_poses)
 81 |   sink_collision_object.meshes.extend(source_collision_object.meshes)
 82 |   sink_collision_object.mesh_poses.extend(source_collision_object.mesh_poses)
 83 |   sink_collision_object.planes.extend(source_collision_object.planes)
 84 |   sink_collision_object.plane_poses.extend(source_collision_object.plane_poses)
 85 | 
 86 | 
 87 | def is_ignored(model):
 88 |   model_full_name = model.get_full_name()
 89 |   for ignored_submodel in ignored_submodels:
 90 |     if model_full_name == ignored_submodel or model_full_name.endswith('::' + ignored_submodel):
 91 |       return True
 92 |   return False
 93 | 
 94 | 
 95 | def get_root_collision_model(link):
 96 |   # Travere model tree upward until either at the root or an ignored submodel is found which is origin of collision object.
 97 |   model = link.parent_model
 98 |   while True:
 99 |     if not model.parent_model:
100 |       break
101 |     elif is_ignored(model):
102 |       break
103 |     model = model.parent_model
104 |   return model
105 | 
106 | 
107 | def link_to_collision_object(link, full_linkname):
108 |   supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']
109 |   linkparts = getattr(link, 'collisions')
110 | 
111 |   if is_ignored(link.parent_model):
112 |     print("Ignoring link %s." % full_linkname)
113 |     return
114 | 
115 |   collision_object = CollisionObject()
116 |   collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname)
117 |   root_collision_model = get_root_collision_model(link)
118 |   link_pose_in_root_frame = pysdf.homogeneous2pose_msg(concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world)))
119 | 
120 |   for linkpart in linkparts:
121 |       if linkpart.geometry_type not in supported_geometry_types:
122 |           print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type))
123 |           continue
124 | 
125 |       if linkpart.geometry_type == 'mesh':
126 |         scale = tuple(float(val) for val in linkpart.geometry_data['scale'].split())
127 |         for models_path in pysdf.models_paths:
128 |           test_mesh_path = linkpart.geometry_data['uri'].replace('model://', models_path)
129 |           if os.path.isfile(test_mesh_path):
130 |             mesh_path = test_mesh_path
131 |             break
132 |         if mesh_path:
133 |           link_pose_stamped = PoseStamped()
134 |           link_pose_stamped.pose = link_pose_in_root_frame
135 |           make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale)
136 |         else:
137 |           print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname))
138 |       elif linkpart.geometry_type == 'box':
139 |         scale = tuple(float(val) for val in linkpart.geometry_data['size'].split())
140 |         box = SolidPrimitive()
141 |         box.type = SolidPrimitive.BOX
142 |         box.dimensions = scale
143 |         collision_object.primitives.append(box)
144 |         collision_object.primitive_poses.append(link_pose_in_root_frame)
145 |       elif linkpart.geometry_type == 'sphere':
146 |         sphere = SolidPrimitive()
147 |         sphere.type = SolidPrimitive.SPHERE
148 |         sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius'])
149 |         collision_object.primitives.append(sphere)
150 |         collision_object.primitive_poses.append(link_pose_in_root_frame)
151 |       elif linkpart.geometry_type == 'cylinder':
152 |         cylinder = SolidPrimitive()
153 |         cylinder.type = SolidPrimitive.CYLINDER
154 |         cylinder.dimensions = tuple((2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length'])))
155 |         collision_object.primitives.append(cylinder)
156 |         collision_object.primitive_poses.append(link_pose_in_root_frame)
157 | 
158 |   #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object))
159 |   return collision_object
160 | 
161 | 
162 | def convert_to_collision_object(link, full_linkname):
163 |   collision_object = link_to_collision_object(link, full_linkname)
164 |   if not collision_object:
165 |     return
166 | 
167 |   link_root = get_root_collision_model(link).root_link.name
168 |   if link_root not in collision_objects:
169 |     collision_objects[link_root] = CollisionObject()
170 |     collision_objects[link_root].id = link_root
171 |     collision_objects[link_root].operation = CollisionObject.ADD
172 |   append_to_collision_object(collision_objects[link_root], collision_object)
173 | 
174 | 
175 | 
176 | def main():
177 |   parser = argparse.ArgumentParser()
178 |   #parser.add_argument('-f', '--freq', type=float, default=2, help='Frequency Markers are published (default: 2 Hz)')
179 |   #parser.add_argument('-p', '--prefix', default='', help='Publish with prefix')
180 |   parser.add_argument('sdf', help='SDF model to publish (e.g. coke_can)')
181 |   args = parser.parse_args(rospy.myargv()[1:])
182 | 
183 |   rospy.init_node('sdf2moveit_collision')
184 | 
185 |   global ignored_submodels
186 |   ignored_submodels = rospy.get_param('~ignore_submodels', '').split(';')
187 |   rospy.loginfo('Ignoring submodels of: %s' % ignored_submodels)
188 | 
189 |   planning_scene_pub = rospy.Publisher('/planning_scene', PlanningScene, queue_size=10)
190 | 
191 |   sdf = pysdf.SDF(model=args.sdf)
192 |   world = sdf.world
193 | 
194 |   world.for_all_links(convert_to_collision_object)
195 |   planning_scene_msg = PlanningScene()
196 |   planning_scene_msg.is_diff = True
197 |   for (collision_object_root, collision_object) in collision_objects.iteritems():
198 |     if collision_object_root in ignored_submodels:
199 |       print('TODO2')  # attached object instead of collision object
200 |     else:
201 |       planning_scene_msg.world.collision_objects.append(collision_object)
202 |       planning_scene_msg.world.collision_objects[-1].header.frame_id = 'world'
203 | 
204 |   while planning_scene_pub.get_num_connections() < 1:
205 |     rospy.sleep(0.1)
206 |   planning_scene_pub.publish(planning_scene_msg)
207 | 
208 | 
209 |   #rospy.loginfo('Spinning')
210 |   #r = rospy.Rate(args.freq)
211 |   #while not rospy.is_shutdown():
212 |   #  publishMarkers();
213 |   #  r.sleep()
214 | 
215 | 
216 | if __name__ == '__main__':
217 |   main()
218 | 


--------------------------------------------------------------------------------