def motorStatesCallback(msg): global g_publishers, g_text_publisher #values = msg.position values = msg.temperature names = msg.name allocatePublishers(len(values)) max_temparature = 0 max_temparature_name = "" for name, val, pub in zip(names, values, g_publishers): pub.publish(Float32(val)) if max_temparature < val: max_temparature = val max_temparature_name = name if max_temparature: if max_temparature > 70: color = fatal_color elif max_temparature > 50: color = warning_color else: color = safe_color text = OverlayText() text.fg_color.r = color[0] / 255.0 text.fg_color.g = color[1] / 255.0 text.fg_color.b = color[2] / 255.0 text.fg_color.a = 1.0 text.bg_color.a = 0.0 text.text = "%dC -- (%s)" % (int(max_temparature), max_temparature_name) g_text_publisher.publish(text)
def collision_callback(data): global num_collisions num_collisions = num_collisions + 1 has_collided = OverlayText(text=str('True')) collision_pub.publish(has_collided) text_collisions = OverlayText(text=str(num_collisions) + ' collision(s)') num_collision_pub.publish(text_collisions)
def callback(msg): text = OverlayText() if msg.contact_state == GroundContactState.CONTACT_BOTH_GROUND: text.text = "Double Stance Phase" text.fg_color.a = 1.0 text.fg_color.r = 25 / 255.0 text.fg_color.g = 1 text.fg_color.b = 1 elif msg.contact_state == GroundContactState.CONTACT_LLEG_GROUND or msg.contact_state == GroundContactState.CONTACT_RLEG_GROUND: text.text = "Single Stance Phase" text.fg_color.a = 1.0 text.fg_color.r = 1 text.fg_color.g = 0 text.fg_color.b = 0 elif msg.contact_state == GroundContactState.CONTACT_UNSTABLE: text.text = "Unstable" text.fg_color.a = 1.0 text.fg_color.r = 1 text.fg_color.g = 0 text.fg_color.b = 0 elif msg.contact_state == GroundContactState.CONTACT_AIR: text.text = "Air" text.fg_color.a = 1.0 text.fg_color.r = 1 text.fg_color.g = 0 text.fg_color.b = 0 pub.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 onReconfigure(self, worlds_names): """ """ #rospy.loginfo("reconfigure") text = OverlayText() text.action = 1 self.text_pub.publish(text) pass
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 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 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 callback(msg): for exclude_regex in exclude_regexes: if re.match(exclude_regex, msg.msg): return global lines if msg.name not in ignore_nodes: if msg.name in nodes or len(nodes) == 0: if len(nodes_regexp) == 0 or nodes_regexp_compiled.match(msg.name): if reverse_lines: lines = [colored_message(msg)] + lines if len(lines) > line_buffer_length: lines = lines[0:line_buffer_length] else: lines = lines + [colored_message(msg)] if len(lines) > line_buffer_length: lines = lines[-line_buffer_length:] text = OverlayText() text.left = 20 text.top = 20 text.width = 1200 text.height = 1200 text.fg_color.a = 1.0 text.fg_color.r = 0.3 text.text_size = 12 text.text = "\n".join(lines) pub.publish(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 onReconfigure(self, worlds_names): """ """ text = OverlayText() text.action = 1 for world_name in worlds_names: 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) pass
def main(self): # read QR code self.pb_rviz_msg.publish( OverlayText( text='Started reading QR code and get position of each bins.')) succeeded = self.cl_qrcode_reader() # Get item self.target_bin = 'bin_E' self.pb_rviz_msg.publish( OverlayText( text='Getting item in bin name: {0}.'.format(self.target_bin))) succeeded = self.cl_get_item() # Release item self.pb_rviz_msg.publish(OverlayText(text='Releasing item.')) self.cl_release_item() self.pb_rviz_msg.publish(OverlayText(text="baxter waiting"))
def __init__(self): self.sub_vehicle_status = rospy.Subscriber("/vehicle_status", VehicleStatus, self.cb_vehicle_status) self.msg_vehicle_status = VehicleStatus() # Obstacle status self.sub_detected_obstacle = rospy.Subscriber( "/detected_obstacles", DetectedObstacles, self.cb_detected_obstacles) self.msg_detected_obstacles = None self.pub_obstacle_text = rospy.Publisher("rei_vehicle_status/obstacle", OverlayText, queue_size=1) # Obstacle report self.msg_obstacle_text = OverlayText() self.msg_obstacle_text.text_size = 24 self.msg_obstacle_text.width = 800 self.msg_obstacle_text.height = 120 self.msg_obstacle_text.bg_color.a = 0.5 self.msg_obstacle_text.fg_color.r = 1.0 self.msg_obstacle_text.fg_color.g = 0.0 self.msg_obstacle_text.fg_color.b = 0.0 self.msg_obstacle_text.fg_color.a = 1.0 self.msg_obstacle_text.font = "Noto Mono" self.msg_obstacle_text.top = 500 # State report self.msg_vehicle_state_text = OverlayText() self.msg_vehicle_state_text.text_size = 14 self.msg_vehicle_state_text.width = 800 self.msg_vehicle_state_text.height = 100 self.msg_vehicle_state_text.bg_color.a = 0.5 self.msg_vehicle_state_text.font = "Noto Mono" self.pub_vehicle_state_text = rospy.Publisher( "rei_vehicle_status/state_text", OverlayText, queue_size=1) self.msg_vehicle_state_text.fg_color.r = 1.0 self.msg_vehicle_state_text.fg_color.g = 0.0 self.msg_vehicle_state_text.fg_color.b = 0.0 self.msg_vehicle_state_text.fg_color.a = 1.0 self.msg_vehicle_state_text.bg_color.a = 0.5 # State report self.msg_state_text = OverlayText() self.msg_state_text.text_size = 14 self.msg_state_text.width = 800 self.msg_state_text.height = 100 self.msg_state_text.bg_color.a = 0.5 self.msg_state_text.font = "Noto Mono"
def update_msg(self, topic, data): self._lock.acquire() if isinstance(data, str): t = OverlayText msg = OverlayText() msg.action = OverlayText.ADD msg.text = str(data) elif isinstance(data, Real): t = Float32 msg = Float32() msg.data = float(data) else: raise ValueError("invalid msg type") if not topic in self._pubs: self._pubs[topic] = PublisherWrapper(topic, t, self._publish_rate, self._queue_size) self._pubs[topic].update_msg(msg) self._lock.release()
def timer_callback(self, event): p = Popen(['rosnode', 'list'], stdout=PIPE) p.wait() nodelist_tmp = p.communicate() # nodelist_tmp: ('/pepper_1556630474468299832\n/rosout\n', None) nodelist = nodelist_tmp[0] # nodelist: '/pepper_1556630474468299832\n/rosout\n' nodelist = nodelist.split("\n") # nodelist: ['/pepper_1556630474468299832', '/rosout', ''] # If '/pepper_robot' node is killed, need to kill jsk_pepper_startup.launch by killing another required node (see https://github.com/jsk-ros-pkg/jsk_robot/issues/1077) if self.naoqi_driver_node_name in nodelist: pass else: call(['rosnode', 'kill', self.pose_controller_node_name]) tmp = [] add_camera_node = False for i in nodelist: # play_audio_stream_node is highlighted because this node needs to take care of people's privacy if i == '/play_audio_stream_node': i = """<span style="color: red;">""" + i + """</span>""" tmp.append(i) # display camera node as "/pepper_robot/camera" because there are a lot of camera nodes else: if 'camera' in i: if not add_camera_node: i = "/pepper_robot/camera" tmp.append(i) add_camera_node = True # ignore '' elif i is not '': tmp.append(i) text = OverlayText() text.left = 350 text.top = 0 text.width = 400 text.height = 300 text.fg_color.r = 25 / 255.0 text.fg_color.g = 255 / 255.0 text.fg_color.b = 255 / 255.0 text.fg_color.a = 0.8 text.bg_color.r = 0 text.bg_color.g = 0 text.bg_color.b = 0 text.bg_color.a = 0.8 text.text_size = 10 text.text = "\n".join(tmp) self.pub.publish(text)
def publishModeText(message='test', bg_color=[0.9, 0.9, 0.9, 0.1], fg_color=[0.2, 0.2, 0.2, 0.1]): send_text = OverlayText() send_text.text = message send_text.top = 50 send_text.left = 25 send_text.width = 1500 send_text.height = 200 send_text.bg_color.r, send_text.bg_color.g, send_text.bg_color.b, send_text.bg_color.a = bg_color send_text.fg_color.r, send_text.fg_color.g, send_text.fg_color.b, send_text.fg_color.a = fg_color #send_text.line_width = 1 send_text.text_size = 100 send_text_pub.publish(send_text)
def publishModeText(message='test', bg_color=[0.9,0.9,0.9,0.1], fg_color=[0.2,0.2,0.2,0.1]): send_text = OverlayText() send_text.text = message send_text.top = 50 send_text.left = 25 send_text.width = 1500 send_text.height = 200 send_text.bg_color.r,send_text.bg_color.g,send_text.bg_color.b,send_text.bg_color.a=bg_color send_text.fg_color.r,send_text.fg_color.g,send_text.fg_color.b,send_text.fg_color.a=fg_color #send_text.line_width = 1 send_text.text_size = 100 send_text_pub.publish(send_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 continuousDataUpdatedTimeCallnack(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 = "CONTINUOUS DATA: Not received yet" diff = 100 else: 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 text.bg_color = UPDATED_TIME_BG_COLOR pub_continuous_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) text.fg_color = OK_COLOR text.bg_color = UPDATED_TIME_BG_COLOR pub_lowspeed_data_updated_time.publish(text)
def publish(self, text): msg = OverlayText() msg.text = text msg.width = self.config.width msg.height = self.config.height msg.top = self.config.top msg.left = self.config.left msg.fg_color.a = self.config.fg_alpha msg.fg_color.r = self.config.fg_red msg.fg_color.g = self.config.fg_green msg.fg_color.b = self.config.fg_blue msg.bg_color.a = self.config.bg_alpha msg.bg_color.r = self.config.bg_red msg.bg_color.g = self.config.bg_green msg.bg_color.b = self.config.bg_blue msg.text_size = self.config.text_size self.pub.publish(msg)
def callback(msg): global pub count = msg.data text = OverlayText() color = (52, 152, 219) text.fg_color.r = color[0] / 255.0 text.fg_color.g = color[1] / 255.0 text.fg_color.b = color[2] / 255.0 text.fg_color.a = 1.0 text.bg_color.a = 0.0 text.text = "Samples: %d" % (count) text.width = 500 text.height = 100 text.left = 10 text.top = 10 text.text_size = 30 pub.publish(text)
def callback(event): target_label_value = rospy.get_param('~target_label_value', None) lines = [] for label_value, label_name in enumerate(label_names): color = 'green' if label_value == target_label_value else 'white' lines.append('<span style="color: {};">{}: {}</span>'.format( color, label_value, label_name)) text = OverlayText() text.left = 20 text.top = 20 text.width = 1200 text.height = 1200 text.fg_color.a = 1.0 text.fg_color.r = 0.3 text.text_size = 12 text.text = '\n'.join(lines) pub.publish(text)
def timer_callback(self, event): p=Popen(['rosnode','list'], stdout=PIPE) p.wait() nodelist_tmp=p.communicate() # nodelist_tmp: ('/pepper_1556630474468299832\n/rosout\n', None) nodelist=nodelist_tmp[0] # nodelist: '/pepper_1556630474468299832\n/rosout\n' nodelist=nodelist.split("\n") # nodelist: ['/pepper_1556630474468299832', '/rosout', ''] # If '/pepper_robot' node is killed, need to kill jsk_pepper_startup.launch by killing another required node (see https://github.com/jsk-ros-pkg/jsk_robot/issues/1077) if self.naoqi_driver_node_name in nodelist: pass else: call(['rosnode', 'kill', self.pose_controller_node_name]) tmp=[] add_camera_node = False for i in nodelist: # play_audio_stream_node is highlighted because this node needs to take care of people's privacy if i == '/play_audio_stream_node': i ="""<span style="color: red;">""" + i + """</span>""" tmp.append(i) # display camera node as "/pepper_robot/camera" because there are a lot of camera nodes else: if 'camera' in i: if not add_camera_node: i = "/pepper_robot/camera" tmp.append(i) add_camera_node = True # ignore '' elif i is not '': tmp.append(i) text = OverlayText() text.left = 350 text.top = 0 text.width = 400 text.height = 300 text.fg_color.r = 25/255.0 text.fg_color.g = 255/255.0 text.fg_color.b = 255/255.0 text.fg_color.a = 0.8 text.bg_color.r = 0 text.bg_color.g = 0 text.bg_color.b = 0 text.bg_color.a = 0.8 text.text_size = 10 text.text = "\n".join(tmp) self.pub.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 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 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 callback(msg): global lines if msg.name not in ignore_nodes: if msg.name in nodes or len(nodes) == 0: if len(nodes_regexp) == 0 or nodes_regexp_compiled.match(msg.name): lines = [colored_message(msg)] + lines if len(lines) > line_buffer_length: lines = lines[0:line_buffer_length] text = OverlayText() text.left = 20 text.top = 20 text.width = 1200 text.height = 1200 text.fg_color.a = 1.0 text.fg_color.r = 0.3 text.text_size = 12 text.text = "\n".join(lines) pub.publish(text)
def __init__(self): self.prompt_text = "HOTARU_TEB_PLANNER_NODE: " self.pub_ov_text = rospy.Publisher("/rei_planner_text_viz", OverlayText, queue_size=1) self.msg_text = OverlayText() self.msg_text.font = "Monospace" self.msg_text.text_size = 24 self.msg_text.width = 300 self.msg_text.bg_color.a = 0.4 self.msg_text.bg_color.r = 0.0 self.msg_text.bg_color.g = 0.0 self.msg_text.bg_color.b = 0.0 # self.msg_text.fg_color.a = 1.0 self.sub_visualizer = rospy.Subscriber( "/hotaru_planner/behav/sync_state_machine/current_state", ReiStateMachineTransitionSignal, self.cbTransitionSignal) self.pub_timer = rospy.Timer(rospy.Duration(0.1), self.cbTimerPublishText)
def __init__(self, mySide='r'): self.mySide = mySide self.map = tmp_targetsMap.getTargetsMap() self.goalPub = rospy.Publisher('/move_base_simple/goal', PoseStamped, queue_size=5) self.myStatePub = rospy.Publisher('my_state', OverlayText, queue_size=5) self.warStateSub = rospy.Subscriber('/war_state', war_state, self.warStateCallback) self.navStateSub = rospy.Subscriber('/move_base/status', GoalStatusArray, self.navStateCallback) self.cvRectSub = rospy.Subscriber('/cv_rect', CvRect, self.cvRectSubCallback) self.twist_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1000) self.laserSub = rospy.Subscriber('/scan', LaserScan, self.laserCallback) self.war_state = war_state() self.war_state.target_names = [] self.war_state.target_owner = [] self.war_state.target_point = [] self.range_data = 0.0 self.my_state_text = OverlayText() self.my_state_text.text = "" self.r = 10.0 # rospy frequency self.isFoundEnemy = False self.isFoundEnemyTarget = False self.rectData = CvRect() self.navStatus = "" # Parameters of PID controller self.Kp = 0.005 self.Ki = 0.0 self.Kd = 0.00005 self.integral = 0.0 self.error = 0.0 self.error_pre = 0.0 self.dt = 1.0/self.r self.target_001 = 320.0 # Parameters of DIST self.threshold_r = 50.0 # threshold of red area width self.threshold_g = 30.0 # threshold of green area width
def __init__(self, hz, lookahead_points=40, global_frame="map", robot_frame="base_link", lane_width=3.0, skip_points=3, lethal_obstacle_val=250.0, non_lethal_val_scale=20.0, sigma_x = 3.5, sigma_y = 3.0, visualize=False): # Locks self.planner_lock = Lock() self.obstacle_lock = Lock() # TF self.global_frame = global_frame self.robot_frame = robot_frame self.tf_buffer = tf2_ros.Buffer() self.tf_listener = tf2_ros.TransformListener(self.tf_buffer) self.trans = None self.trans_global = None self.skip_points = skip_points # self.pub_final_trajectory = rospy.Publisher("/final_waypoints", Lane, queue_size=1) # State self.cnt_obstacles = 0 self.lanes = None self.poly_vertices = None self.current_pose = None self.closest_waypoint = None self.current_wps = [] self.current_cvs = [] self.cvs = [] self.current_cvs_2d = None self.visualize = visualize self.hz = hz self.planning_point = 0 self.feasible_plan = False # Obstacle self.prev_obstacle_event = rospy.Time.now() self.states = {"RELAY": 0, "REPLANNING": 1, "AVOID": 2} self.ref_lane_length = 0 self.current_state = self.states["RELAY"] # Store trajectories self.coarse_trajectory = None self.final_trajectory = None self.final_lane = Lane() self.obstacle_list = [] # Subscribers # TODO: Hybrid state machine self.lane_polygon = DynamicLanePolygon(lane_width, 7, 15, lethal_val_scale=lethal_obstacle_val, non_lethal_val_scale=non_lethal_val_scale, sigma_x=sigma_x, sigma_y=sigma_y) self.lookahead_points = lookahead_points self.planner = DynamicLaneAstarPlanner(None, self.lane_polygon) self.interpolator_final_waypoints = RationalBezierCurve() self.sub_current_pose = rospy.Subscriber("/current_pose", PoseStamped, self.cb_current_pose) self.sub_current_lane = rospy.Subscriber("/base_waypoints", Lane, self.cb_lane_waypoints) self.sub_closest_waypoint = rospy.Subscriber("/closest_waypoint", Int32, self.cb_closest_waypoint) # self.sub_obstacles = rospy.Subscriber("/detected_obstacles", DetectedObstacles, self.cb_obstacle_detection) # Obstacle deque self.obstacle_deque = deque() # State report self.msg_state_text = OverlayText() self.msg_state_text.text_size = 14 self.msg_state_text.width = 800 self.msg_state_text.height = 100 self.msg_state_text.bg_color.a = 0.5 self.msg_state_text.font = "Noto Mono" self.pub_state_text = rospy.Publisher("hotaru/state_text", OverlayText, queue_size=1) # Handle visualization if visualize: # Visualize selected trajectory self.pub_viz_selected_trajectory_points = rospy.Publisher("/trajectory_lookahead/visualization", MarkerArray, queue_size=1) self.viz_markers = MarkerArray() self.viz_marker = Marker() self.viz_marker.header.frame_id = self.robot_frame self.viz_marker.type = Marker.LINE_STRIP self.viz_marker.color.r = 1.0 self.viz_marker.color.g = 1.0 self.viz_marker.color.a = 0.5 self.viz_marker.scale.x = 0.6 # Visualize current lane self.pub_viz_current_lane_point = rospy.Publisher("/current_lane/visualization", Marker, queue_size=1) self.viz_current_lane_marker = Marker() self.viz_current_lane_marker.type = Marker.SPHERE self.viz_current_lane_marker.color.r = 1.0 self.viz_current_lane_marker.color.b = 1.0 self.viz_current_lane_marker.color.a = 0.4 self.viz_current_lane_marker.scale.x = 0.5 self.viz_current_lane_marker.scale.y = 0.5 self.viz_current_lane_marker.scale.z = 0.5 self.viz_current_lane_marker.header.frame_id = self.robot_frame # Final trajectory self.pub_viz_final_trajectory = rospy.Publisher("/final_trajectory/visualization", MarkerArray, queue_size=1) self.viz_marker_array_final_trajectory = MarkerArray() # Visualize orientation self.viz_marker_orientation_field = Marker() self.viz_marker_orientation_field.type = Marker.LINE_LIST self.viz_marker_orientation_field.ns = "orientation_field" self.viz_marker_orientation_field.color.r = 0.2 self.viz_marker_orientation_field.color.g = 0.9 self.viz_marker_orientation_field.color.b = 0.4 self.viz_marker_orientation_field.color.a = 0.95 self.viz_marker_orientation_field.scale.x = 0.4 self.viz_marker_orientation_field.header.frame_id = self.global_frame self.viz_marker_array_final_trajectory.markers.append(self.viz_marker_orientation_field) # Tesselation visualization self.viz_marker_tesselation = MarkerArray() # Visualize tesselation grid self.pub_viz_tesselation_points = rospy.Publisher("/tesselation_grid/visualization", MarkerArray, queue_size=1)
rospy.init_node("tf_diff") src1 = rospy.get_param("~src1") src2 = rospy.get_param("~src2") 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:
def callback(self, front_MarkerArray, back_MarkerArray, TwistStamped): # print("front",len(front_MarkerArray.markers)/4) # print("back",len(back_MarkerArray.markers)/4) # # Concat front and back MarkerArray Messages add_MarkerArray = copy.deepcopy(front_MarkerArray) for i in range(len(back_MarkerArray.markers)): add_MarkerArray.markers.append(back_MarkerArray.markers[i]) # print("add",len(add_MarkerArray.markers)/4) # print("done") if len(add_MarkerArray.markers) == 0: return header = add_MarkerArray.markers[0].header frame = header.seq boxes = BoundingBoxArray() #3D Boxes with JSK boxes.header = header texts = PictogramArray() #Labels with JSK texts.header = header obj_ori_arrows = MarkerArray() #arrow with visualization_msgs velocity_markers = MarkerArray() #text with visualization_msgs obj_path_markers = MarkerArray() # passed path warning_line_markers = MarkerArray() dets = np.zeros((0, 9)) # (None, 9) : 9는 사용할 3d bbox의 파라미터 개수 obj_box_info = np.empty((0, 7)) obj_label_info = np.empty((0, 2)) # frame을 rviz에 출력 overlayTxt = OverlayText() overlayTxt.left = 10 overlayTxt.top = 10 overlayTxt.width = 1200 overlayTxt.height = 1200 overlayTxt.fg_color.a = 1.0 overlayTxt.fg_color.r = 1.0 overlayTxt.fg_color.g = 1.0 overlayTxt.fg_color.b = 1.0 overlayTxt.text_size = 12 overlayTxt.text = "Frame_seq : {}".format(frame) det_boxes = BoundingBoxArray() #3D Boxes with JSK det_boxes.header = header # Receive each objects info in this frame for object_info in add_MarkerArray.markers: #extract info [ frame,type(label),tx,ty,tz,h,w,l,ry ] if object_info.ns == "/detection/lidar_detector/box_markers": tx = object_info.pose.position.x ty = object_info.pose.position.y tz = object_info.pose.position.z l = object_info.scale.x w = object_info.scale.y h = object_info.scale.z quaternion_xyzw = [object_info.pose.orientation.x, object_info.pose.orientation.y, \ object_info.pose.orientation.z, object_info.pose.orientation.w] rz = tf.transformations.euler_from_quaternion( quaternion_xyzw)[2] obj_box_info = np.append( obj_box_info, [[-ty, -tz, tx - 0.27, h, w, l, -rz + np.pi / 2]], axis=0) size_det = Vector3(l, w, h) det_box = BoundingBox() det_box.header = header det_box.pose.position = Point(tx, ty, tz) q_det_box = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz) # 어쩔 수 없이 끝단에서만 90도 돌림 det_box.pose.orientation = Quaternion(*q_det_box) det_box.dimensions = size_det det_boxes.boxes.append(det_box) elif object_info.ns == "/detection/lidar_detector/label_markers": label = object_info.text.strip() if label == '': label = 'None' obj_label_info = np.append(obj_label_info, [[frame, label]], axis=0) dets = np.concatenate((obj_label_info, obj_box_info), axis=1) self.pub_det_markerarray.publish(det_boxes) del current_id_list[:] # All Detection Info in one Frame bboxinfo = dets[dets[:, 0] == str(frame), 2:9] # [ tx, ty, tz, h, w, l, rz ] additional_info = dets[dets[:, 0] == str(frame), 0:2] # frame, labe reorder = [3, 4, 5, 0, 1, 2, 6] # [tx,ty,tz,h,w,l,ry] -> [h,w,l,tx,ty,tz,theta] reorder_back = [3, 4, 5, 0, 1, 2, 6] # [h,w,l,tx,ty,tz,theta] -> [tx,ty,tz,h,w,l,ry] reorder2velo = [2, 0, 1, 3, 4, 5, 6] bboxinfo = bboxinfo[:, reorder] # reorder bboxinfo parameter [h,w,l,x,y,z,theta] bboxinfo = bboxinfo.astype(np.float64) dets_all = {'dets': bboxinfo, 'info': additional_info} # ObjectTracking from Detection trackers = self.mot_tracker.update(dets_all) # h,w,l,x,y,z,theta trackers_bbox = trackers[:, 0:7] trackers_info = trackers[:, 7:10] # id, frame, label trackers_bbox = trackers_bbox[:, reorder_back] # reorder_back bboxinfo parameter [tx,ty,tz,h,w,l,ry] trackers_bbox = trackers_bbox[:, reorder2velo] # reorder coordinate system cam to velo trackers_bbox = trackers_bbox.astype(np.float64) trackers_bbox[:, 0] = trackers_bbox[:, 0] trackers_bbox[:, 1] = trackers_bbox[:, 1] * -1 trackers_bbox[:, 2] = trackers_bbox[:, 2] * -1 trackers_bbox[:, 6] = trackers_bbox[:, 6] * -1 # for문을 통해 각 objects들의 정보를 추출하여 사용 for b, info in zip(trackers_bbox, trackers_info): bbox = BoundingBox() bbox.header = header # parameter 뽑기 [tx,ty,tz,h,w,l,rz] tx_trk, ty_trk, tz_trk = float(b[0]), float(b[1]), float(b[2]) rz_trk = float(b[6]) size_trk = Vector3(float(b[5]), float(b[4]), float(b[3])) obj_id = info[0] label_trk = info[2] bbox_color = colorCategory20(int(obj_id)) odom_mat = get_odom(self.tf2, "velo_link", "map") xyz = np.array(b[:3]).reshape(1, -1) points = np.array((0, 3), float) if odom_mat is not None: points = get_transformation(odom_mat, xyz) # 이전 x frame 까지 지나온 points들을 저장하여 반환하는 함수 # obj_id와 bbox.label은 단지 type차이만 날뿐 같은 데이터 # path_points_list = points_path(tx_trk, ty_trk, tz_trk, obj_id) path_points_list = points_path(points[0, 0], points[0, 1], points[0, 2], obj_id) map_header = copy.deepcopy(header) map_header.frame_id = "/map" bbox_color = colorCategory20(int(obj_id)) path_marker = Marker( type=Marker.LINE_STRIP, id=int(obj_id), lifetime=rospy.Duration(0.5), # pose=Pose(Point(0,0,0), Quaternion(0, 0, 0, 1)), # origin point position scale=Vector3(0.1, 0.0, 0.0), # line width header=map_header, color=bbox_color) path_marker.points = path_points_list obj_path_markers.markers.append(path_marker) # Tracker들의 BoundingBoxArray 설정 bbox.pose.position = Point(tx_trk, ty_trk, tz_trk / 2.0) q_box = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림 bbox.pose.orientation = Quaternion(*q_box) bbox.dimensions = size_trk bbox.label = int(obj_id) boxes.boxes.append(bbox) picto_text = Pictogram() picto_text.header = header picto_text.mode = Pictogram.STRING_MODE picto_text.pose.position = Point(tx_trk, ty_trk, -tz_trk) # q = tf.transformations.quaternion_from_euler(0.7, 0.0, -0.7) picto_text.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5) picto_text.size = 4 picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1) picto_text.character = label_trk + ' ' + str(bbox.label) texts.pictograms.append(picto_text) # GPS sensor values oxtLinear = TwistStamped.twist.linear # oxtLinear = TwistStamped.twist.linear # Tracker들의 속도 추정 obj_velo, dx_t, dy_t, dz_t = obj_velocity([tx_trk, ty_trk, tz_trk], bbox.label, oxtLinear) if obj_velo != None: obj_velo = np.round_(obj_velo, 1) # m/s obj_velo = obj_velo * 3.6 # km/h obj_velo_scale = convert_velo2scale(obj_velo) # # Tracker들의 Orientation q_ori = tf.transformations.quaternion_from_euler( 0.0, 0.0, rz_trk + np.pi / 2) # 어쩔 수 없이 끝단에서만 90도 돌림 obj_ori_arrow = Marker( type=Marker.ARROW, id=bbox.label, lifetime=rospy.Duration(0.2), pose=Pose(Point(tx_trk, ty_trk, tz_trk / 2.0), Quaternion(*q_ori)), scale=Vector3(obj_velo_scale, 0.5, 0.5), header=header, # color=ColorRGBA(0.0, 1.0, 0.0, 0.8)) color=bbox_color) obj_ori_arrows.markers.append(obj_ori_arrow) obj_velo_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=bbox.label, lifetime=rospy.Duration(0.5), pose=Pose(Point(tx_trk, ty_trk, tz_trk), Quaternion(0.0, -0.5, 0.0, 0.5)), scale=Vector3(1.5, 1.5, 1.5), header=header, color=ColorRGBA(1.0, 1.0, 1.0, 1.0), text="{}km/h".format(obj_velo)) velocity_markers.markers.append(obj_velo_marker) current_id_list.append(bbox.label) # Warning object line warning_line = Marker( type=Marker.LINE_LIST, id=int(obj_id), lifetime=rospy.Duration(0.2), pose=Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1)), # origin point position scale=Vector3(0.2, 0.0, 0.0), # line width header=header, color=ColorRGBA(1.0, 0.0, 0.0, 1.0)) d = dist_from_objBbox(tx_trk, ty_trk, tz_trk, size_trk.x, size_trk.y, size_trk.z) if d < MIN_WARNING_DIST: warning_line.points = Point(tx_trk, ty_trk, tz_trk), Point(0.0, 0.0, 0.0) warning_line_markers.markers.append(warning_line) # Change Outer Circle Color outer_circle_color = ColorRGBA(1.0 * 25 / 255, 1.0, 0.0, 1.0) if len(warning_line_markers.markers) > 0: outer_circle_color = ColorRGBA(1.0 * 255 / 255, 1.0 * 0 / 255, 1.0 * 0 / 255, 1.0) # ego_vehicle's warning boundary outer_circle = Marker( type=Marker.CYLINDER, id=int(obj_id), lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -2.0), Quaternion(0, 0, 0, 1)), scale=Vector3(8.0, 8.0, 0.1), # line width header=header, color=outer_circle_color) inner_circle = Marker( type=Marker.CYLINDER, id=int(obj_id), lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -1.8), Quaternion(0, 0, 0, 1)), scale=Vector3(7.0, 7.0, 0.2), # line width header=header, color=ColorRGBA(0.22, 0.22, 0.22, 1.0)) # ego-vehicle velocity selfvelo = np.sqrt(oxtLinear.x**2 + oxtLinear.y**2 + oxtLinear.z**2) selfvelo = np.round_(selfvelo, 1) # m/s selfvelo = selfvelo * 3.6 # km/h oxtAngular = TwistStamped.twist.angular q_gps = tf.transformations.quaternion_from_euler( oxtAngular.x, oxtAngular.y, oxtAngular.z) # # ego-vehicle 사진 출력 ego_car = Marker(type=Marker.MESH_RESOURCE, id=0, lifetime=rospy.Duration(0.5), pose=Pose(Point(0.0, 0.0, -1.8), Quaternion(0, 0, 0, 1)), scale=Vector3(1.5, 1.5, 1.5), header=header, action=Marker.ADD, mesh_resource=CAR_DAE_PATH, color=ColorRGBA(1.0, 1.0, 1.0, 1.0)) # Self ego Velocity text_marker = Marker(type=Marker.TEXT_VIEW_FACING, id=0, lifetime=rospy.Duration(0.5), pose=Pose(Point(-7.0, 0.0, 0.0), Quaternion(0, 0, 0, 1)), scale=Vector3(1.5, 1.5, 1.5), header=header, color=ColorRGBA(1.0, 1.0, 1.0, 1.0), text="{}km/h".format(selfvelo)) for i in prior_trk_xyz.keys(): if i not in current_id_list: prior_trk_xyz.pop(i) self.pub_frame_seq.publish(overlayTxt) self.pub_boxes.publish(boxes) self.pub_pictograms.publish(texts) self.pub_selfvelo_text.publish(text_marker) # self.pub_selfveloDirection.publish(arrow_marker) self.pub_objs_ori.publish(obj_ori_arrows) self.pub_objs_velo.publish(velocity_markers) self.pub_path.publish(obj_path_markers) self.pub_warning_lines.publish(warning_line_markers) self.pub_ego_outCircle.publish(outer_circle) self.pub_ego_innerCircle.publish(inner_circle) self.pub_ego_car.publish(ego_car)
def disable(self): msg = OverlayText() msg.action = OverlayText.DELETE self.pub.publish(msg)
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 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)
rospy.init_node("visualize_on_rviz") json_file = rospy.get_param("~json") bin_contents = dict(list(get_bin_contents(json_file))) work_order = dict(list(get_work_order(json_file))) pub_l = rospy.Publisher("~left", OverlayText, queue_size=1) pub_r = rospy.Publisher("~right", OverlayText, queue_size=1) rate = rospy.Rate(10) while not rospy.is_shutdown(): msg = {} for arm in ["left", "right"]: msg[arm] = OverlayText() # robot state state_text = rospy.get_param(arm + "_hand/state", "") target_bin = rospy.get_param(arm + "_hand/target_bin", "") target_text = target_bin # contents and order contents_text = "" order_text = "" if target_bin: contents = bin_contents[target_bin] contents_text = "objects in bin '{}': {}".format(target_bin.upper(), ", ".join(contents)) order = work_order[target_bin] order_text = order # recognition result result_text = "" if recognition_msg[arm] is not None:
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 publish_txt_info(self, txt): msg = OverlayText() msg.text = txt self.text_publisher.publish(msg)
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