def publish_timer_callback(self, event):
if self.got_autosequence:
as_points = [(p.hover_point.x, p.hover_point.y, 0) for p in self.current_autosequence.points]
self.as_poly.set(points=as_points)
if self.latest_hover_info is not None:
slhi = self.latest_hover_info
self.as_hover_point_sphere.set(origin=(slhi.north_cmd, slhi.east_cmd, 0))
now = rospy.Time.now()
VizModuleBase.publish(self,now)
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)