def continuousDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "CONTINUOUS DATA: Updated %0.1f secs before" % (diff)
if diff <= 1.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
pub_continuous_data_updated_time.publish(text)
def burstDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "BURST DATA: Updated %0.1f secs before" % (diff)
if diff <= 30.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
pub_burst_data_updated_time.publish(text)
def robotStatusBasicInfoCallback(msg):
text = OverlayText()
text.bg_color = TRANSPARENT_COLOR
if msg.data == FC2OCSLarge.ROBOT_IDLE:
text.text = "BURST: Robot was idle"
text.fg_color = OK_COLOR
elif msg.data == FC2OCSLarge.ROBOT_MOVING:
text.text = "BURST: Robot was moving"
text.fg_color = WARN_COLOR
else:
text.text = "BURST: Robot was in unnknown status"
text.fg_color = UNKNOWN_COLOR
pub_burst_robot_status.publish(text)
def robotStatusBasicInfoCallback(msg):
text = OverlayText()
text.bg_color = TRANSPARENT_COLOR
if msg.data == FC2OCSLarge.ROBOT_IDLE:
text.text = "BURST: Robot was idle"
text.fg_color = OK_COLOR
elif msg.data == FC2OCSLarge.ROBOT_MOVING:
text.text = "BURST: Robot was moving"
text.fg_color = WARN_COLOR
else:
text.text = "BURST: Robot was in unnknown status"
text.fg_color = UNKNOWN_COLOR
text.bg_color = ROBOT_STATE_BG_COLOR
pub_burst_robot_status.publish(text)
def lowspeedDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
if msg.data.to_sec() == 0:
text.text = "LOWSPEED DATA: Not received yet"
diff = 100
else:
text.text = "LOWSPEED DATA: Updated %0.1f secs before" % (diff)
if diff <= 1.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
text.bg_color = UPDATED_TIME_BG_COLOR
pub_lowspeed_data_updated_time.publish(text)
def lowspeedDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
if msg.data.to_sec() == 0:
text.text = "LOWSPEED DATA: Not received yet"
diff = 100
else:
text.text = "LOWSPEED DATA: Updated %0.1f secs before" % (diff)
if diff <= 1.0:
text.fg_color = OK_COLOR
else:
text.fg_color = WARN_COLOR
text.bg_color = UPDATED_TIME_BG_COLOR
pub_lowspeed_data_updated_time.publish(text)
def lowspeedDataUpdatedTimeCallnack(msg):
text = OverlayText()
now = rospy.Time.now()
diff = (now - msg.data).to_sec()
text.bg_color = TRANSPARENT_COLOR
text.text = "LOWSPEED DATA: Updated %0.1f secs before" % (diff)
text.fg_color = OK_COLOR
pub_lowspeed_data_updated_time.publish(text)
def publishOverlaytext(self, world_name):
"""
"""
situations_text = " <" + world_name + "> " + self.__overlay_name + "\n\r"
situations_text += "- facts\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
fact_sorted = {}
fact_desc = {}
for situation in self.ctx.worlds()[world_name].timeline().situations():
if situation.end.data != situation.start.data:
if situation.end.data == rospy.Time(0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
else:
if rospy.Time.now() - situation.end.data < rospy.Duration(
5.0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
for key in sorted(fact_sorted.iterkeys()):
situations_text += fact_sorted[key].id[:5] + " : " + fact_sorted[
key].description + "\n\r"
situations_text += "\n\r- events\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
event_sorted = {}
for situation in self.ctx.worlds()[world_name].timeline().situations():
if situation.start.data == situation.end.data:
if rospy.Time.now() - situation.end.data < rospy.Duration(5.0):
event_sorted[situation.start.data] = situation
for key in sorted(event_sorted.iterkeys()):
situations_text += event_sorted[key].id[:5] + " : " + event_sorted[
key].description + "\n\r"
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = situations_text
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
if world_name not in self.__text_pub:
self.__text_pub[world_name] = rospy.Publisher(world_name +
"/timeline",
OverlayText,
queue_size=2)
self.__text_pub[world_name].publish(text)
def publishOverlaytext(self, world_name):
"""
"""
situations_text = " FACTS\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
fact_sorted = {}
fact_desc = {}
for situation_id, situation in self.worlds[
world_name].timeline.situations.items():
if situation.end.data != situation.start.data:
if situation.end.data == rospy.Time(0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
else:
if rospy.Time.now() - situation.end.data < rospy.Duration(
5.0):
if situation.description not in fact_desc:
fact_sorted[situation.start.data] = situation
fact_desc[situation.description] = situation
for key in sorted(fact_sorted.iterkeys()):
situations_text += fact_sorted[key].id[:5] + " : " + fact_sorted[
key].description + "\n\r"
situations_text += "\n\rEVENTS\n\r"
situations_text += "id : description\n\r"
situations_text += "----------------\n\r"
event_sorted = {}
for situation_id, situation in self.worlds[
world_name].timeline.situations.items():
if situation.start.data == situation.end.data:
if rospy.Time.now() - situation.end.data < rospy.Duration(5.0):
event_sorted[situation.start.data] = situation
for key in sorted(event_sorted.iterkeys()):
situations_text += event_sorted[key].id[:5] + " : " + event_sorted[
key].description + "\n\r"
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = situations_text
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
self.text_pub.publish(text)
def update_overlaytext(self, number=1.23, number2=20):
text = OverlayText()
text.width = 200
text.height = 400
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = """Distance= %s.
Angle=%s.
Counter = <span style="color: green;">%d.</span>
""" % (str(number), str(number2), self.counter)
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
return text
def odometry_callback(odometry):
# speed info
twist = odometry.twist.twist
speed = magnitude(twist.linear)
msg = Float32()
msg.data = speed
linear_speed_pub.publish(msg)
# position info
pos = odometry.pose.pose.position
position_text = OverlayText()
position_text.action = OverlayText.ADD
position_text.width = 128
position_text.height = 64
position_text.left = 0
position_text.top = 0
position_text.bg_color = ColorRGBA(0, 0, 0, 0.5)
position_text.fg_color = ColorRGBA(1, 1, 1, 1)
position_text.text_size = 8
position_text.text = "Current position: {:.2f} {:.2f} {:.2f}".format(pos.x, pos.y, pos.z)
position_pub.publish(position_text)
def update_overlaytext(self, number=1.23, number2=1.23, number3=20):
text = OverlayText()
text.width = 200
text.height = 400
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = """x= %s
y= %s
Height(z)= %s
""" % (str(number), str(number2), str(number3))
text.fg_color = ColorRGBA(0 / 255.0, 100.0 / 255, 0.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
return text
def joyCB(self, status, history):
msg = OverlayText()
if status.left:
self.left = self.left - 10
if status.right:
self.left = self.left + 10
if status.up:
self.top = self.top - 10
if status.down:
self.top = self.top + 10
msg.text_size = self.text_size
msg.width = self.width
msg.height = self.height
msg.left = self.left
msg.top = self.top
msg.line_width = self.line_width
msg.font = self.font
msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
msg.text = self.usage
self.pub.publish(msg)
listener = tf.TransformListener()
r = rospy.Rate(1)
pub = rospy.Publisher("diff_text", OverlayText)
while not rospy.is_shutdown():
try:
(pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
pos_diff = np.linalg.norm(pos)
# quaternion to rpy
rpy = euler_from_quaternion(rot)
rot_diff = np.linalg.norm(rpy)
print (pos_diff, rot_diff)
msg = OverlayText()
msg.width = 1000
msg.height = 200
msg.left = 10
msg.top = 10
msg.text_size = 20
msg.line_width = 2
msg.font = "DejaVu Sans Mono"
msg.text = """%s <-> %s
pos: %f
rot: %f
""" % (src1, src2, pos_diff, rot_diff)
msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.0)
pub.publish(msg)
except:
rospy.logerr("ignore error")
finally:
r.sleep()
def trajectory_callback(traj):
id_gen = id_generator(1)
# path visualizing
# waypoint position, connected with line
traj_viz = MarkerArray()
path_viz = Marker()
path_viz.header.frame_id = 'world'
path_viz.header.stamp = rospy.Time.now()
path_viz.id = next(id_gen)
path_viz.scale = Vector3(0.05, 0, 0)
path_viz.color = ColorRGBA(0, 1, 0, 1)
path_viz.type = Marker.LINE_STRIP
path_viz.action = Marker.ADD
for pt in traj.points:
path_viz.points.append(pt.transforms[0].translation)
traj_viz.markers.append(path_viz)
# waypoint direction. (linear velocity at waypoint)
# the direction of linear velocity is the same as the x direction of the drone, which is also the
# desired heading of the drone.
#
# to fully determine the orientation, we still need the direction of y or z axis of the drone
# after that we can construct a rotation matrix, and then convert it into quaternion
num_points = len(traj.points)
num_velocity_viz = 5
step = max(1, num_points / num_velocity_viz)
i = 0
while i < num_points:
pt = traj.points[i]
translation = pt.transforms[0].translation
linear = pt.velocities[0].linear
norm = math.sqrt(linear.x ** 2 + linear.y ** 2 + linear.z ** 2)
if norm < 1e-3:
i += step
continue
q = geo.vector_to_quat(linear)
velocity_viz = Marker()
velocity_viz.header.frame_id = "world"
velocity_viz.header.stamp = rospy.Time.now()
velocity_viz.id = next(id_gen)
velocity_viz.type = Marker.ARROW
velocity_viz.action = Marker.ADD
velocity_viz.pose.position = translation
velocity_viz.pose.orientation = Quaternion(*(q.tolist()))
velocity_viz.scale = Vector3(norm, 0.05, 0.05)
velocity_viz.color = ColorRGBA(1, 0, 0, 0.5)
traj_viz.markers.append(velocity_viz)
i += step
viz_pub.publish(traj_viz)
# visualize information of the next 3 waypoints
if num_points >= 3:
p1 = traj.points[0]
p2 = traj.points[1]
p3 = traj.points[2]
waypoints_info_text = OverlayText()
waypoints_info_text.action = OverlayText.ADD
waypoints_info_text.width = 128
waypoints_info_text.height = 64
waypoints_info_text.left = 0
waypoints_info_text.top = 0
waypoints_info_text.bg_color = ColorRGBA(0, 0, 0, 0.5)
waypoints_info_text.fg_color = ColorRGBA(1, 1, 1, 1)
waypoints_info_text.text_size = 8
waypoints_info_text.text = """Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
Next waypoint @ {:.2f}s: {:.2f} {:.2f} {:.2f}. Speed: {:.2f}
""".format(p1.time_from_start.to_sec(), p1.transforms[0].translation.x, p1.transforms[0].translation.y, p1.transforms[0].translation.z, magnitude(p1.velocities[0].linear),
p2.time_from_start.to_sec(), p2.transforms[0].translation.x, p2.transforms[0].translation.y, p2.transforms[0].translation.z, magnitude(p2.velocities[0].linear),
p3.time_from_start.to_sec(), p3.transforms[0].translation.x, p3.transforms[0].translation.y, p3.transforms[0].translation.z, magnitude(p3.velocities[0].linear),)
waypoints_info_pub.publish(waypoints_info_text)
def callback(self, msg):
main_msg = np.reshape(msg.data, [3, 7])
#print(main_msg)
pos_x = main_msg[0, 0]
pos_y = main_msg[0, 1]
vel_x = main_msg[0, 2]
vel_y = main_msg[0, 3]
acc_x = main_msg[0, 4]
acc_y = main_msg[0, 5]
rad = main_msg[0, 6]
if PUBLISH:
self.x_1.publish(pos_x)
self.y_1.publish(pos_y)
self.x_vel_1.publish(vel_x)
self.y_vel_1.publish(vel_y)
self.x_acc_1.publish(acc_x)
self.y_acc_1.publish(acc_y)
self.rad_1.publish(rad)
text = OverlayText()
text.width = 400
text.height = 600
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
text.font = "DejaVu Sans Mono"
arg = [x for x in main_msg[0, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Runtime: %.2f ms
Object 1
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (self.runtime, arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4],
arg[5], arg[6])
self.text_pub_1.publish(text)
arg = [x for x in main_msg[1, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Object 2
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4], arg[5], arg[6])
self.text_pub_2.publish(text)
arg = [x for x in main_msg[2, :]]
dist = math.sqrt((arg[0]**2 + arg[1]**2))
speed = math.sqrt((arg[2]**2 + arg[3]**2))
text.text = """
Object 3
Position = [%.2f, %.2f]
Distance = %.2f
Velocity = [%.2f, %.2f]
Speed = %.2f
Acceleration = [%.2f, %.2f]
True Radius = %.2f
""" % (arg[0], arg[1], dist, arg[2], arg[3], speed, arg[4], arg[5], arg[6])
self.text_pub_3.publish(text)
for i, mu in enumerate(main_msg[:3]):
self.time_elapsed = rospy.get_time() - self.start_time
circles = self.obstacles[i]
circles_lidar = self.lidars[i]
self.print_mu[i].append(
np.append(mu,
(self.runtime, self.time_elapsed, circles.center.x,
circles.center.y, circles.true_radius,
circles_lidar.center.x, circles_lidar.center.y,
circles_lidar.true_radius)))
listener = tf.TransformListener()
r = rospy.Rate(1)
pub = rospy.Publisher("diff_text", OverlayText)
while not rospy.is_shutdown():
try:
(pos, rot) = listener.lookupTransform(src1, src2, rospy.Time(0))
pos_diff = np.linalg.norm(pos)
# quaternion to rpy
rpy = euler_from_quaternion(rot)
rot_diff = np.linalg.norm(rpy)
print(pos_diff, rot_diff)
msg = OverlayText()
msg.width = 1000
msg.height = 200
msg.left = 10
msg.top = 10
msg.text_size = 20
msg.line_width = 2
msg.font = "DejaVu Sans Mono"
msg.text = """%s <-> %s
pos: %f
rot: %f
""" % (src1, src2, pos_diff, rot_diff)
msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.0)
pub.publish(msg)
except:
rospy.logerr("ignore error")
finally:
r.sleep()
