Move base server miss a goal

asked 2021-10-22 04:09:22 -0600

zuy gravatar image

updated 2021-10-22 04:09:41 -0600

Hi, I'm having a problem using move_base server. I have a rospy node, which keep sending goals to move_base server. After the current goal is reached, the next goal is sent, after executed all goals in the goals list it resends the first goal and than the next goal and so on. Every things seems fine, except that sometimes after a new goal is sent, the move_base server assumes the goal is reached without doing anything and send the next goal immediately. Although this error not usually happends but it really annoys me. So what is the main reason for this problem and how can I fix this? Thank you. Here is the code in my rospy node: The initialize function:

def __init__(self ):
    self.move_base = actionlib.SimpleActionClient("/move_base",MoveBaseAction)
    self.Goal = MoveBaseGoal()
    self.Goal.target_pose.header.frame_id = "map"

The sendgoal function:

def sendgoal(self,x_goal,y_goal,yaw_goal):
    try:

        self.Goal.target_pose.pose.position.x = x_goal
        self.Goal.target_pose.pose.position.y = y_goal
        q = quaternion_from_euler(0,0,yaw_goal)
        self.Goal.target_pose.pose.orientation = Quaternion(*q)

        rospy.loginfo("Sending goal")
        self.move_base.send_goal(self.Goal)
        state = self.move_base.get_state()
        rospy.loginfo(state)
        success = self.move_base.wait_for_result()

        if not success:
            self.move_base.cancel_goal()
            rospy.loginfo("Failed to reach goal for some reason")
            return 0
        else:
            state = self.move_base.get_state()
            if state == GoalStatus.SUCCEEDED:
                rospy.loginfo("Goal reached!")
                return 1
            else:
                return 0
    except:
        return 0

The main part of the code:

while k <= self.count:
        if not self.sendgoal(self.list_point[k][0],self.list_point[k][1],self.list_point[k][2]):
            rospy.loginfo("Canceled goal, needed attention")
            break
        k+=1
        if k==self.count:
            k = 0
edit retag flag offensive close merge delete