def publish_tracked_people(self, now): """ Publish markers of tracked people to Rviz and to <people_tracked> topic """ people_tracked_msg = PersonArray() people_tracked_msg.header.stamp = now people_tracked_msg.header.frame_id = self.publish_people_frame marker_id = 0 # Make sure we can get the required transform first: if self.use_scan_header_stamp_for_tfs: tf_time = now try: self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0)) transform_available = True except: transform_available = False else: tf_time = rospy.Time(0) transform_available = self.listener.canTransform( self.publish_people_frame, self.fixed_frame, tf_time) marker_id = 0 if not transform_available: rospy.loginfo( "Person tracker: tf not avaiable. Not publishing people") else: # Publish tracked people to /people_tracked topic and to rviz for person in self.people_tracked: leg_1 = person.leg_1 leg_2 = person.leg_2 if self.publish_occluded or leg_1.seen_in_current_scan or leg_2.seen_in_current_scan: # Get person's position in the <self.publish_people_frame> frame ps = PointStamped() ps.header.frame_id = self.fixed_frame ps.header.stamp = tf_time ps.point.x = (leg_1.pos_x + leg_2.pos_x) / 2. ps.point.y = (leg_1.pos_y + leg_2.pos_y) / 2. try: ps = self.listener.transformPoint( self.publish_people_frame, ps) except: rospy.logerr( "Not publishing people due to no transform from fixed_frame-->publish_people_frame" ) continue # publish to people_tracked topic new_person = Person() new_person.pose.position.x = ps.point.x new_person.pose.position.y = ps.point.y new_person.id = person.id_num people_tracked_msg.people.append(new_person) # publish rviz markers marker = Marker() marker.header.frame_id = self.publish_people_frame marker.header.stamp = now marker.ns = "People_tracked" marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = ( rospy.Duration(3) - (rospy.get_rostime() - leg_1.last_seen) ).to_sec() / rospy.Duration(3).to_sec() + 0.1 marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y for i in xrange( 2 ): # publish two markers per person: one for body and one for head marker.id = marker_id #person.id_num + 20000*i marker_id += 1 if i == 0: # cylinder for body shape marker.type = Marker.CYLINDER marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 1.2 marker.pose.position.z = 0.8 else: # sphere for head shape marker.type = Marker.SPHERE marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.pose.position.z = 1.5 self.marker_pub.publish(marker) # Text showing person's ID number marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.id = marker_id marker_id += 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = str(person.id_num) marker.scale.z = 0.2 marker.pose.position.z = 1.7 self.marker_pub.publish(marker) # Clear previously published people markers for m_id in xrange(marker_id, self.prev_person_marker_id): marker = Marker() marker.header.stamp = now marker.header.frame_id = self.publish_people_frame marker.ns = "People_tracked" marker.id = m_id marker.action = marker.DELETE self.marker_pub.publish(marker) self.prev_person_marker_id = marker_id # Publish people tracked message self.people_tracked_pub.publish(people_tracked_msg)
def publish_tracked_people(self, now): """ Publish markers of tracked people to Rviz and to <people_tracked> topic """ people_tracked_msg = PersonArray() people_tracked_msg.header.stamp = now people_tracked_msg.header.frame_id = self.publish_people_frame marker_id = 0 # Make sure we can get the required transform first: if self.use_scan_header_stamp_for_tfs: tf_time = now try: self.listener.waitForTransform(self.publish_people_frame, self.fixed_frame, tf_time, rospy.Duration(1.0)) transform_available = True except: transform_available = False else: tf_time = rospy.Time(0) transform_available = self.listener.canTransform( self.publish_people_frame, self.fixed_frame, tf_time) marker_id = 0 if not transform_available: rospy.loginfo( "Person tracker: tf not avaiable. Not publishing people") else: for person in self.objects_tracked: if person.is_person == True: if self.publish_occluded or person.seen_in_current_scan: # Only publish people who have been seen in current scan, unless we want to publish occluded people # Get position in the <self.publish_people_frame> frame ps = PointStamped() ps.header.frame_id = self.fixed_frame ps.header.stamp = tf_time ps.point.x = person.pos_x ps.point.y = person.pos_y try: ps = self.listener.transformPoint( self.publish_people_frame, ps) except: rospy.logerr( "Not publishing people due to no transform from fixed_frame-->publish_people_frame" ) continue # publish to people_tracked topic new_person = Person() new_person.pose.position.x = ps.point.x new_person.pose.position.y = ps.point.y yaw = math.atan2(person.vel_y, person.vel_x) quaternion = tf.transformations.quaternion_from_euler( 0, 0, yaw) new_person.pose.orientation.x = quaternion[0] new_person.pose.orientation.y = quaternion[1] new_person.pose.orientation.z = quaternion[2] new_person.pose.orientation.w = quaternion[3] new_person.id = person.id_num people_tracked_msg.people.append(new_person) # publish rviz markers # Cylinder for body marker = Marker() marker.header.frame_id = self.publish_people_frame marker.header.stamp = now marker.ns = "People_tracked" marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = ( rospy.Duration(3) - (rospy.get_rostime() - person.last_seen) ).to_sec() / rospy.Duration(3).to_sec() + 0.1 marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y marker.id = marker_id marker_id += 1 marker.type = Marker.CYLINDER marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 1.2 marker.pose.position.z = 0.8 self.marker_pub.publish(marker) # Sphere for head shape marker.type = Marker.SPHERE marker.scale.x = 0.2 marker.scale.y = 0.2 marker.scale.z = 0.2 marker.pose.position.z = 1.5 marker.id = marker_id marker_id += 1 self.marker_pub.publish(marker) # Text showing person's ID number marker.color.r = 1.0 marker.color.g = 1.0 marker.color.b = 1.0 marker.color.a = 1.0 marker.id = marker_id marker_id += 1 marker.type = Marker.TEXT_VIEW_FACING marker.text = str(person.id_num) marker.scale.z = 0.2 marker.pose.position.z = 1.7 self.marker_pub.publish(marker) # Arrow pointing in direction they're facing with magnitude proportional to speed marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = ( rospy.Duration(3) - (rospy.get_rostime() - person.last_seen) ).to_sec() / rospy.Duration(3).to_sec() + 0.1 start_point = Point() end_point = Point() start_point.x = marker.pose.position.x start_point.y = marker.pose.position.y end_point.x = start_point.x + 0.5 * person.vel_x end_point.y = start_point.y + 0.5 * person.vel_y marker.pose.position.x = 0. marker.pose.position.y = 0. marker.pose.position.z = 0.1 marker.id = marker_id marker_id += 1 marker.type = Marker.ARROW marker.points.append(start_point) marker.points.append(end_point) marker.scale.x = 0.05 marker.scale.y = 0.1 marker.scale.z = 0.2 self.marker_pub.publish(marker) # <self.confidence_percentile>% confidence bounds of person's position as an ellipse: cov = person.filtered_state_covariances[0][ 0] + person.var_obs # cov_xx == cov_yy == cov std = cov**(1. / 2.) gate_dist_euclid = scipy.stats.norm.ppf( 1.0 - (1.0 - self.confidence_percentile) / 2., 0, std) marker.pose.position.x = ps.point.x marker.pose.position.y = ps.point.y marker.type = Marker.SPHERE marker.scale.x = 2 * gate_dist_euclid marker.scale.y = 2 * gate_dist_euclid marker.scale.z = 0.01 marker.color.r = person.colour[0] marker.color.g = person.colour[1] marker.color.b = person.colour[2] marker.color.a = 0.1 marker.pose.position.z = 0.0 marker.id = marker_id marker_id += 1 self.marker_pub.publish(marker) # Clear previously published people markers for m_id in xrange(marker_id, self.prev_person_marker_id): marker = Marker() marker.header.stamp = now marker.header.frame_id = self.publish_people_frame marker.ns = "People_tracked" marker.id = m_id marker.action = marker.DELETE self.marker_pub.publish(marker) self.prev_person_marker_id = marker_id # Publish people tracked message self.people_tracked_pub.publish(people_tracked_msg)
def clicked_point_callback(self, clicked_point_msg): if self.next_scan_msg_time is None or self.cur_scan_msg_time is None: self.nextScanMsg() return # Save messages previously set to write to rosbag, as long as they haven't been cleared elsewhere already for topic, msg, time in self.to_save_to_savebag: self.savebag.write(topic, msg, time) del self.to_save_to_savebag[:] # Calc time to save messages to rosbags: time = (self.next_scan_msg_time - self.cur_scan_msg_time)/2 + self.cur_scan_msg_time # Save person's position in savebag person = Person() person.id = self.annotation_id person.pose.position = clicked_point_msg.point self.to_save_to_savebag.append(("/ground_truth_people_tracks", person, time)) # Rviz marker to save to the rosbag m2 = Marker() m2.header.frame_id = self.fixed_frame m2.id = self.annotation_id m2.ns = "ground_truth_people_tracks_markers" m2.type = Marker.SPHERE if self.bag_empty: # In this case we'll be exiting right after and won't have time to clear the marker m2.lifetime = rospy.Duration(0.25); m2.scale.x = 0.2 m2.scale.y = 0.2 m2.scale.z = 0.3 m2.color.r = self.colour[0] m2.color.g = self.colour[1] m2.color.b = self.colour[2] m2.color.a = 1.0 m2.pose.position.x = person.pose.position.x m2.pose.position.y = person.pose.position.y m2.pose.position.z = 0.1 self.to_save_to_savebag.append(("/visualization_marker", m2, time)) # Rviz marker showing id number m3 = Marker() m3.header.frame_id = self.fixed_frame m3.id = self.annotation_id m3.ns = "annotation_ids" m3.type = Marker.TEXT_VIEW_FACING if self.bag_empty: # In this case we'll be exiting right after and won't have time to clear the marker m3.lifetime = rospy.Duration(0.25); m3.scale.x = 0.3 m3.scale.y = 0.3 m3.scale.z = 0.3 m3.color.r = 1.0 m3.color.g = 1.0 m3.color.b = 1.0 m3.color.a = 1.0 m3.pose.position.x = person.pose.position.x m3.pose.position.y = person.pose.position.y m3.pose.position.z = 0.3 m3.text = str(self.annotation_id) self.to_save_to_savebag.append(("/visualization_marker", m3, time)) # Rviz marker of where annotator clicked to display to rviz temporarily (not saving to rosbag) m = Marker() m.header.frame_id = self.fixed_frame m.id = self.annotation_id m.ns = "clicked_points" m.type = Marker.SPHERE m.lifetime = rospy.Duration(0.05); # Just a short duration to show in rviz m.scale.x = 0.2 m.scale.y = 0.2 m.scale.z = 0.3 m.color.r = self.colour[0] m.color.g = self.colour[1] m.color.b = self.colour[2] m.color.a = 1.0 m.pose.position.x = person.pose.position.x m.pose.position.y = person.pose.position.y m.pose.position.z = 0.1 self.marker_pub.publish(m) self.prev_annotation_id = self.annotation_id # So we know which markers need to be removed on next iteration self.nextScanMsg()