def readXML(file): tree = ET.parse(file) root = tree.getroot() item = root.findall('./tracklets/item') bboxs = {} pictograms = {} arrows = {} paths = {} for i, v in enumerate(item): h = float(v.find('h').text) w = float(v.find('w').text) l = float(v.find('l').text) frame = int(v.find('first_frame').text) size = Vector3(l, w, h) label = v.find('objectType').text track_id = int(v.find('trackid').text) pose = v.findall('./poses/item') for j, p in enumerate(pose): tx = float(p.find('tx').text) ty = float(p.find('ty').text) tz = float(p.find('tz').text) + 0.85 rz = float(p.find('rz').text) q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz) bbox = BoundingBox() picto = Pictogram() arrow = Marker() bbox = viz_bbox(Vector3(tx, ty, tz), q, size, track_id) picto = viz_picto(Vector3(tx, ty, -tz / 2.0), q, label) arrow = viz_arrow(Vector3(tx, ty, tz), q, track_id) if bboxs.has_key(frame + j) == True: bboxs[frame + j].append(bbox) pictograms[frame + j].append(picto) arrows[frame + j].append(arrow) else: bboxs[frame + j] = [bbox] pictograms[frame + j] = [picto] arrows[frame + j] = [arrow] return bboxs, pictograms, arrows
def viz_picto(position, q, label): picto = Pictogram() picto.mode = Pictogram.STRING_MODE picto.pose.position = position picto.pose.orientation = Quaternion(0.0, -0.5, 0.0, 0.5) picto.size = 3 picto.color = ColorRGBA(1, 1, 1, 1) picto.character = label return picto
def readXML(file): tree = ET.parse(file) root = tree.getroot() item = root.findall('./tracklets/item') d = {} pictograms = {} for i, v in enumerate(item): h = float(v.find('h').text) w = float(v.find('w').text) l = float(v.find('l').text) frame = int(v.find('first_frame').text) size = Vector3(l, w, h) label = v.find('objectType').text pose = v.findall('./poses/item') for j, p in enumerate(pose): tx = float(p.find('tx').text) ty = float(p.find('ty').text) tz = float(p.find('tz').text) rz = float(p.find('rz').text) q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz) b = BoundingBox() b.pose.position = Vector3(tx, ty, tz/2.0) b.pose.orientation = Quaternion(*q) b.dimensions = size b.label = i picto_text = Pictogram() picto_text.mode = Pictogram.STRING_MODE picto_text.pose.position = Vector3(tx, ty, -tz/2.0) 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 = 1 picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1) picto_text.character = label if d.has_key(frame + j) == True: d[frame + j].append(b) pictograms[frame + j].append(picto_text) else: d[frame + j] = [b] pictograms[frame + j] = [picto_text] return d, pictograms
def make_pictogram(character, position, frame_id='/map', duration=0.5, color=[1.0, 1.0, 1.0]): """ Helper function for generating visualization markers Args: character (str): Character (icon) to be displayed. position (list): List containing [x,y,z] positions frame_id (str): ROS TF frame id duration (rospy.Duration): How long the label will be displayed for color (list): List of label color floats from 0 to 1 [r,g,b] Returns: Pictogram: A jsk_rviz_plugins/Pictogram message which can be published to RViz """ msg = Pictogram() msg.action = Pictogram.ADD msg.header.frame_id = frame_id msg.header.stamp = rospy.Time.now() msg.mode = Pictogram.PICTOGRAM_MODE msg.character = character msg.speed = 1.0 msg.ttl = duration msg.size = 3 msg.color.r = color[0] msg.color.g = color[1] msg.color.b = color[2] msg.color.a = 1.0 msg.pose.orientation.x = 0.0 msg.pose.orientation.y = -1.0 msg.pose.orientation.z = 0.0 msg.pose.orientation.w = 1.0 msg.pose.position.x = position[0] msg.pose.position.y = position[1] msg.pose.position.z = position[2] return msg
def make_jsk_text_pub(self, name, x, y): qua = Quaternion() qua = self.euler_to_quaternion(Vector3(0.0, -1.57, 0.0)) print qua jsk_text_msg = Pictogram() jsk_text_msg.header.stamp = rospy.Time.now() jsk_text_msg.header.frame_id = "map" jsk_text_msg.pose.position.x = x jsk_text_msg.pose.position.y = y jsk_text_msg.pose.position.z = 1.5 jsk_text_msg.pose.orientation.x = qua.x jsk_text_msg.pose.orientation.y = qua.y jsk_text_msg.pose.orientation.z = qua.z jsk_text_msg.pose.orientation.w = qua.w jsk_text_msg.character = name jsk_text_msg.mode = 1 jsk_text_msg.color.b = 1.0 jsk_text_msg.color.g = 1.0 jsk_text_msg.color.a = 1.0 jsk_text_msg.action = 4 jsk_text_msg.speed = 0.3 self.jsk_text_pub.publish(jsk_text_msg)
"fa-toggle-up", "fa-trash", "fa-trash-o", "fa-tree", "fa-trello", "fa-trophy", "fa-truck", "fa-try", "fa-tty", "fa-tumblr", "fa-tumblr-square", "fa-turkish-lira", "fa-twitch", "fa-twitter", "fa-twitter-square", "fa-umbrella", "fa-underline", "fa-undo", "fa-university", "fa-unlink", "fa-unlock", "fa-unlock-alt", "fa-unsorted", "fa-upload", "fa-usd", "fa-user", "fa-user-md", "fa-users", "fa-video-camera", "fa-vimeo-square", "fa-vine", "fa-vk", "fa-volume-down", "fa-volume-off", "fa-volume-up", "fa-warning", "fa-wechat", "fa-weibo", "fa-weixin", "fa-wheelchair", "fa-wifi", "fa-windows", "fa-won", "fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo", "fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square" ] counter = 0 while not rospy.is_shutdown(): msg = Pictogram() msg.action = Pictogram.JUMP_ONCE msg.header.frame_id = "/base_link" msg.header.stamp = rospy.Time.now() msg.pose.position.z = 1.6 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.mode = Pictogram.PICTOGRAM_MODE msg.speed = 1.0 # msg.ttl = 5.0 msg.size = 1 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0
"fa-windows", "fa-won", "fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo", "fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square"] counter = 0 while not rospy.is_shutdown(): msg = Pictogram() msg.action = Pictogram.JUMP_ONCE msg.header.frame_id = "/base_link" msg.header.stamp = rospy.Time.now() msg.pose.position.z = 1.6 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.size = 0.5 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0 msg.color.a = 1.0 msg.character = pictograms[counter] p.publish(msg)
def callback(ref, act): "msgs = ContactStatesStamped" if len(ref_contact_states_queue) > buffer_size - 1: arr = PictogramArray() arr.header.frame_id = "/odom" arr.header.stamp = rospy.Time.now() for i, (ref_st, act_st) in enumerate(zip(ref.states, act.states)): picto = Pictogram() if ref_st.state.state == act_st.state.state: continue else: if [ref_st.state.state, act_st.state.state ] == [ContactState.OFF, ContactState.ON]: if [ x.states[i].state.state for x in ref_contact_states_queue ] == [ContactState.OFF] * buffer_size and [ x.states[i].state.state for x in act_contact_states_queue ] == [ContactState.OFF] * buffer_size: picto.character = "fa-long-arrow-down" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8) rospy.loginfo("%s early landing %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time)) elif [ x.states[i].state.state for x in ref_contact_states_queue ] == [ContactState.ON] * buffer_size and [ x.states[i].state.state for x in act_contact_states_queue ] == [ContactState.ON] * buffer_size: picto.character = "up-bold" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8) rospy.loginfo("%s late taking off", ref_st.header.frame_id) print("oso hanare") else: continue elif [ref_st.state.state, act_st.state.state ] == [ContactState.ON, ContactState.OFF]: if [ x.states[i].state.state for x in ref_contact_states_queue ] == [ContactState.OFF] * buffer_size and [ x.states[i].state.state for x in act_contact_states_queue ] == [ContactState.OFF] * buffer_size: picto.character = "fa-long-arrow-down" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8) rospy.loginfo("%s late landing", ref_st.header.frame_id) elif [ x.states[i].state.state for x in ref_contact_states_queue ] == [ContactState.ON] * buffer_size and [ x.states[i].state.state for x in act_contact_states_queue ] == [ContactState.ON] * buffer_size: picto.character = "up-bold" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8) rospy.loginfo("%s early taking off %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time)) else: continue picto.header.frame_id = ref_st.header.frame_id picto.header.stamp = ref_st.header.stamp arr.pictograms.append(picto) if len(arr.pictograms) > 0: pub.publish(arr) ref_contact_states_queue.pop(0) act_contact_states_queue.pop(0) ref_contact_states_queue.append(ref) act_contact_states_queue.append(act)
"fa-windows", "fa-won", "fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo", "fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square"] counter = 0 while not rospy.is_shutdown(): msg = Pictogram() msg.action = Pictogram.JUMP_ONCE msg.header.frame_id = "/base_link" msg.header.stamp = rospy.Time.now() msg.pose.position.z = 1.6 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.mode = Pictogram.PICTOGRAM_MODE msg.speed = 1.0 # msg.ttl = 5.0 msg.size = 1 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0
"fa-sort-numeric-down-alt", "fa-sort-numeric-up-alt", "fa-spell-check", "fa-voicemail", "fa-cotton-bureau", "fa-buy-n-large", "fa-hat-cowboy", "fa-hat-cowboy-side", "fa-mdb", "fa-mouse", "fa-orcid", "fa-record-vinyl", "fa-swift", "fa-umbraco", "fa-caravan", "fa-firefox-browser", "fa-ideal", "fa-microblog", "fa-pied-piper-square", "fa-trailer", "fa-unity" ] counter = 0 while not rospy.is_shutdown(): initial_x = -int(math.sqrt(len(pictograms))) / 2 arr = PictogramArray() arr.header.frame_id = "base_link" arr.header.stamp = rospy.Time.now() prev_xyz = [initial_x, -10, 0] for character in pictograms: msg = Pictogram() msg.header.frame_id = "base_link" msg.action = choice(actions) msg.header.stamp = rospy.Time.now() msg.pose.position.x = prev_xyz[0] + 1 msg.pose.position.y = prev_xyz[1] msg.pose.position.z = 0 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.size = 1 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0 msg.color.a = 1.0
def callback(ref, act): "msgs = ContactStatesStamped" if len(ref_contact_states_queue) > buffer_size - 1: arr = PictogramArray() arr.header.frame_id = "/odom" arr.header.stamp = rospy.Time.now() for i, (ref_st, act_st) in enumerate(zip(ref.states, act.states)): picto = Pictogram() if ref_st.state.state == act_st.state.state: continue else: if [ref_st.state.state, act_st.state.state] == [ContactState.OFF, ContactState.ON]: if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size: picto.character = "fa-long-arrow-down" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8) rospy.loginfo("%s early landing %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time)) elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size: picto.character = "up-bold" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8) rospy.loginfo("%s late taking off", ref_st.header.frame_id) print "oso hanare" else: continue elif [ref_st.state.state, act_st.state.state] == [ContactState.ON, ContactState.OFF]: if [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.OFF] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.OFF] * buffer_size: picto.character = "fa-long-arrow-down" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(0.0, 1.0, 0.0, 0.8) rospy.loginfo("%s late landing", ref_st.header.frame_id) elif [x.states[i].state.state for x in ref_contact_states_queue] == [ContactState.ON] * buffer_size and [x.states[i].state.state for x in act_contact_states_queue] == [ContactState.ON] * buffer_size: picto.character = "up-bold" picto.size = 1 picto.pose.orientation = Quaternion(0, -1, 0, 1) picto.action = Pictogram.ADD picto.color = ColorRGBA(1.0, 0.0, 0.0, 0.8) rospy.loginfo("%s early taking off %s [s]", ref_st.header.frame_id, str(ref_st.state.remaining_time)) else: continue picto.header.frame_id = ref_st.header.frame_id picto.header.stamp = ref_st.header.stamp arr.pictograms.append(picto) if len(arr.pictograms) > 0: pub.publish(arr) ref_contact_states_queue.pop(0) act_contact_states_queue.pop(0) ref_contact_states_queue.append(ref) act_contact_states_queue.append(act)
[0, 0, 0, ' ', False], # 30 [-0.850, -1.000, 0, 'P', True], # 31 [0, 0, 0, ' ', False], # 32 [1.130, -0.200, pi / 2, 'Q', True] # 33 ] p = 0 p_old = 1 posn = 0 move0 = {'theta': 0.0, 'phi': 0.0, 'r': 1.0} move1 = {'x': 0.0, 'y': 0.0, 'z': 0.0} old_adrs = 99 pp = Pose() pict = Pictogram() auto_cnt = 0 auto_p = 0 autof = resetf = False auto_focus_point = Point(0, 0, 0) adrsf = False auto = True adrs = True rate_float = 2 def callback(message): global move0, move1
"fa-yahoo", "fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square"] counter = 0 while not rospy.is_shutdown(): initial_x = -int(math.sqrt(len(pictograms)))/2 arr = PictogramArray() arr.header.frame_id = "/base_link" arr.header.stamp = rospy.Time.now() prev_xyz = [initial_x, -10, 0] for character in pictograms: msg = Pictogram() msg.header.frame_id = "/base_link" msg.action = choice(actions) msg.header.stamp = rospy.Time.now() msg.pose.position.x = prev_xyz[0] + 1 msg.pose.position.y = prev_xyz[1] msg.pose.position.z = 0 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.size = 1 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0 msg.color.a = 1.0
def callback(data): PictArr.header = data.header PictArr.pictograms = [] for box in data.boxes: msg = Pictogram() msg_string = Pictogram() msg.action = Pictogram.JUMP msg.speed = 0.3 msg.header.frame_id = box.header.frame_id msg.pose.position = box.pose.position msg.pose.position.y -= 0.1 msg.pose.position.z -= 0.1 msg.pose.orientation.w = 0 msg.pose.orientation.x = 0.7 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.header.stamp = rospy.Time.now() msg.mode = Pictogram.PICTOGRAM_MODE msg.size = 0.05 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0 msg.color.a = 1.0 msg.character = "chevron-thin-down" PictArr.pictograms.append(msg) msg_string = copy.deepcopy(msg) msg_string.pose.position.y -= 0.05 msg_string.mode = Pictogram.STRING_MODE msg_string.size = 0.05 msg_string.character = "box found" PictArr.pictograms.append(msg_string) p.publish(PictArr)
def readXML(file): tree = ET.parse(file) root = tree.getroot() item = root.findall('./tracklets/item') d = {} boxes_2d = {} pictograms = {} for i, v in enumerate(item): h = float(v.find('h').text) w = float(v.find('w').text) l = float(v.find('l').text) frame = int(v.find('first_frame').text) size = Vector3(l, w, h) label = v.find('objectType').text pose = v.findall('./poses/item') for j, p in enumerate(pose): tx = float(p.find('tx').text) ty = float(p.find('ty').text) tz = float(p.find('tz').text) rz = float(p.find('rz').text) occlusion = float(p.find('occlusion').text) q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz) b = BoundingBox() b.pose.position = Vector3(tx, ty, tz/2.0) b.pose.orientation = Quaternion(*q) b.dimensions = size b.label = i picto_text = Pictogram() picto_text.mode = Pictogram.STRING_MODE picto_text.pose.position = Vector3(tx, ty, -tz/2.0) 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 = 5 picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1) picto_text.character = label # Bounding Box corners corner_x = np.array([l/2, l/2, -l/2, -l/2, l/2, l/2, -l/2, -l/2]) corner_y = np.array([w/2, -w/2, -w/2, w/2, w/2, -w/2, -w/2, w/2]) corner_z = np.array([0, 0, 0, 0, h, h, h, h]) rz = wrapToPi(rz) ################### #create box on origin, then translate and rotate according to pose. finally, project into 2D image # Rotate and translate 3D bounding box in velodyne coordinate system R = np.array([ [math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]]) corner_3d = np.dot(R,np.array([corner_x, corner_y, corner_z])) #Translate corner_3d[0,:] = corner_3d[0,:] + tx corner_3d[1,:] = corner_3d[1,:] + ty corner_3d[2,:] = corner_3d[2,:] + tz #Project to 2D low_row = np.vstack([corner_3d, np.ones(corner_3d.shape[1], dtype=np.float)]) corner_3d = np.dot(np.asarray(rt_matrix), low_row) ################################# #Create an orientation vector orientation_3d = np.dot( R, np.array([[0,0.7*l],[0,0],[0,0]]) ) #Translate orientation_3d[0,:] = orientation_3d[0,:] + tx orientation_3d[1,:] = orientation_3d[1,:] + ty orientation_3d[2,:] = orientation_3d[2,:] + tz #Project low_row = np.vstack([orientation_3d, np.ones(orientation_3d.shape[1], dtype=np.float)]) orientation_3d = np.dot(rt_matrix, low_row) K = np.asarray(cam_to_cam['P_rect_02']).reshape(3,4) K = K[:3,:3] corners_2d = projectToImage(corner_3d, K) orientation_2d = projectToImage(orientation_3d, K) x1 = min(corners_2d[0,:]) x2 = max(corners_2d[0,:]) y1 = min(corners_2d[1,:]) y2 = max(corners_2d[1,:]) bbox_2d = ImageRect() bbox_2d.score = -10.0 if ( (label == 'Car' or label=='Truck' or label=='Van') and np.any(corner_3d[2,:]>=0.5)) and (np.any(orientation_3d[2,:]>=0.5) and x1>=0 and x2>=0 and y1>0 and y2>=0 and occlusion <2): bbox_2d.x = x1 bbox_2d.y = y1 bbox_2d.width = x2-x1 bbox_2d.height = y2-y1 bbox_2d.score = 1.0 if d.has_key(frame + j) == True: d[frame + j].append(b) boxes_2d[frame + j].append(bbox_2d) pictograms[frame + j].append(picto_text) else: d[frame + j] = [b] boxes_2d[frame + j] = [bbox_2d] pictograms[frame + j]= [picto_text] return d, boxes_2d, pictograms
def readXML(file): tree = ET.parse(file) root = tree.getroot() item = root.findall('./tracklets/item') d = {} boxes_2d = {} pictograms = {} for i, v in enumerate(item): h = float(v.find('h').text) w = float(v.find('w').text) l = float(v.find('l').text) frame = int(v.find('first_frame').text) size = Vector3(l, w, h) label = v.find('objectType').text pose = v.findall('./poses/item') for j, p in enumerate(pose): tx = float(p.find('tx').text) ty = float(p.find('ty').text) tz = float(p.find('tz').text) rz = float(p.find('rz').text) occlusion = float(p.find('occlusion').text) q = tf.transformations.quaternion_from_euler(0.0, 0.0, rz) b = BoundingBox() b.pose.position = Vector3(tx, ty, tz / 2.0) b.pose.orientation = Quaternion(*q) b.dimensions = size b.label = i picto_text = Pictogram() picto_text.mode = Pictogram.STRING_MODE picto_text.pose.position = Vector3(tx, ty, -tz / 2.0) 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 = 5 picto_text.color = std_msgs.msg.ColorRGBA(1, 1, 1, 1) picto_text.character = label # Bounding Box corners corner_x = np.array( [l / 2, l / 2, -l / 2, -l / 2, l / 2, l / 2, -l / 2, -l / 2]) corner_y = np.array( [w / 2, -w / 2, -w / 2, w / 2, w / 2, -w / 2, -w / 2, w / 2]) corner_z = np.array([0, 0, 0, 0, h, h, h, h]) rz = wrapToPi(rz) ################### #create box on origin, then translate and rotate according to pose. finally, project into 2D image # Rotate and translate 3D bounding box in velodyne coordinate system R = np.array([[math.cos(rz), -math.sin(rz), 0], [math.sin(rz), math.cos(rz), 0], [0, 0, 1]]) corner_3d = np.dot(R, np.array([corner_x, corner_y, corner_z])) #Translate corner_3d[0, :] = corner_3d[0, :] + tx corner_3d[1, :] = corner_3d[1, :] + ty corner_3d[2, :] = corner_3d[2, :] + tz #Project to 2D low_row = np.vstack( [corner_3d, np.ones(corner_3d.shape[1], dtype=np.float)]) corner_3d = np.dot(np.asarray(rt_matrix), low_row) ################################# #Create an orientation vector orientation_3d = np.dot(R, np.array([[0, 0.7 * l], [0, 0], [0, 0]])) #Translate orientation_3d[0, :] = orientation_3d[0, :] + tx orientation_3d[1, :] = orientation_3d[1, :] + ty orientation_3d[2, :] = orientation_3d[2, :] + tz #Project low_row = np.vstack([ orientation_3d, np.ones(orientation_3d.shape[1], dtype=np.float) ]) orientation_3d = np.dot(rt_matrix, low_row) K = np.asarray(cam_to_cam['P_rect_02']).reshape(3, 4) K = K[:3, :3] corners_2d = projectToImage(corner_3d, K) orientation_2d = projectToImage(orientation_3d, K) x1 = min(corners_2d[0, :]) x2 = max(corners_2d[0, :]) y1 = min(corners_2d[1, :]) y2 = max(corners_2d[1, :]) bbox_2d = image_rect() bbox_2d.score = -10.0 if ((label == 'Car' or label == 'Truck' or label == 'Van') and np.any(corner_3d[2, :] >= 0.5)) and ( np.any(orientation_3d[2, :] >= 0.5) and x1 >= 0 and x2 >= 0 and y1 > 0 and y2 >= 0 and occlusion < 2): bbox_2d.x = x1 bbox_2d.y = y1 bbox_2d.width = x2 - x1 bbox_2d.height = y2 - y1 bbox_2d.score = 1.0 if d.has_key(frame + j) == True: d[frame + j].append(b) boxes_2d[frame + j].append(bbox_2d) pictograms[frame + j].append(picto_text) else: d[frame + j] = [b] boxes_2d[frame + j] = [bbox_2d] pictograms[frame + j] = [picto_text] return d, boxes_2d, pictograms
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)
"fa-toggle-up", "fa-trash", "fa-trash-o", "fa-tree", "fa-trello", "fa-trophy", "fa-truck", "fa-try", "fa-tty", "fa-tumblr", "fa-tumblr-square", "fa-turkish-lira", "fa-twitch", "fa-twitter", "fa-twitter-square", "fa-umbrella", "fa-underline", "fa-undo", "fa-university", "fa-unlink", "fa-unlock", "fa-unlock-alt", "fa-unsorted", "fa-upload", "fa-usd", "fa-user", "fa-user-md", "fa-users", "fa-video-camera", "fa-vimeo-square", "fa-vine", "fa-vk", "fa-volume-down", "fa-volume-off", "fa-volume-up", "fa-warning", "fa-wechat", "fa-weibo", "fa-weixin", "fa-wheelchair", "fa-wifi", "fa-windows", "fa-won", "fa-wordpress", "fa-wrench", "fa-xing", "fa-xing-square", "fa-yahoo", "fa-yelp", "fa-yen", "fa-youtube", "fa-youtube-play", "fa-youtube-square" ] counter = 0 while not rospy.is_shutdown(): msg = Pictogram() msg.action = Pictogram.JUMP_ONCE msg.header.frame_id = "/base_link" msg.header.stamp = rospy.Time.now() msg.pose.position.z = 1.6 msg.pose.orientation.w = 0.7 msg.pose.orientation.x = 0 msg.pose.orientation.y = -0.7 msg.pose.orientation.z = 0 msg.size = 0.5 msg.color.r = 25 / 255.0 msg.color.g = 255 / 255.0 msg.color.b = 240 / 255.0 msg.color.a = 1.0 msg.character = pictograms[counter] p.publish(msg)