def init_viz(self):
self.as_poly = PolygonMarker('autosequence', (),
color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False)
self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5))
self.mg = MarkerGroup(VizModuleBase.mapub)
self.mg.add(self.as_poly, self.as_hover_point_sphere)
self.init_int_marker()
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
class AutosequencesVizModule(VizModuleBase):
"""
Visualization module for control_mode_autosequence.
Provides:
- polygon representing autosequence path
- markers for autosequence points
- labels
- heading indicators at each point
"""
got_autosequence = False
current_autosequence = None
latest_autoseq_info = None
latest_hover_info = None
loaded_as_name = ""
def __init__(self):
super(AutosequencesVizModule, self).__init__(id="autoseq")
def init_viz(self):
self.as_poly = PolygonMarker('autosequence', (),
color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False)
self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5))
self.mg = MarkerGroup(VizModuleBase.mapub)
self.mg.add(self.as_poly, self.as_hover_point_sphere)
def init_vars(self):
pass
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.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info',
control_mode_autosequence_info, self.autoseq_info_callback)
self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info',
control_mode_hover_info, self.hover_info_callback)
self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback)
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 autoseq_info_callback(self, msg):
#rospy.loginfo('got autoseq info: %s' % str(msg))
self.latest_autoseq_info = msg
def hover_info_callback(self, msg):
self.latest_hover_info = msg
def _new_autosequence(self):
return (self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name))
def controller_status_callback(self, msg):
self.latest_controller_status = msg
#rospy.loginfo('got controller status: %s' % str(msg))
if msg.active_mode == 'autosequence':
if self._new_autosequence():
self.got_autosequence = False
if (not self.got_autosequence
and self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and len(self.latest_autoseq_info.current_autosequence) > 0):
as_name = self.latest_autoseq_info.current_autosequence
self.get_autosequence(as_name)
def get_autosequence(self, as_name):
get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence)
rospy.loginfo("Asking for autosequence '%s'..." % as_name)
resp = get_auto_proxy(autosequence_name=as_name)
if resp.found:
self.current_autosequence = resp.autosequence
rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence))
self.got_autosequence = True
self.loaded_as_name = as_name
else:
rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)
class AutosequencesVizModule(VizModuleBase):
"""
Visualization module for control_mode_autosequence.
Provides:
- polygon representing autosequence path
- markers for autosequence points
- labels
- heading indicators at each point
"""
got_autosequence = False
current_autosequence = None
latest_autoseq_info = None
latest_hover_info = None
loaded_as_name = ""
def __init__(self):
super(AutosequencesVizModule, self).__init__(id="autoseq")
def init_viz(self):
self.as_poly = PolygonMarker('autosequence', (),
color=Colors.BLUE+Alpha(0.5), width=0.02, closed=False)
self.as_hover_point_sphere = SphereMarker('hover_point', (0,0,0), radius=0.02, color=Colors.BLUE+Alpha(0.5))
self.mg = MarkerGroup(VizModuleBase.mapub)
self.mg.add(self.as_poly, self.as_hover_point_sphere)
self.init_int_marker()
def init_vars(self):
pass
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.autoseq_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/autosequence_info',
control_mode_autosequence_info, self.autoseq_info_callback)
self.hover_info_sub = rospy.Subscriber(prefix+'control_mode_autosequence/info',
control_mode_hover_info, self.hover_info_callback)
self.controller_status_sub = rospy.Subscriber(prefix+'controller/status', controller_status, self.controller_status_callback)
def init_int_marker(self):
self.ims = InteractiveMarkerServer("simple_marker")
self.im = InteractiveMarker()
self.im.header.frame_id = "/ned"
self.im.name = "my_marker"
self.im.description = "Simple 1-DOF control"
bm = Marker()
bm.type = Marker.CUBE
bm.scale.x = bm.scale.y = bm.scale.z = 0.45
bm.color.r = 0.0
bm.color.g = 0.5
bm.color.b = 0.5
bm.color.a = 1.0
bc = InteractiveMarkerControl()
bc.always_visible = True
bc.markers.append(bm)
self.im.controls.append(bc)
rc = InteractiveMarkerControl()
rc.name = "move_x"
rc.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
self.im.controls.append(rc)
self.ims.insert(self.im, self.process_marker_feedback)
self.ims.applyChanges()
def process_marker_feedback(self, feedback):
p = feedback.pose.position
print feedback.marker_name + " is now at " + str(p.x) + ", " + str(p.y) + ", " + str(p.z)
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 autoseq_info_callback(self, msg):
#rospy.loginfo('got autoseq info: %s' % str(msg))
self.latest_autoseq_info = msg
def hover_info_callback(self, msg):
self.latest_hover_info = msg
def _new_autosequence(self):
return (self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and (self.latest_autoseq_info.current_autosequence != self.loaded_as_name))
def controller_status_callback(self, msg):
self.latest_controller_status = msg
#rospy.loginfo('got controller status: %s' % str(msg))
if msg.active_mode == 'autosequence':
if self._new_autosequence():
self.got_autosequence = False
if (not self.got_autosequence
and self.latest_autoseq_info is not None
and len(self.latest_autoseq_info.defined_autosequences) > 0
and len(self.latest_autoseq_info.current_autosequence) > 0):
as_name = self.latest_autoseq_info.current_autosequence
self.get_autosequence(as_name)
def get_autosequence(self, as_name):
get_auto_proxy = rospy.ServiceProxy('control_mode_autosequence/get_autosequence', GetAutosequence)
rospy.loginfo("Asking for autosequence '%s'..." % as_name)
resp = get_auto_proxy(autosequence_name=as_name)
if resp.found:
self.current_autosequence = resp.autosequence
rospy.loginfo('Got autosequence: %s' % str(self.current_autosequence))
self.got_autosequence = True
self.loaded_as_name = as_name
else:
rospy.logerr("Service call failed: autosequence '%s' not found" % as_name)
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)