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