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)