def __init__(self, human_num=1):
    self.human_num = human_num
    self.goals_x = [1.5, 7.0, 9.0, 10.5, 1.5, 10.3, 8.5]
    self.goals_y = [2.0, 8.0, 12.5, 15.0, 15.0, 1.5, (- 4.5)]
    self.goal_num = 7
    self.predicted_goal = PoseStamped()
    self.last_idx = 0
    self.changed = False
    self.current_poses = [[] for i in range(self.human_num)]
    self.prev_poses = [[] for i in range(self.human_num)]
    self.mv_nd = multivariate_normal(mean=0, cov=0.1)
    self.theta_phi = [([0] * self.goal_num) for i in range(self.human_num)]
    self.window_size = 10
    self.probability_goal = [np.array(([(1.0 / self.goal_num)] * self.goal_num)) for i in range(self.human_num)]
    self.probability_goal_window = [np.array(([([(1.0 / self.goal_num)] * self.goal_num)] * self.window_size)) for i in range(self.human_num)]
    self.done = False
    self.itr = 0
    NODE_NAME = 'human_goal_predict'
    rospy.init_node(NODE_NAME)
    self.humans_sub_ = rospy.Subscriber('/tracked_humans', TrackedHumans, self.tracked_humansCB)
    self.goal_pub_ = rospy.Publisher((NODE_NAME + '/predicted_goal'), PredictedGoal, queue_size=2)
    self.goal_srv_ = rospy.Service('goal_changed', Trigger, self.goal_changed)
    rospy.spin()
def __init__(self, human_num=1):
    self.human_num = human_num
    self.goals_x = [1.5, 7.0, 9.0, 10.5, 1.5, 10.3, 8.5]
    self.goals_y = [2.0, 8.0, 12.5, 15.0, 15.0, 1.5, (- 4.5)]
    self.goal_num = 7
    self.predicted_goal = PoseStamped()
    self.last_idx = 0
    self.changed = False
    self.current_poses = [[] for i in range(self.human_num)]
    self.prev_poses = [[] for i in range(self.human_num)]
    self.mv_nd = multivariate_normal(mean=0, cov=0.1)
    self.theta_phi = [([0] * self.goal_num) for i in range(self.human_num)]
    self.window_size = 10
    self.probability_goal = [np.array(([(1.0 / self.goal_num)] * self.goal_num)) for i in range(self.human_num)]
    self.probability_goal_window = [np.array(([([(1.0 / self.goal_num)] * self.goal_num)] * self.window_size)) for i in range(self.human_num)]
    self.done = False
    self.itr = 0
    NODE_NAME = 'human_goal_predict'
    rospy.init_node(NODE_NAME)
    self.humans_sub_ = rospy.Subscriber('/tracked_humans', TrackedHumans, self.tracked_humansCB)
    self.goal_pub_ = rospy.Publisher((NODE_NAME + '/predicted_goal'), PredictedGoal, queue_size=2)
    self.goal_srv_ = rospy.Service('goal_changed', Trigger, self.goal_changed)
    rospy.spin()