def tracked_humansCB(self, msg):
    self.prev_poses = self.current_poses
    self.current_poses = [[] for i in range(self.human_num)]
    for human in msg.humans:
        for segment in human.segments:
            if (segment.type == TrackedSegmentType.TORSO):
                self.current_poses[(human.track_id - 1)].append(segment.pose.pose)
    if (not self.done):
        self.prev_poses = self.current_poses
    for i in range(0, len(self.current_poses[0])):
        diff = np.linalg.norm([(self.current_poses[0][i].position.x - self.prev_poses[0][i].position.x), (self.current_poses[0][i].position.y - self.prev_poses[0][i].position.y)])
        if ((diff > EPS) or (not self.done)):
            dist = []
            for j in range(0, len(self.goals_x)):
                vec1 = (np.array([self.goals_x[j], self.goals_y[j], 0.0]) - np.array([self.current_poses[0][i].position.x, self.current_poses[0][i].position.y, 0.0]))
                rotation = (self.current_poses[0][i].orientation.x, self.current_poses[0][i].orientation.y, self.current_poses[0][i].orientation.z, self.current_poses[0][i].orientation.w)
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rotation)
                unit_vec = np.array([np.cos(yaw), np.sin(yaw), 0.0])
                self.theta_phi[i][j] = np.arccos((np.dot(vec1, unit_vec) / np.linalg.norm(vec1)))
                dist.append(np.linalg.norm([(self.current_poses[0][i].position.x - self.goals_x[j]), (self.current_poses[0][i].position.y - self.goals_y[j])]))
            self.probability_goal_window[i][self.itr] = self.mv_nd.pdf(np.array(self.theta_phi[i]))
            self.probability_goal[i] = np.array(([1.0] * self.goal_num))
            for k in range(0, len(self.probability_goal_window[i])):
                gf = np.exp(((k - self.window_size) / 5))
                self.probability_goal[i] = (np.power(self.probability_goal_window[i][k], gf) * np.array(self.probability_goal[i]))
            for ln in range(0, len(self.goals_x)):
                self.probability_goal[i][ln] = ((1 / dist[ln]) * self.probability_goal[i][ln])
            self.probability_goal[i] = ((self.probability_goal[i] - np.min(self.probability_goal[i])) / (np.max(self.probability_goal[i]) - np.min(self.probability_goal[i])))
            self.itr = (self.itr + 1)
            if (self.itr == self.window_size):
                self.itr = 0
            self.done = True
    self.predict_goal()
def tracked_humansCB(self, msg):
    self.prev_poses = self.current_poses
    self.current_poses = [[] for i in range(self.human_num)]
    for human in msg.humans:
        for segment in human.segments:
            if (segment.type == TrackedSegmentType.TORSO):
                self.current_poses[(human.track_id - 1)].append(segment.pose.pose)
    if (not self.done):
        self.prev_poses = self.current_poses
    for i in range(0, len(self.current_poses[0])):
        diff = np.linalg.norm([(self.current_poses[0][i].position.x - self.prev_poses[0][i].position.x), (self.current_poses[0][i].position.y - self.prev_poses[0][i].position.y)])
        if ((diff > EPS) or (not self.done)):
            dist = []
            for j in range(0, len(self.goals_x)):
                vec1 = (np.array([self.goals_x[j], self.goals_y[j], 0.0]) - np.array([self.current_poses[0][i].position.x, self.current_poses[0][i].position.y, 0.0]))
                rotation = (self.current_poses[0][i].orientation.x, self.current_poses[0][i].orientation.y, self.current_poses[0][i].orientation.z, self.current_poses[0][i].orientation.w)
                (roll, pitch, yaw) = tf.transformations.euler_from_quaternion(rotation)
                unit_vec = np.array([np.cos(yaw), np.sin(yaw), 0.0])
                self.theta_phi[i][j] = np.arccos((np.dot(vec1, unit_vec) / np.linalg.norm(vec1)))
                dist.append(np.linalg.norm([(self.current_poses[0][i].position.x - self.goals_x[j]), (self.current_poses[0][i].position.y - self.goals_y[j])]))
            self.probability_goal_window[i][self.itr] = self.mv_nd.pdf(np.array(self.theta_phi[i]))
            self.probability_goal[i] = np.array(([1.0] * self.goal_num))
            for k in range(0, len(self.probability_goal_window[i])):
                gf = np.exp(((k - self.window_size) / 5))
                self.probability_goal[i] = (np.power(self.probability_goal_window[i][k], gf) * np.array(self.probability_goal[i]))
            for ln in range(0, len(self.goals_x)):
                self.probability_goal[i][ln] = ((1 / dist[ln]) * self.probability_goal[i][ln])
            self.probability_goal[i] = ((self.probability_goal[i] - np.min(self.probability_goal[i])) / (np.max(self.probability_goal[i]) - np.min(self.probability_goal[i])))
            self.itr = (self.itr + 1)
            if (self.itr == self.window_size):
                self.itr = 0
            self.done = True
    self.predict_goal()