autonomous_bicycle

from __future__ import print_function

# Developed by: andreasBihlmaier
# source: https://github.com/andreasBihlmaier/gazebo2rviz

import copy

import rospy
from rospkg import RosPack, ResourceNotFound
from visualization_msgs.msg import Marker

import pysdf
import os.path

protoMarkerMsg = Marker()
protoMarkerMsg.frame_locked = True
protoMarkerMsg.id = 0
protoMarkerMsg.action = Marker.ADD
protoMarkerMsg.mesh_use_embedded_materials = True
protoMarkerMsg.color.a = 0.0
protoMarkerMsg.color.r = 0.0
protoMarkerMsg.color.g = 0.0
protoMarkerMsg.color.b = 0.0
supported_geometry_types = ['mesh', 'cylinder', 'sphere', 'box']

gazebo_rospack = RosPack()


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.Time.now()  # 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)

        if marker_msg.scale.x <= 0:
            marker_msg.scale.x = 0.0001
        if marker_msg.scale.y <= 0:
            marker_msg.scale.y = 0.0001
        if marker_msg.scale.z <= 0:
            marker_msg.scale.z = 0.0001

        msgs.append(marker_msg)
    return msgs