def viacon_callback(self, msg):
if self.last_pos is None:
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
return
people = People()
person = Person()
people.header.stamp = msg.header.stamp
people.header.frame_id = "map"
last_x, last_y, last_t = self.last_pos
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
if msg.header.stamp.to_sec() - last_t <= 0:
return
self.vx = (1 - self.gamma) * self.vx + \
self.gamma * (-msg.transform.translation.x - last_x) / (msg.header.stamp.to_sec() - last_t)
self.vy = (1 - self.gamma) * self.vy + \
self.gamma * (-msg.transform.translation.y - last_y) / (msg.header.stamp.to_sec() - last_t)
person.position.x = -msg.transform.translation.x
person.position.y = -msg.transform.translation.y
self.last_pos = -msg.transform.translation.x, -msg.transform.translation.y, msg.header.stamp.to_sec(
)
people.people = [person]
self.p_pub.publish(people)
def run(self):
self.start_time = rospy.Time.now()
while not rospy.is_shutdown():
if (
rospy.Time.now() - self.start_time <
rospy.Duration(self.delayed_start)
):
rospy.sleep(0.1)
continue
poses = []
persons = []
for w in self.walkers:
if random() < self.prob_switch_visible:
self.visible[w.name] = (random() < self.prob_visible)
if self.visible[w.name]:
rospy.loginfo(
"switch sensor %s to see %s" % (self.name, w.name))
else:
rospy.loginfo(
"switch sensor %s to NOT see %s" % (self.name, w.name))
if self.visible[w.name]:
ps = w.get_pose()
ps.position.x += nprnd.randn() * self.noise[0]
ps.position.y += nprnd.randn() * self.noise[1]
poses.append(ps)
p = w.get_person()
p.position.x = ps.position.x
p.position.y = ps.position.y
if self.anonymous:
p.name = ''
persons.append(p)
pa = PoseArray()
pa.header.frame_id = '/base_link'
pa.header.stamp = rospy.Time.now()
pa.poses = poses
self.pub_poses.publish(pa)
ppl = People()
ppl.header.frame_id = '/base_link'
ppl.header.stamp = rospy.Time.now()
ppl.people = persons
self.pub_people.publish(ppl)
sleep_rate = (
self.avg_rate + nprnd.randn() * self.std_rate)
sleep_rate = sleep_rate if sleep_rate > .1 else .1
rospy.sleep(1.0 / sleep_rate)
def people_callback(self, people):
assert len(people.people) == 1
person = people.people[0]
predictions = PeoplePrediction()
markers = MarkerArray()
markers.markers = []
predictions.predicted_people = []
# if the message stamp is empty, we assign the current time
if people.header.stamp == rospy.Time():
people.header.stamp = rospy.get_rostime()
#delta_t = []
tdist = 0.0
for goal in self.goals:
goal.update_distance(person)
dist = -(goal.heuristic_distance[-1] - goal.heuristic_distance[0])
dist = max(dist, 0.0)
tdist += dist**2.0
#delta_t.append(dist)
if tdist > 0.0: # to avoid singularities
for goal in self.goals:
dist = -(goal.heuristic_distance[-1] -
goal.heuristic_distance[0])
dist = max(dist, 0.0)
goal.probability = dist**2.0 / tdist
#print(tdist, [goal.probability for goal in self.goals], sum([goal.probability for goal in self.goals]))
#if sum(delta_t) > 0.0:
# print(tdist, delta_t, sum(delta_t), delta_t[0]/sum(delta_t))
#else:
# print(tdist, delta_t, sum(delta_t))
predictions.path_probabilities = probabilities = []
for goal in self.goals:
goal.predict_path(person)
probabilities.append(Float64())
probabilities[-1].data = goal.probability
k = 0
for i in range(self.num_predictions):
people_one_timestep = People()
people_one_timestep.people = []
for goal in self.goals[::-1]:
person_prediction = goal.path_prediction[i]
people_one_timestep.people.append(person_prediction)
prediction_marker = self.get_marker()
prediction_marker.header.frame_id = people.header.frame_id
prediction_marker.header.stamp = people.header.stamp
prediction_marker.id = k
k += 1
prediction_marker.pose.position = person_prediction.position
# the opacity of the marker is adjusted according to the prediction step
prediction_marker.color.a = (
1 - i / float(self.num_predictions)) * goal.probability
markers.markers.append(prediction_marker)
# fill the header
people_one_timestep.header.frame_id = people.header.frame_id
people_one_timestep.header.stamp = people.header.stamp + \
rospy.Duration(i * self.time_resolution)
# push back the predictions for this time step to the prediction container
predictions.predicted_people.append(people_one_timestep)
self.prediction_pub.publish(predictions)
self.marker_pub.publish(markers)
