def predict_goal(self):
    idx = 0
    max_prob = 0.0
    p_goal = PredictedGoal()
    for i in range(0, len(self.current_poses[0])):
        for j in range(0, len(self.goals_x)):
            if (max_prob < self.probability_goal[i][j]):
                idx = j
                max_prob = self.probability_goal[i][j]
        self.predicted_goal.header.stamp = rospy.Time.now()
        self.predicted_goal.header.frame_id = 'map'
        self.predicted_goal.pose.position.x = self.goals_x[idx]
        self.predicted_goal.pose.position.y = self.goals_y[idx]
        self.predicted_goal.pose.position.z = 0.0
        self.predicted_goal.pose.orientation = self.current_poses[0][i].orientation
        if (self.last_idx != idx):
            p_goal.changed = True
            self.changed = True
    self.last_idx = idx
    p_goal.goal = self.predicted_goal
    self.goal_pub_.publish(p_goal)
def predict_goal(self):
    idx = 0
    max_prob = 0.0
    p_goal = PredictedGoal()
    for i in range(0, len(self.current_poses[0])):
        for j in range(0, len(self.goals_x)):
            if (max_prob < self.probability_goal[i][j]):
                idx = j
                max_prob = self.probability_goal[i][j]
        self.predicted_goal.header.stamp = rospy.Time.now()
        self.predicted_goal.header.frame_id = 'map'
        self.predicted_goal.pose.position.x = self.goals_x[idx]
        self.predicted_goal.pose.position.y = self.goals_y[idx]
        self.predicted_goal.pose.position.z = 0.0
        self.predicted_goal.pose.orientation = self.current_poses[0][i].orientation
        if (self.last_idx != idx):
            p_goal.changed = True
            self.changed = True
    self.last_idx = idx
    p_goal.goal = self.predicted_goal
    self.goal_pub_.publish(p_goal)