def changeTaskState(e):
send_text = OverlayText()
send_text.text = 'TaskState: ' + e.dst
send_text.top = 10
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
if e.dst in tsm.fatal:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.3
send_text.fg_color.b = 0.3
elif e.dst in tsm.warn:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.5
send_text.fg_color.b = 0.1
else:
send_text.fg_color.r = 0.3
send_text.fg_color.g = 0.8
send_text.fg_color.b = 0.3
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
task_state_text_pub.publish(send_text)
def changeObjState(e):
send_text = OverlayText()
send_text.text = 'ObjState: '+e.dst
send_text.top = 70
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
if e.dst in osm.fatal:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.3
send_text.fg_color.b = 0.3
elif e.dst in osm.warn:
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.5
send_text.fg_color.b = 0.1
else:
send_text.fg_color.r = 0.2
send_text.fg_color.g = 0.7
send_text.fg_color.b = 0.7
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
obj_state_text_pub.publish(send_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 publishModeText(message):
send_text = OverlayText()
send_text.text = message
send_text.top = 130
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.8
send_text.fg_color.b = 0.8
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
send_text_pub.publish(send_text)
def publishModeText(message):
send_text = OverlayText()
send_text.text = message
send_text.top = 100
send_text.left = 0
send_text.width = 300
send_text.height = 50
send_text.bg_color.r = 0.9;
send_text.bg_color.b = 0.9;
send_text.bg_color.g = 0.9;
send_text.bg_color.a = 0.1;
send_text.fg_color.r = 0.3;
send_text.fg_color.g = 0.8;
send_text.fg_color.b = 0.3;
send_text.fg_color.a = 1;
send_text.line_width = 1;
send_text.text_size = 30;
send_text_pub.publish(send_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 publishModeText(message):
send_text = OverlayText()
send_text.text = message
send_text.top = 130
send_text.left = 10
send_text.width = 750
send_text.height = 50
send_text.bg_color.r = 0.9
send_text.bg_color.b = 0.9
send_text.bg_color.g = 0.9
send_text.bg_color.a = 0.1
send_text.fg_color.r = 0.8
send_text.fg_color.g = 0.8
send_text.fg_color.b = 0.8
send_text.fg_color.a = 1
send_text.line_width = 1
send_text.text_size = 30
send_text_pub.publish(send_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)
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
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()
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 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)))
def isConvergent(self,
frame,
target_xy,
target_z,
target_yaw,
gps_mode,
pos_conv_thresh,
yaw_conv_thresh,
vel_conv_thresh,
att_conv_thresh=0.06):
current_xy = self.getBaselinkPos()[0:2]
current_z = self.getBaselinkPos()[2]
current_yaw = self.getBaselinkRPY()[2]
current_vel = self.getBaselinkLinearVel()
current_rp = self.getBaselinkRPY()[0:2]
if gps_mode:
delta_pos = gps2xy(self.gps_lat_lon_, target_xy)
else:
if frame == 'global':
delta_pos = np.array([
target_xy[0] - current_xy[0], target_xy[1] - current_xy[1],
target_z - current_z
])
elif frame == 'local':
delta_pos = np.array(
[target_xy[0], target_xy[1], target_z - current_z])
else:
rospy.logerr('invalid frame %s' % (frame))
return
delta_yaw = target_yaw - current_yaw
if delta_yaw > np.pi:
delta_yaw -= np.pi * 2
elif delta_yaw < -np.pi:
delta_yaw += np.pi * 2
if self.debug_view_:
text = OverlayText()
text.width = 400
text.height = 200
text.left = 10
text.top = 10
text.text_size = 12
text.line_width = 2
text.font = "DejaVu Sans Mono"
text.text = 'Diff\n pos: {:.4g}, yaw: {:.4g}, vel: {:.4g}\n roll: {:.4g}, pitch: {:.4g}\nSetPoint\n x: {:.11g} y: {:.11g} z: {:.4g} yaw: {:.4g}'.format(
np.linalg.norm(delta_pos), abs(delta_yaw),
np.linalg.norm(current_vel), abs(current_rp[0]),
abs(current_rp[1]), target_xy[0], target_xy[1], target_z,
target_yaw)
self.nav_debug_pub_.publish(text)
if np.linalg.norm(delta_pos) < pos_conv_thresh and abs(
delta_yaw) < yaw_conv_thresh and np.linalg.norm(
current_vel) < vel_conv_thresh and abs(
current_rp[0]) < att_conv_thresh and abs(
current_rp[0]) < att_conv_thresh:
return True
else:
return False
#publication
self.state_machine_pub_.publish(self.state_name_[self.state_machine_])
if self.visualization_ == True:
if self.state_machine_ != self.FINISH_STATE_:
self.task_elapsed_time = (rospy.Time.now() -
self.task_start_time_).to_sec()
text_msg = OverlayText()
text_msg.width = 500
text_msg.height = 110
text_msg.left = 10
text_msg.top = 10
text_msg.text_size = 20
text_msg.line_width = 2
text_msg.font = "DejaVu Sans Mono"
text_msg.text = """Task Kind:%d
State:%d,%s
Time:%.1f """ % (
self.task_kind_, self.state_machine_,
self.state_name_[self.state_machine_], self.task_elapsed_time)
text_msg.fg_color = ColorRGBA(25 / 255.0, 1.0, 240.0 / 255.0, 1.0)
text_msg.bg_color = ColorRGBA(0.0, 0.0, 0.0, 0.2)
self.state_visualization_pub_.publish(text_msg)
target_pos_msg = Odometry()
target_pos_msg.header = self.odom_.header
if self.target_frame_ == self.GLOBAL_FRAME_:
target_pos_msg.child_frame_id = "global"
elif self.target_frame_ == self.LOCAL_FRAME_:
