last_time = this_time
ax, ay, az = (random.gauss(0.05,0.5)*math.cos(angle)+random.gauss(0,0.05) - 0.1*x,
random.gauss(0.1,0.5)*math.sin(angle)+random.gauss(0,0.1)-0.1*y,
random.gauss(0,0.5))
vx, vy, vz = vx + ax*dt, vy + ay*dt, vz + az*dt
vxerr, vyerr = random.gauss(0,0.1), random.gauss(0,0.1)
xerr, yerr = xerr + vxerr*dt, yerr + vyerr*dt
if z <= 0.0 and vz < 0:
vz = 0
elif z >= 1.5 and vz > 0:
vz = 0
x, y, z = min(10,max(-10,x + vx*dt )), min(10,max(-10,y + vy*dt )), min(1.5,max(0,z + vz*dt))
xout,yout = x + xerr, y+ yerr
alt = z + random.gauss(0,0.001)
voltage = max(9.0, voltage - dt*3/(3*60))
rospy.logdebug('%f, %f, %f' % (xout, yout, alt))
trail.append_points([(xout,yout,alt)], [(min(1,1.0*alt/2.0), 1-min(1,1.0*alt/1.0), 1-min(1,alt/0.8), 0.9)])
ground_trail.append_points([(xout,yout,0)])
ground_nominal_trail.append_points([(x,y,0)])
vlm.set((xout,yout), zend=alt)
tm.set(text="%.2f,%.2f" % (xout,yout), pos=(xout,yout,-0.02))
tm2.set(text="%.2f" % alt, pos=(xout+0.1,yout,alt/2))
batt_txt.set(text="%.2f V" % voltage, color=batt_color(voltage) + Alpha(1.0), pos=(xout,yout,alt+0.1))
heading.set(pos=(xout,yout,z+0.01), heading=math.degrees(math.atan2(vy,vx)))
prre.set(origin=(xout,yout,0), axis=(vx,vy), length=norm(np.array([vx,vy])))
dt_pub = (this_time - last_pub_time).to_sec()
if dt_pub > 0.05:
mg.publish()
last_pub_time = this_time
angle += math.radians(0.5) % math.radians(360)
r.sleep()
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)
