viz_map

#!/usr/bin/env python

"""Adapted from the node 'osm_server' from the 'osm_cartography' package."""

import rospy

import sys

from geographic_msgs.msg import GeographicMap

from geometry_msgs.msg import Point
from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Vector3
from std_msgs.msg import ColorRGBA
from visualization_msgs.msg import Marker
from visualization_msgs.msg import MarkerArray

class VizNode:
    def __init__(self):
        """ROS node to publish visualization markers for a GeographicMap."""
        rospy.init_node("viz_map")
        self.config = None

        # advertise visualization marker topic
        self.pub = rospy.Publisher(
            "visualization_marker_array", MarkerArray, latch=True, queue_size=10
        )
        self.map = None
        self.msg = None
        #rospy.wait_for_service("topography")
        #self.get_map = rospy.ServiceProxy("topography", GetTopography)

        # refresh the markers every three seconds, making them last fourh.
        self.timer_interval = rospy.Duration(3)
        self.marker_life = self.timer_interval + rospy.Duration(1)
        rospy.Timer(self.timer_interval, self.timer_callback)

        self.sub = rospy.Subscriber("topography", GeographicMap, self.handle_topo, queue_size=10)

        # register dynamic reconfigure callback, which runs immediately
        #self.reconf_server = ReconfigureServer(Config, self.reconfigure)

    def match(self, msg, val_set):
        if type(val_set) is not set:
            raise ValueError('property matching requires a set of keys')
        for prop in msg.props:
            if prop.value in val_set:
                return (prop.key, prop.value)
        return None

    def get_markers(self, gmap):
        """Get markers for a GeographicMap message.

        :post: self.msg = visualization markers message
        """
        self.map = gmap
        
        self.map_points = gmap.points

        # WUPOINT style:
        self.way_point_ids = {}  
        for wid in range(len(self.map_points)):
            self.way_point_ids[self.map_points[wid].id.uuid] = wid

        self.msg = MarkerArray()
        self.mark_boundaries(ColorRGBA(r=0.5, g=0.5, b=0.5, a=0.8))
        self.mark_way_points(ColorRGBA(r=1.0, g=1.0, b=0.0, a=0.8))

        # define arguments for displaying various feature types
        fargs = [
            (
                lambda f: self.match(f, set(["roads"])),
                ColorRGBA(r=rospy.get_param("/c_roads/r"),
                          g=rospy.get_param("/c_roads/g"),
                          b=rospy.get_param("/c_roads/b"),
                          a=rospy.get_param("/c_roads/a")),
                "roads",
            ),
            (
                lambda f: self.match(f, set(["buildings"])),
                ColorRGBA(r=rospy.get_param("/c_buildings/r"),
                          g=rospy.get_param("/c_buildings/g"),
                          b=rospy.get_param("/c_buildings/b"),
                          a=rospy.get_param("/c_buildings/a")),
                "buildings",
            ),
            (
                lambda f: self.match(f, set(["rails"])),
                ColorRGBA(r=rospy.get_param("/c_rails/r"),
                          g=rospy.get_param("/c_rails/g"),
                          b=rospy.get_param("/c_rails/b"),
                          a=rospy.get_param("/c_rails/a")),
                "rails",
            ),
            (
                lambda f: self.match(f, set(["footways"])),
                ColorRGBA(r=rospy.get_param("/c_footways/r"),
                          g=rospy.get_param("/c_footways/g"),
                          b=rospy.get_param("/c_footways/b"),
                          a=rospy.get_param("/c_footways/a")),
                "footways",
            ),
            (
                lambda f: self.match(f, set(["water"])),
                ColorRGBA(r=rospy.get_param("/c_water/r"),
                          g=rospy.get_param("/c_water/g"),
                          b=rospy.get_param("/c_water/b"),
                          a=rospy.get_param("/c_water/a")),
                "water",
            ),
            (
                lambda f: self.match(f, set(["forest"])),
                ColorRGBA(r=rospy.get_param("/c_forest/r"),
                          g=rospy.get_param("/c_forest/g"),
                          b=rospy.get_param("/c_forest/b"),
                          a=rospy.get_param("/c_forest/a")),
                "forest",
            ),
            (
                lambda f: self.match(f, set(["antiforest"])),
                ColorRGBA(r=rospy.get_param("/c_antiforest/r"),
                          g=rospy.get_param("/c_antiforest/g"),
                          b=rospy.get_param("/c_antiforest/b"),
                          a=rospy.get_param("/c_antiforest/a")),
                "antiforest",
            ),
            (
                lambda f: self.match(f, set(["agriculture"])),
                ColorRGBA(r=rospy.get_param("/c_agriculture/r"),
                          g=rospy.get_param("/c_agriculture/g"),
                          b=rospy.get_param("/c_agriculture/b"),
                          a=rospy.get_param("/c_agriculture/a")),
                "agriculture",
            ),
            (
                lambda f: self.match(f, set(["untraversable"])),
                ColorRGBA(r=rospy.get_param("/c_untraversable/r"),
                          g=rospy.get_param("/c_untraversable/g"),
                          b=rospy.get_param("/c_untraversable/b"),
                          a=rospy.get_param("/c_untraversable/a")),
                "untraversable",
            ),
            (
                lambda f: self.match(f, set(["traversable"])),
                ColorRGBA(r=rospy.get_param("/c_traversable/r"),
                          g=rospy.get_param("/c_traversable/g"),
                          b=rospy.get_param("/c_traversable/b"),
                          a=rospy.get_param("/c_traversable/a")),
                "traversable",
            ),
            (
                lambda f: self.match(f, set(["obstacles"])),
                ColorRGBA(r=rospy.get_param("/c_obstacles/r"),
                          g=rospy.get_param("/c_obstacles/g"),
                          b=rospy.get_param("/c_obstacles/b"),
                          a=rospy.get_param("/c_obstacles/a")),
                "obstacles",
            ),
            
        ]
        for args in fargs:
            self.mark_features(*args)

    def mark_boundaries(self, color):
        # draw outline of map boundaries
        marker = Marker(
            header=self.map.header,
            ns="bounds_osm",
            id=0,
            type=Marker.LINE_STRIP,
            action=Marker.ADD,
            scale=Vector3(x=2.0),
            color=color,
            lifetime=self.marker_life,
        )

        # Convert bounds latitudes and longitudes to UTM (no
        # altitude), convert UTM points to geometry_msgs/Point
        bbox = self.map.bounds
        some_altitude = self.map.points[0].position.altitude 
        p0 = Point(bbox.min_pt.longitude, bbox.min_pt.latitude, some_altitude)
        p1 = Point(bbox.max_pt.longitude, bbox.min_pt.latitude, some_altitude)
        p2 = Point(bbox.max_pt.longitude, bbox.max_pt.latitude, some_altitude)
        p3 = Point(bbox.min_pt.longitude, bbox.max_pt.latitude, some_altitude)

        #min_lat, min_lon, max_lat, max_lon = bounding_box.getLatLong(bbox)
        #p0 = geodesy.utm.fromLatLong(min_lat, min_lon).toPoint()
        #p1 = geodesy.utm.fromLatLong(min_lat, max_lon).toPoint()
        #p2 = geodesy.utm.fromLatLong(max_lat, max_lon).toPoint()
        #p3 = geodesy.utm.fromLatLong(max_lat, min_lon).toPoint()

        # add line strips to bounds marker
        marker.points.append(p0)
        marker.points.append(p1)
        marker.points.append(p1)
        marker.points.append(p2)
        marker.points.append(p2)
        marker.points.append(p3)
        marker.points.append(p3)
        marker.points.append(p0)
        self.msg.markers.append(marker)

    def mark_features(self, predicate, color, namespace):
        """Create outline for map features

        :param predicate: function to match desired features
        :param color: RGBA value
        :param namespace: Rviz namespace.

        :todo: differentiate properties for: highway, building,
               bridge, tunnel, amenity, etc.
        """
        index = 0
        for feature in filter(predicate, self.map.features):
            if len(feature.components) <= 1:
                continue
            marker = Marker(
                header=self.map.header,
                ns=namespace,
                id=index,
                type=Marker.LINE_STRIP,
                action=Marker.ADD,
                scale=Vector3(x=2.0),
                color=color,
                lifetime=self.marker_life,
            )
            index += 1
            prev_point = None
            for mbr in feature.components:
                wu_point_index = self.way_point_ids.get(mbr.uuid)
                point = self.map_points[wu_point_index]

                if point:  # this component is a way point
                    p = Point(point.position.latitude, point.position.longitude, point.position.altitude)
                    if prev_point:
                        marker.points.append(prev_point)
                        marker.points.append(p)
                    prev_point = p
            self.msg.markers.append(marker)

    def mark_way_points(self, color):
        """Create slightly transparent disks for way-points.

        :param color: disk RGBA value
        """
        index = 0
        for wp in self.map_points:
            marker = Marker(
                header=self.map.header,
                ns="waypoints_osm",
                id=index,
                type=Marker.CYLINDER,
                action=Marker.ADD,
                scale=Vector3(x=2.0, y=2.0, z=0.2),
                color=color,
                lifetime=self.marker_life,
            )
            index += 1
            # use easting and northing coordinates (ignoring altitude)
            marker.pose.position = Point(wp.position.latitude, wp.position.longitude, wp.position.altitude)
            marker.pose.orientation = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0)
            self.msg.markers.append(marker)

    def handle_topo(self, msg):
        geo_map = msg
        self.get_markers(geo_map)
        self.pub.publish(self.msg)

        return None

    def timer_callback(self, event):
        """ Called periodically to refresh map visualization. """
        if self.msg is not None:
            now = rospy.Time()
            for m in self.msg.markers:
                m.header.stamp = now
            self.pub.publish(self.msg)


def main():
    viznode = VizNode()
    try:
        rospy.spin()
    except rospy.ROSInterruptException:
        pass


if __name__ == "__main__":
    # run main function and exit
    sys.exit(main())