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()