def predict_path(self, person): path_prediction = self.path_prediction = [] goal_x, goal_y = self.goal_pos vx, vy = person.velocity.x, person.velocity.y px, py = person.position.x, person.position.y goal_dist = ((goal_x - px)**2 + (goal_y - py)**2)**0.5 speed = (vx**2 + vy**2)**0.5 theta = math.atan2(goal_y - py, goal_x - px) step_dist = speed * self.time_resolution if not speed: max_prediction = 0 else: max_prediction = min(self.num_predictions, math.ceil(goal_dist / step_dist)) step_x, step_y = step_dist * math.cos(theta), step_dist * math.sin( theta) last_x, last_y = px, py for i in range(1, self.num_predictions + 1): person_prediction = Person() person_prediction.position.z = 0 person_prediction.velocity = person.velocity path_prediction.append(person_prediction) if i >= max_prediction: person_prediction.position.x = last_x person_prediction.position.y = last_y else: last_x = person_prediction.position.x = px + i * step_x last_y = person_prediction.position.y = py + i * step_y
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