def init_viz(self):
     self.mode_t = TextMarker("mode", "", (0, 0, 0))
     self.heading_marker = HeadingMarker("heading", (0, 0, 0))
     self.vlm = VerticalLineMarker("altitude", (1, 1), color=Colors.WHITE + Alpha(0.5))
     self.alt_t = TextMarker("altitude", "0.0", (0, 0, 0))
     self.pos_t = TextMarker("position", "0.0,0.0", (0, 0, 0))
     self.trail = TrailMarker("trail", [], colors=[], color=Colors.BLUE + Alpha(0.8))
     self.trail.set_max_points(500)
     self.ground_trail = TrailMarker("ground_track", [], color=Colors.WHITE + Alpha(0.2))
     self.ground_trail.set_max_points(500)
     self.boundary = PolygonMarker(
         "boundary",
         ((1.5, 1.5, 0), (-1.5, 1.5, 0), (-1.5, -1.5, 0), (1.5, -1.5, 0)),
         color=Colors.RED + Alpha(0.5),
         width=0.02,
     )
     self.mg = MarkerGroup(self.mapub)
     self.mg.add(
         self.mode_t,
         self.vlm,
         self.alt_t,
         self.pos_t,
         self.trail,
         self.ground_trail,
         self.heading_marker,
         self.boundary,
     )
 def init_viz(self):
     self.mode_t = TextMarker('mode', '', (0,0,0))
     self.heading_marker = HeadingMarker('heading', (0,0,0))
     self.vlm = VerticalLineMarker('altitude', (1,1), color=Colors.WHITE + Alpha(0.5))
     self.alt_t = TextMarker('altitude', '0.0', (0,0,0))
     self.pos_t = TextMarker('position', '0.0,0.0', (0,0,0))
     self.trail = TrailMarker('trail', [], colors=[], color=Colors.BLUE + Alpha(0.8))
     self.trail.set_max_points(500)
     self.ground_trail = TrailMarker('ground_track', [], color=Colors.WHITE + Alpha(0.2))
     self.ground_trail.set_max_points(500)
     self.boundary = PolygonMarker('boundary', ((1.5,1.5,0), (-1.5,1.5,0), (-1.5,-1.5,0), (1.5,-1.5,0)), 
                                   color=Colors.RED+Alpha(0.5), width=0.02)
     self.mg = MarkerGroup(VizModuleBase.mapub)
     self.mg.add(self.mode_t, self.vlm, self.alt_t, self.pos_t, self.trail, 
                 self.ground_trail, self.heading_marker, self.boundary)
class BasicVizModule(VizModuleBase):
    """
    A 'basic' visualization module. Provides the following:
    
    - altitude (graphical and text)
    - heading (graphical and text)
    - position (text)
    - trajectory 'trail', with altitude-color-coding
    - ground track, the projection of the trajectory on the ground plane
    - boundary, changes color from green -> yellow -> red depending on proximity of flyer
    """
    
    def __init__(self):
        super(BasicVizModule, self).__init__(id="basic")
        
    def init_viz(self):
        self.mode_t = TextMarker('mode', '', (0,0,0))
        self.heading_marker = HeadingMarker('heading', (0,0,0))
        self.vlm = VerticalLineMarker('altitude', (1,1), color=Colors.WHITE + Alpha(0.5))
        self.alt_t = TextMarker('altitude', '0.0', (0,0,0))
        self.pos_t = TextMarker('position', '0.0,0.0', (0,0,0))
        self.trail = TrailMarker('trail', [], colors=[], color=Colors.BLUE + Alpha(0.8))
        self.trail.set_max_points(500)
        self.ground_trail = TrailMarker('ground_track', [], color=Colors.WHITE + Alpha(0.2))
        self.ground_trail.set_max_points(500)
        self.boundary = PolygonMarker('boundary', ((1.5,1.5,0), (-1.5,1.5,0), (-1.5,-1.5,0), (1.5,-1.5,0)), 
                                      color=Colors.RED+Alpha(0.5), width=0.02)
        self.mg = MarkerGroup(VizModuleBase.mapub)
        self.mg.add(self.mode_t, self.vlm, self.alt_t, self.pos_t, self.trail, 
                    self.ground_trail, self.heading_marker, self.boundary)

#    def init_publishers(self):
#        self.mpub = rospy.Publisher('flyer_viz', Marker)
#        self.mapub = rospy.Publisher('flyer_viz_array', MarkerArray)
#        self.tfbr = TransformBroadcaster()
        
    def init_vars(self):
        self.trajectory = np.empty((1000,3))
        self.n_trajectory = 0

    def init_subscribers(self):
        prefix = self.subscriber_topic_prefix
        #self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic?
        self.odom_sub = rospy.Subscriber(prefix+'estimator/output', Odometry, self.odom_callback)
        self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback)

    def publish_timer_callback(self, event):
        now = rospy.Time.now()
        if self.latest_controller_status is not None:
            self.mode_t.set(text=self.latest_controller_status.active_mode)
        if self.latest_odom is not None:
            pos = self.latest_odom.pose.pose.position
            loppo = self.latest_odom.pose.pose.orientation
            ori_ypr = tft.euler_from_quaternion((loppo.x, loppo.y, loppo.z, loppo.w), 'rzyx')
            self.vlm.set((pos.x,pos.y), zend=pos.z)
            self.mode_t.set(pos=(pos.x,pos.y,pos.z - 0.1))
            self.alt_t.set(text="%.3f" % -pos.z, pos=(pos.x,pos.y,pos.z/2))
            self.pos_t.set(text="%.2f, %.2f" % (pos.x, pos.y), pos=(pos.x,pos.y,0.02))
            self.heading_marker.set(pos=(pos.x,pos.y,pos.z), heading=degrees(ori_ypr[0]))
            self.tfbr.sendTransform((pos.x,pos.y,pos.z), (loppo.x, loppo.y, loppo.z, loppo.w), now, '/viz/flyer_axes', '/ned')
            self.tfbr.sendTransform((pos.y,pos.x,-pos.z), (0,0,0,1), now, '/viz/chase', '/enu')
            self.tfbr.sendTransform((0,0,0), tft.quaternion_from_euler(0,radians(180),0,'rzyx'), now, '/viz/onboard', '/viz/flyer_axes')
        VizModuleBase.publish(self,now)
        
    def odom_callback(self, msg):
        self.latest_odom = msg
        pos = self.latest_odom.pose.pose.position
        self.trajectory[self.n_trajectory,:] = (pos.x, pos.y, pos.z)
        self.n_trajectory += 1
        if self.n_trajectory == self.trajectory.shape[0]:
            self.trajectory.resize((self.n_trajectory+1000,3))
        self.trail.append_points([(pos.x, pos.y, pos.z)], [alt_color(-pos.z)])
        self.ground_trail.append_points([(pos.x, pos.y, 0)])
        mindist = min([a-b for a,b in ((XMAX, pos.x), (pos.x, XMIN), (YMAX, pos.y), (pos.y, YMIN))])
        if mindist <= 0:
            bcolor = Colors.RED + Alpha(1.0)
        elif mindist <= WARN_DIST:
            bcolor = Colors.YELLOW + Alpha(1.0)
        else:
            bcolor = Colors.GREEN + Alpha(1.0)
        self.boundary.set(color=bcolor)

    def controller_status_callback(self, msg):
        self.latest_controller_status = msg
def batt_color(voltage):
    if voltage > 10.5:
        return Colors.GREEN
    elif voltage >= 10.0:
        return Colors.YELLOW
    else:
        return Colors.RED
    
rospy.init_node('markers_test')
mpub = rospy.Publisher('markers', Marker)
mapub = rospy.Publisher('markers_array', MarkerArray)
mg = MarkerGroup(mapub)
default_frame_id = "/ned"
vlm = VerticalLineMarker('demo', (1,1), color=Colors.WHITE + Alpha(0.5))
tm = TextMarker('demo', 'foo', (0,0,0))
tm2 = TextMarker('demo', 'Hello', (0,0,0))
batt_txt = TextMarker('battery', '', (0,0,0))
trail = TrailMarker('trail', [], colors=[], color=Colors.BLUE + Alpha(0.8))
trail.set_max_points(500)
ground_trail = TrailMarker('ground_track', [], color=Colors.GREEN + Alpha(0.2))
ground_nominal_trail = TrailMarker('commanded', [], color=Colors.YELLOW + Alpha(0.2), width=0.015)
heading = HeadingMarker('heading', (0,0,0), height=0.02)
boundary = PolygonMarker('boundary', ((1,1,0), (-1,1,0), (-1,-1,0), (1,-1,0)), color=Colors.RED+Alpha(0.5), width=0.02)
prre = PlanarRectangleWithRoundedEnds('prre', (0,0,0), (1,0), 5, 0.25, color=Colors.GREEN+Alpha(0.8))
mg.add(vlm, tm, tm2, batt_txt, trail, ground_trail, ground_nominal_trail, heading, boundary, prre)
r = rospy.Rate(50)
angle = 0
points = []
print "Run the following command:"
print "rosrun rviz rviz -d $(rospack find starmac_roslib)/demos/testviz.vcg"
class FlyerViz(object):
    """
    The main purpose of this node is to listen to telemetry from the flyer and use it to generate
    messages for the various rviz display types (in most cases, Marker or MarkerArray).
    """

    # latest_llstatus = None
    latest_odom = None
    latest_controller_status = None

    def __init__(self):
        self.init_params()
        self.init_publishers()
        self.init_vars()
        self.init_viz()
        self.init_subscribers()
        self.init_timers()
        rospy.loginfo("Initialization complete")

    def init_params(self):
        self.publish_freq = rospy.get_param("~publish_freq", 20.0)
        self.subscriber_topic_prefix = rospy.get_param("~subscriber_topic_prefix", "downlink/")

    def init_publishers(self):
        self.mpub = rospy.Publisher("flyer_viz", Marker)
        self.mapub = rospy.Publisher("flyer_viz_array", MarkerArray)
        self.tfbr = TransformBroadcaster()

    def init_vars(self):
        self.trajectory = np.empty((1000, 3))
        self.n_trajectory = 0

    def init_viz(self):
        self.mode_t = TextMarker("mode", "", (0, 0, 0))
        self.heading_marker = HeadingMarker("heading", (0, 0, 0))
        self.vlm = VerticalLineMarker("altitude", (1, 1), color=Colors.WHITE + Alpha(0.5))
        self.alt_t = TextMarker("altitude", "0.0", (0, 0, 0))
        self.pos_t = TextMarker("position", "0.0,0.0", (0, 0, 0))
        self.trail = TrailMarker("trail", [], colors=[], color=Colors.BLUE + Alpha(0.8))
        self.trail.set_max_points(500)
        self.ground_trail = TrailMarker("ground_track", [], color=Colors.WHITE + Alpha(0.2))
        self.ground_trail.set_max_points(500)
        self.boundary = PolygonMarker(
            "boundary",
            ((1.5, 1.5, 0), (-1.5, 1.5, 0), (-1.5, -1.5, 0), (1.5, -1.5, 0)),
            color=Colors.RED + Alpha(0.5),
            width=0.02,
        )
        self.mg = MarkerGroup(self.mapub)
        self.mg.add(
            self.mode_t,
            self.vlm,
            self.alt_t,
            self.pos_t,
            self.trail,
            self.ground_trail,
            self.heading_marker,
            self.boundary,
        )

    def init_subscribers(self):
        prefix = self.subscriber_topic_prefix
        # self.llstatus_sub = rospy.Subscriber(prefix+'autopilot/LL_STATUS', LLStatus, self.llstatus_callback) # AscTec specific.. how to make more generic?
        self.odom_sub = rospy.Subscriber(prefix + "estimator/output", Odometry, self.odom_callback)
        self.controller_status_sub = rospy.Subscriber(
            prefix + "controller/status", controller_status, self.controller_status_callback
        )

    def init_timers(self):
        self.publish_timer = Timer(rospy.Duration(1 / self.publish_freq), self.publish_timer_callback)

    def publish_timer_callback(self, event):
        now = rospy.Time.now()
        if self.latest_controller_status is not None:
            self.mode_t.set(text=self.latest_controller_status.active_mode)
        if self.latest_odom is not None:
            pos = self.latest_odom.pose.pose.position
            loppo = self.latest_odom.pose.pose.orientation
            ori_ypr = tft.euler_from_quaternion((loppo.x, loppo.y, loppo.z, loppo.w), "rzyx")
            self.vlm.set((pos.x, pos.y), zend=pos.z)
            self.mode_t.set(pos=(pos.x, pos.y, pos.z - 0.1))
            self.alt_t.set(text="%.3f" % -pos.z, pos=(pos.x, pos.y, pos.z / 2))
            self.pos_t.set(text="%.2f, %.2f" % (pos.x, pos.y), pos=(pos.x, pos.y, 0.02))
            self.heading_marker.set(pos=(pos.x, pos.y, pos.z), heading=degrees(ori_ypr[0]))
            self.tfbr.sendTransform(
                (pos.x, pos.y, pos.z), (loppo.x, loppo.y, loppo.z, loppo.w), now, "/viz/flyer_axes", "/ned"
            )
            self.tfbr.sendTransform((pos.y, pos.x, -pos.z), (0, 0, 0, 1), now, "/viz/chase", "/enu")
            self.tfbr.sendTransform(
                (0, 0, 0), tft.quaternion_from_euler(0, radians(180), 0, "rzyx"), now, "/viz/onboard", "/viz/flyer_axes"
            )
        self.mg.publish(now)

    #    def llstatus_callback(self, msg):
    #        self.latest_llstatus = msg

    def controller_status_callback(self, msg):
        self.latest_controller_status = msg

    def odom_callback(self, msg):
        self.latest_odom = msg
        pos = self.latest_odom.pose.pose.position
        self.trajectory[self.n_trajectory, :] = (pos.x, pos.y, pos.z)
        self.n_trajectory += 1
        if self.n_trajectory == self.trajectory.shape[0]:
            self.trajectory.resize((self.n_trajectory + 1000, 3))
        self.trail.append_points([(pos.x, pos.y, pos.z)], [alt_color(-pos.z)])
        self.ground_trail.append_points([(pos.x, pos.y, 0)])
        mindist = min([a - b for a, b in ((XMAX, pos.x), (pos.x, XMIN), (YMAX, pos.y), (pos.y, YMIN))])
        if mindist <= 0:
            bcolor = Colors.RED + Alpha(1.0)
        elif mindist <= WARN_DIST:
            bcolor = Colors.YELLOW + Alpha(1.0)
        else:
            bcolor = Colors.GREEN + Alpha(1.0)
        self.boundary.set(color=bcolor)