def publishUsage(self): overlay_text = OverlayText() overlay_text.text = """ Left Analog: x/y L2/R2 : +-z L1/R1 : +-yaw Left/Right : +-roll Up/Down : +-pitch circle : Go cross : Reset/Cancel triangle : Clear maps and look around ground up/down : Move menu cursors """ overlay_text.width = 500 overlay_text.height = 500 overlay_text.text_size = 12 overlay_text.left = 10 overlay_text.top = 10 overlay_text.font = "Ubuntu Mono Regular" overlay_text.bg_color.a = 0 overlay_text.fg_color.r = 25 / 255.0 overlay_text.fg_color.g = 1 overlay_text.fg_color.b = 1 overlay_text.fg_color.a = 1 self.usage_pub.publish(overlay_text)
def publishUsage(self): overlay_text = OverlayText() overlay_text.text = """ Left Analog: x/y L2/R2 : +-z L1/R1 : +-yaw Left/Right : +-roll Up/Down : +-pitch circle : Go cross : Reset/Cancel triangle : Clear maps and look around ground up/down : Move menu cursors """ overlay_text.width = 500 overlay_text.height = 500 overlay_text.text_size = 12 overlay_text.left = 10 overlay_text.top = 10 overlay_text.font = "Ubuntu Mono Regular" overlay_text.bg_color.a = 0 overlay_text.fg_color.r = 25 / 255.0 overlay_text.fg_color.g = 1 overlay_text.fg_color.b = 1 overlay_text.fg_color.a = 1 self.usage_pub.publish(overlay_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 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