def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
def predict(self, tracked_persons):
tracks = tracked_persons.tracks
track_dict = {}
for track in tracks:
#print track
#print track.pose.pose.position.x
track_dict[track.track_id] = [
track.pose.pose.position.x, track.pose.pose.position.y
]
del self.prev_frames[0]
self.prev_frames.append(track_dict)
if len(tracks) == 0:
return
input_data = self.__generate_input(tracks)
#print input_data.shape
#print input_data
grid_batch = getSequenceGridMask(input_data, self.dimensions,
self.saved_args.neighborhood_size,
self.saved_args.grid_size)
obs_traj = input_data[:self.obs_length]
obs_grid = grid_batch[:self.obs_length]
print "********************** PREDICT NEW TRAJECTORY ******************************"
complete_traj = self.social_lstm_model.sample(self.sess, obs_traj,
obs_grid,
self.dimensions,
input_data,
self.pred_length)
#print complete_traj
# Initialize the markers array
prediction_markers = MarkerArray()
# Publish them
people_predictions = PeoplePrediction()
for frame_index in range(self.pred_length):
people = People()
people.header.stamp = tracked_persons.header.stamp + rospy.Duration(
frame_index * self.time_resolution)
people.header.frame_id = tracked_persons.header.frame_id
predicted_frame_index = frame_index + self.obs_length
for person_index in range(self.max_pedestrians):
track_id = complete_traj[predicted_frame_index, person_index,
0]
x_coord = complete_traj[predicted_frame_index, person_index, 1]
y_coord = complete_traj[predicted_frame_index, person_index, 2]
if track_id == 0:
break
person = Person()
person.name = str(track_id)
point = Point()
point.x = x_coord
point.y = y_coord
person.position = point
people.people.append(person)
self.prediction_marker.header.frame_id = tracked_persons.header.frame_id
self.prediction_marker.header.stamp = tracked_persons.header.stamp
self.prediction_marker.id = int(track_id)
self.prediction_marker.pose.position.x = person.position.x
self.prediction_marker.pose.position.y = person.position.y
#self.prediction_marker.color.a = 1 - (frame_index * 1.0 / (self.pred_length * 1.0))
self.prediction_marker.color.a = 1.0
prediction_markers.markers.append(self.prediction_marker)
people_predictions.predicted_people.append(people)
#print people_predictions
self.pedestrian_prediction_pub.publish(people_predictions)
self.prediction_marker_pub.publish(prediction_markers)
def get_bbox_cog(self, yolo_boxes, cloud):
# Clear the people array
self.people.people = list()
# Clear the people pose array
self.people_poses.poses = list()
# Clear the bounding box array
self.people_bounding_boxes.boxes = list()
# Clear the detected persons array
self.detected_persons.detections = list()
# Assign arbitrary IDs
person_id = 0
for detection in yolo_boxes.bboxes:
if detection.Class == 'person':
bbox = [
detection.xmin, detection.ymin, detection.xmax,
detection.ymax
]
#try:
person_pose, person_bounding_box = self.get_bbox_pose(
bbox, cloud)
if person_pose is None:
continue
#except:
# print person_pose, person_bounding_box
# os._exit(1)
# Pose
self.people_poses.poses.append(person_pose.pose)
# Bounding box
self.people_bounding_boxes.boxes.append(person_bounding_box)
# Person message type for social cost map
person = Person()
person.position = person_pose.pose.position
#person.position.z = 0.0
person.name = str(len(self.people.people))
person.reliability = 1.0
# Spencer DetectedPerson
detected_person = DetectedPerson()
detected_person.modality = DetectedPerson.MODALITY_GENERIC_RGBD
detected_person.detection_id = person_id
person_id += 1
detected_person.pose.pose = person_pose.pose
self.detected_persons.detections.append(detected_person)
self.people.people.append(person)
now = rospy.Time.now()
self.people.header.stamp = now
self.people_pub.publish(self.people)
self.people_poses.header.stamp = now
self.people_poses_pub.publish(self.people_poses)
self.people_bounding_boxes.header.stamp = now
self.people_bounding_boxes_pub.publish(self.people_bounding_boxes)
self.detected_persons.header.stamp = now
self.detected_persons_pub.publish(self.detected_persons)
