def on_link_states_msg(link_states_msg): """ Publish tf for each model in current Gazebo world """ global lastUpdateTime sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime if sinceLastUpdateDuration.to_sec() < updatePeriod: return lastUpdateTime = rospy.get_rostime() poses = {'gazebo_world': identity_matrix()} for (link_idx, link_name) in enumerate(link_states_msg.name): poses[link_name] = pysdf.pose_msg2homogeneous( link_states_msg.pose[link_idx]) #print('%s:\n%s' % (link_name, poses[link_name])) for (link_idx, link_name) in enumerate(link_states_msg.name): #print(link_idx, link_name) modelinstance_name = link_name.split('::')[0] #print('modelinstance_name:', modelinstance_name) model_name = pysdf.name2modelname(modelinstance_name) #print('model_name:', model_name) if not model_name in model_cache: sdf = pysdf.SDF(model=model_name) model_cache[model_name] = sdf.world.models[0] if len( sdf.world.models) >= 1 else None if model_cache[model_name]: rospy.loginfo('Loaded model: %s' % model_cache[model_name].name) else: rospy.loginfo('Unable to load model: %s' % model_name) model = model_cache[model_name] link_name_in_model = link_name.replace(modelinstance_name + '::', '') if model: link = model.get_link(link_name_in_model) if link.tree_parent_joint: parent_link = link.tree_parent_joint.tree_parent_link parent_link_name = parent_link.get_full_name() #print('parent:', parent_link_name) parentinstance_link_name = parent_link_name.replace( model_name, modelinstance_name, 1) else: # direct child of world parentinstance_link_name = 'gazebo_world' else: # Not an SDF model parentinstance_link_name = 'gazebo_world' #print('parentinstance:', parentinstance_link_name) if is_ignored(parentinstance_link_name): rospy.loginfo("Ignoring TF %s -> %s" % (parentinstance_link_name, link_name)) continue pose = poses[link_name] parent_pose = poses[parentinstance_link_name] rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose) translation, quaternion = pysdf.homogeneous2translation_quaternion( rel_tf) #print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion)) tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
def publish_tf(self): model_cache = {} poses = {'gazebo_world': identity_matrix()} for (link_idx, link_name) in enumerate(self.link_states_msg.name): poses[link_name] = pysdf.pose_msg2homogeneous( self.link_states_msg.pose[link_idx]) # print('%s:\n%s' % (link_name, poses[link_name])) for (link_idx, link_name) in enumerate(self.link_states_msg.name): # print(link_idx, link_name) modelinstance_name = link_name.split('::')[0] # print('modelinstance_name:', modelinstance_name) model_name = pysdf.name2modelname(modelinstance_name) # print('model_name:', model_name) if not model_name in model_cache: sdf = pysdf.SDF(model=model_name) model_cache[model_name] = sdf.world.models[0] if len( sdf.world.models) >= 1 else None if not model_cache[model_name]: print('Unable to load model: %s' % model_name) model = model_cache[model_name] link_name_in_model = link_name.replace(modelinstance_name + '::', '') if model: link = model.get_link(link_name_in_model) if link.tree_parent_joint: parent_link = link.tree_parent_joint.tree_parent_link parent_link_name = parent_link.get_full_name() # print('parent:', parent_link_name) parentinstance_link_name = parent_link_name.replace( model_name, modelinstance_name, 1) else: # direct child of world parentinstance_link_name = 'gazebo_world' else: # Not an SDF model parentinstance_link_name = 'gazebo_world' # print('parentinstance:', parentinstance_link_name) pose = poses[link_name] #parent_pose = pysdf.pose_msg2homogeneous(self.model_states_msg.pose[1]) parent_pose = poses[parentinstance_link_name] rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose) translation, quaternion = pysdf.homogeneous2translation_quaternion( rel_tf) # print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion)) self.tfBroadcaster.sendTransform( translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name)) if self.link_odometry == pysdf.sdf2tfname(link_name): self.publish_odometry( translation, quaternion, pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
def link2marker_msg(link, full_linkname, use_collision=False, lifetime=rospy.Duration(0)): marker_msg = None linkpart = None if use_collision: linkpart = getattr(link, 'collision') else: # visual linkpart = getattr(link, 'visual') if not linkpart.geometry_type in supported_geometry_types: return marker_msg = copy.deepcopy(protoMarkerMsg) marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname) marker_msg.header.stamp = rospy.get_rostime() marker_msg.lifetime = lifetime marker_msg.ns = pysdf.sdf2tfname(full_linkname) marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose) if linkpart.geometry_type == 'mesh': marker_msg.type = Marker.MESH_RESOURCE marker_msg.mesh_resource = linkpart.geometry_data['uri'].replace( 'model://', 'file://' + pysdf.models_path) scale = (float(val) for val in linkpart.geometry_data['scale'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale else: marker_msg.color.a = 1 marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5 if linkpart.geometry_type == 'box': marker_msg.type = Marker.CUBE scale = (float(val) for val in linkpart.geometry_data['size'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale elif linkpart.geometry_type == 'sphere': marker_msg.type = Marker.SPHERE marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float( linkpart.geometry_data['radius']) elif linkpart.geometry_type == 'cylinder': marker_msg.type = Marker.CYLINDER marker_msg.scale.x = marker_msg.scale.y = 2.0 * float( linkpart.geometry_data['radius']) marker_msg.scale.z = float(linkpart.geometry_data['length']) #print(marker_msg) return marker_msg
def link_to_collision_object(link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) root_collision_model = get_root_collision_model(link) link_pose_in_root_frame = pysdf.homogeneous2pose_msg( concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world))) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple( float(val) for val in linkpart.geometry_data['scale'].split()) mesh_path = linkpart.geometry_data['uri'].replace( 'model://', pysdf.models_path) link_pose_stamped = PoseStamped() link_pose_stamped.pose = link_pose_in_root_frame make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale) elif linkpart.geometry_type == 'box': scale = tuple( float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius']) collision_object.primitives.append(sphere) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple( (2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append(link_pose_in_root_frame) #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object)) return collision_object
def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)): marker_msg = None linkpart = None if use_collision: linkpart = getattr(link, 'collision') else: # visual linkpart = getattr(link, 'visual') if not linkpart.geometry_type in supported_geometry_types: return marker_msg = copy.deepcopy(protoMarkerMsg) marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname) marker_msg.header.stamp = rospy.get_rostime() marker_msg.lifetime = lifetime marker_msg.ns = pysdf.sdf2tfname(full_linkname) marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose) if linkpart.geometry_type == 'mesh': marker_msg.type = Marker.MESH_RESOURCE marker_msg.mesh_resource = linkpart.geometry_data['uri'].replace('model://', 'file://' + pysdf.models_path) scale = (float(val) for val in linkpart.geometry_data['scale'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale else: marker_msg.color.a = 1 marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5 if linkpart.geometry_type == 'box': marker_msg.type = Marker.CUBE scale = (float(val) for val in linkpart.geometry_data['size'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale elif linkpart.geometry_type == 'sphere': marker_msg.type = Marker.SPHERE marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius']) elif linkpart.geometry_type == 'cylinder': marker_msg.type = Marker.CYLINDER marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius']) marker_msg.scale.z = float(linkpart.geometry_data['length']) #print(marker_msg) return marker_msg
def link_to_collision_object(link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) root_collision_model = get_root_collision_model(link) link_pose_in_root_frame = pysdf.homogeneous2pose_msg(concatenate_matrices(link.pose_world, inverse_matrix(root_collision_model.pose_world))) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple(float(val) for val in linkpart.geometry_data['scale'].split()) for models_path in pysdf.models_paths: test_mesh_path = linkpart.geometry_data['uri'].replace('model://', models_path) if os.path.isfile(test_mesh_path): mesh_path = test_mesh_path break if mesh_path: link_pose_stamped = PoseStamped() link_pose_stamped.pose = link_pose_in_root_frame make_mesh(collision_object, full_linkname, link_pose_stamped, mesh_path, scale) else: print("ERROR: No mesh found for '%s' in element '%s'." % (linkpart.geometry_data['uri'], full_linkname)) elif linkpart.geometry_type == 'box': scale = tuple(float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = 2.0 * float(linkpart.geometry_data['radius']) collision_object.primitives.append(sphere) collision_object.primitive_poses.append(link_pose_in_root_frame) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple((2.0 * float(linkpart.geometry_data['radius']), float(linkpart.geometry_data['length']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append(link_pose_in_root_frame) #print('CollisionObject for %s:\n%s' % (full_linkname, collision_object)) return collision_object
def on_link_states_msg(link_states_msg): """ Publish tf for each model in current Gazebo world """ global lastUpdateTime sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime if sinceLastUpdateDuration.to_sec() < updatePeriod: return lastUpdateTime = rospy.get_rostime() poses = {'gazebo_world': identity_matrix()} for (link_idx, link_name) in enumerate(link_states_msg.name): poses[link_name] = pysdf.pose_msg2homogeneous(link_states_msg.pose[link_idx]) #print('%s:\n%s' % (link_name, poses[link_name])) for (link_idx, link_name) in enumerate(link_states_msg.name): #print(link_idx, link_name) modelinstance_name = link_name.split('::')[0] #print('modelinstance_name:', modelinstance_name) model_name = pysdf.name2modelname(modelinstance_name) #print('model_name:', model_name) if not model_name in model_cache: sdf = pysdf.SDF(model=model_name) model_cache[model_name] = sdf.world.models[0] if len(sdf.world.models) >= 1 else None if model_cache[model_name]: rospy.loginfo('Loaded model: %s' % model_cache[model_name].name) else: rospy.loginfo('Unable to load model: %s' % model_name) model = model_cache[model_name] link_name_in_model = link_name.replace(modelinstance_name + '::', '') if model: link = model.get_link(link_name_in_model) if link.tree_parent_joint: parent_link = link.tree_parent_joint.tree_parent_link parent_link_name = parent_link.get_full_name() #print('parent:', parent_link_name) parentinstance_link_name = parent_link_name.replace(model_name, modelinstance_name, 1) else: # direct child of world parentinstance_link_name = 'gazebo_world' else: # Not an SDF model parentinstance_link_name = 'gazebo_world' #print('parentinstance:', parentinstance_link_name) if is_ignored(parentinstance_link_name): rospy.loginfo("Ignoring TF %s -> %s" % (parentinstance_link_name, link_name)) continue pose = poses[link_name] parent_pose = poses[parentinstance_link_name] rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose) translation, quaternion = pysdf.homogeneous2translation_quaternion(rel_tf) #print('Publishing TF %s -> %s: t=%s q=%s' % (pysdf.sdf2tfname(parentinstance_link_name), pysdf.sdf2tfname(link_name), translation, quaternion)) tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
def update_collision_object(self, link, full_linkname, **kwargs): if 'name' in kwargs: modelinstance_name = kwargs['name'] full_linkname_mod = modelinstance_name + "::" + full_linkname.split( "::")[1] link_root = pysdf.sdf2tfname(full_linkname_mod) self.collision_objects_updated[link_root] = CollisionObject() self.collision_objects_updated[link_root].id = link_root self.collision_objects_updated[ link_root].operation = CollisionObject.MOVE if 'pose' in kwargs: updated_pose = kwargs['pose'] self.move_collision_object( self.collision_objects_updated[link_root], self.collision_objects[link_root], updated_pose)
def prepare_markers(prefix): world.for_all_links(prepare_link_marker) for marker in markers: marker.header.frame_id = prefix + pysdf.sdf2tfname( marker.header.frame_id) marker.ns = prefix + pysdf.sdf2tfname(marker.ns)
def link2marker_msg(link, full_linkname, use_collision=False, lifetime=rospy.Duration(0)): marker_msg = None linkpart = None if use_collision: linkparts = getattr(link, 'collisions') else: # visual linkparts = getattr(link, 'visuals') msgs = [] for linkpart in linkparts: if not linkpart.geometry_type in supported_geometry_types: if linkpart.geometry_type: print( "Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) return None marker_msg = copy.deepcopy(protoMarkerMsg) marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname) marker_msg.header.stamp = rospy.get_rostime() marker_msg.lifetime = lifetime marker_msg.ns = pysdf.sdf2tfname(full_linkname + "::" + linkpart.name) marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose) if linkpart.geometry_type == 'mesh': marker_msg.type = Marker.MESH_RESOURCE # print('linkpart.geometry_data: %s' % linkpart.geometry_data['uri']) for models_path in pysdf.models_paths: resource = linkpart.geometry_data['uri'].replace( 'model://', models_path + '/') # print('resource: %s' % resource) if os.path.isfile(resource): marker_msg.mesh_resource = 'file://' + resource # print('found resource %s at %s' % (linkpart.geometry_data['uri'], resource)) break # support URDF-like resource paths starting with model:// if not marker_msg.mesh_resource and linkpart.geometry_data[ 'uri'].startswith('model://'): stripped_uri = linkpart.geometry_data['uri'].replace( 'model://', '') uri_parts = stripped_uri.split('/', 1) if len(uri_parts) == 2: package_name = uri_parts[0] try: package_path = gazebo_rospack.get_path(package_name) mesh_path = os.path.join(package_path, uri_parts[1]) if os.path.isfile(mesh_path): marker_msg.mesh_resource = 'file://' + mesh_path except ResourceNotFound, e: pass if not marker_msg.mesh_resource: print('ERROR! could not find resource: %s' % linkpart.geometry_data['uri']) return None scale = (float(val) for val in linkpart.geometry_data['scale'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale else: marker_msg.color.a = 1 marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5 if linkpart.geometry_type == 'box': marker_msg.type = Marker.CUBE scale = (float(val) for val in linkpart.geometry_data['size'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale elif linkpart.geometry_type == 'sphere': marker_msg.type = Marker.SPHERE marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float( linkpart.geometry_data['radius']) elif linkpart.geometry_type == 'cylinder': marker_msg.type = Marker.CYLINDER marker_msg.scale.x = marker_msg.scale.y = 2.0 * float( linkpart.geometry_data['radius']) marker_msg.scale.z = float(linkpart.geometry_data['length']) # print(marker_msg) msgs.append(marker_msg)
def link_to_collision_object(self, link, full_linkname): supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box'] linkparts = getattr(link, 'collisions') if self.is_ignored(link.parent_model): print("Ignoring link %s." % full_linkname) return collision_object = CollisionObject() collision_object.header.frame_id = pysdf.sdf2tfname(full_linkname) for linkpart in linkparts: if linkpart.geometry_type not in supported_geometry_types: print( "Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) continue if linkpart.geometry_type == 'mesh': scale = tuple( float(val) for val in linkpart.geometry_data['scale'].split()) for models_path in pysdf.models_paths: resource = linkpart.geometry_data['uri'].replace( 'model://', models_path) if os.path.isfile(resource): mesh_path = resource break if mesh_path: link_pose_stamped = PoseStamped() link_pose_stamped.pose = pysdf.homogeneous2pose_msg( linkpart.pose) if not self.make_mesh(collision_object, link_pose_stamped, mesh_path, scale): return None elif linkpart.geometry_type == 'box': scale = tuple( float(val) for val in linkpart.geometry_data['size'].split()) box = SolidPrimitive() box.type = SolidPrimitive.BOX box.dimensions = scale collision_object.primitives.append(box) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) elif linkpart.geometry_type == 'sphere': sphere = SolidPrimitive() sphere.type = SolidPrimitive.SPHERE sphere.dimensions = [float(linkpart.geometry_data['radius'])] collision_object.primitives.append(sphere) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) elif linkpart.geometry_type == 'cylinder': cylinder = SolidPrimitive() cylinder.type = SolidPrimitive.CYLINDER cylinder.dimensions = tuple( (float(linkpart.geometry_data['length']), float(linkpart.geometry_data['radius']))) collision_object.primitives.append(cylinder) collision_object.primitive_poses.append( pysdf.homogeneous2pose_msg(linkpart.pose)) return collision_object
def calculate_tfs(prefix): world.for_all_joints(calculate_joint_tf) for tf in tfs: tf[0] = prefix + pysdf.sdf2tfname(tf[0]) tf[1] = prefix + pysdf.sdf2tfname(tf[1])
def on_link_states_msg(link_states_msg): """ Publish tf for each model in current Gazebo world """ global lastUpdateTime sinceLastUpdateDuration = rospy.get_rostime() - lastUpdateTime if sinceLastUpdateDuration.to_sec() < updatePeriod: return lastUpdateTime = rospy.get_rostime() poses = {'gazebo_world': identity_matrix()} for (link_idx, link_name) in enumerate(link_states_msg.name): poses[link_name] = pysdf.pose_msg2homogeneous( link_states_msg.pose[link_idx]) for (link_idx, link_name) in enumerate(link_states_msg.name): modelinstance_name = link_name.split('::')[0] try: model_name = modelinstance_name if (submodelsToBeIncluded and model_name not in submodelsToBeIncluded ) or model_name in submodelsToBeIgnored: rospy.logdebug( "gazebo2tf_node: Model instance {} not included".format( model_name)) continue else: rospy.logdebug( "gazebo2tf_node: Model instance {} included".format( model_name)) model = load_model(model_name) parentinstance_link_name = get_parentinstance_link_name( model, model_name, modelinstance_name, link_name) assert (parentinstance_link_name) except Exception: try: model_name = pysdf.name2modelname(modelinstance_name) if (submodelsToBeIncluded and model_name not in submodelsToBeIncluded ) or model_name in submodelsToBeIgnored: rospy.logdebug( "gazebo2tf_node: Model {} not included".format( model_name)) continue else: rospy.logdebug( "gazebo2tf_node: Model {} included".format(model_name)) model = load_model(model_name) parentinstance_link_name = get_parentinstance_link_name( model, model_name, modelinstance_name, link_name) assert (parentinstance_link_name) except Exception as e: rospy.logdebug( 'gazebo2tf_node: Failed to find parent instance link name for ' 'model {}, model instance {}: {}'.format( model_name, modelinstance_name, repr(e))) continue if not is_included(parentinstance_link_name) or is_ignored( parentinstance_link_name): rospy.loginfo('Ignoring TF {} -> {}'.format( parentinstance_link_name, link_name)) continue pose = poses[link_name] parent_pose = poses[parentinstance_link_name] rel_tf = concatenate_matrices(inverse_matrix(parent_pose), pose) translation, quaternion = pysdf.homogeneous2translation_quaternion( rel_tf) tfBroadcaster.sendTransform(translation, quaternion, rospy.get_rostime(), pysdf.sdf2tfname(link_name), pysdf.sdf2tfname(parentinstance_link_name))
def prepare_markers(prefix): world.for_all_links(prepare_link_marker) for marker in markers: marker.header.frame_id = prefix + pysdf.sdf2tfname(marker.header.frame_id) marker.ns = prefix + pysdf.sdf2tfname(marker.ns)
def link2marker_msg(link, full_linkname, use_collision = False, lifetime = rospy.Duration(0)): marker_msg = None linkpart = None if use_collision: linkparts = getattr(link, 'collisions') else: # visual linkparts = getattr(link, 'visuals') msgs = [] for linkpart in linkparts: if not linkpart.geometry_type in supported_geometry_types: if linkpart.geometry_type: print("Element %s with geometry type %s not supported. Ignored." % (full_linkname, linkpart.geometry_type)) return None marker_msg = copy.deepcopy(protoMarkerMsg) marker_msg.header.frame_id = pysdf.sdf2tfname(full_linkname) marker_msg.header.stamp = rospy.get_rostime() marker_msg.lifetime = lifetime marker_msg.ns = pysdf.sdf2tfname(full_linkname + "::" + linkpart.name) marker_msg.pose = pysdf.homogeneous2pose_msg(linkpart.pose) if linkpart.geometry_type == 'mesh': marker_msg.type = Marker.MESH_RESOURCE #print('linkpart.geometry_data: %s' % linkpart.geometry_data['uri']) for models_path in pysdf.models_paths: resource = linkpart.geometry_data['uri'].replace('model://', models_path + '/') #print('resource: %s' % resource) if os.path.isfile(resource): marker_msg.mesh_resource = 'file://' + resource #print('found resource %s at %s' % (linkpart.geometry_data['uri'], resource)) break # support URDF-like resource paths starting with model:// if not marker_msg.mesh_resource and linkpart.geometry_data['uri'].startswith('model://'): stripped_uri = linkpart.geometry_data['uri'].replace('model://', '') uri_parts = stripped_uri.split('/', 1) if len(uri_parts) == 2: package_name = uri_parts[0] try: package_path = gazebo_rospack.get_path(package_name) mesh_path = os.path.join(package_path, uri_parts[1]) if os.path.isfile(mesh_path): marker_msg.mesh_resource = 'file://' + mesh_path except ResourceNotFound, e: pass if not marker_msg.mesh_resource: print('ERROR! could not find resource: %s' % linkpart.geometry_data['uri']) return None scale = (float(val) for val in linkpart.geometry_data['scale'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale else: marker_msg.color.a = 1 marker_msg.color.r = marker_msg.color.g = marker_msg.color.b = 0.5 if linkpart.geometry_type == 'box': marker_msg.type = Marker.CUBE scale = (float(val) for val in linkpart.geometry_data['size'].split()) marker_msg.scale.x, marker_msg.scale.y, marker_msg.scale.z = scale elif linkpart.geometry_type == 'sphere': marker_msg.type = Marker.SPHERE marker_msg.scale.x = marker_msg.scale.y = marker_msg.scale.z = 2.0 * float(linkpart.geometry_data['radius']) elif linkpart.geometry_type == 'cylinder': marker_msg.type = Marker.CYLINDER marker_msg.scale.x = marker_msg.scale.y = 2.0 * float(linkpart.geometry_data['radius']) marker_msg.scale.z = float(linkpart.geometry_data['length']) #print(marker_msg) msgs.append(marker_msg)