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 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 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 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 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 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 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 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 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(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 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 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 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 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 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)
elif self.state_machine_ == self.FINISH_STATE_: pass #end state machine #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()
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 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)
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: rospy.logerr("ignore error") finally:
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
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)))
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: rospy.logerr("ignore error") finally: