goal_loop.py思路及关键代码解释

思路：采用距离判断，等到车与目标点之间距离小于一定值的时候 才发布下一个目标点

    def statusCB(self):
        if self.pubfinal == True:
            self.pub_final.publish(1.0)
            print("the final goal")

        self.gx = self.goalListX[self.goalId-1] if(self.goalId != 0) else self.goalListX[self.goalId]
        self.gy = self.goalListY[self.goalId-1] if(self.goalId != 0) else self.goalListY[self.goalId]

        self.dist = self.distance(self.kx, self.ky, self.gx, self.gy)

        if self.dist < self.MIN_DISTANCE and self.flag == 1:

            finish_time = rospy.get_time()
            interval = finish_time - self.start_time
            print(interval)
            if self.goalId == self.LONG:
                self.goalId = 0
            self.goalMsg.header.stamp = rospy.Time.now()
            self.goalMsg.pose.position.x = self.goalListX[self.goalId]
            self.goalMsg.pose.position.y = self.goalListY[self.goalId]
            self.goalMsg.pose.orientation.z = self.goalListZ[self.goalId]
            self.goalMsg.pose.orientation.w = self.goalListW[self.goalId]
            self.pub.publish(self.goalMsg)
            rospy.loginfo(
                "Initial goal published! Goal ID is: %d", self.goalId)
            rospy.loginfo("intostatusCB")
            self.count = self.count+1
            print(self.count)

            if self.goalId < (len(self.goalListX)):
                self.goalId = self.goalId + 1

            else:
                self.goalId = 0
                print("final")

定义回调函数 statusCB:  
    如果 发布最终状态 标志为真:  
        发布最终状态消息（例如，发布数值1.0）  
        打印 "the final goal"（最终目标）  
  
    # 获取当前目标的x和y坐标  
    如果 当前目标ID不为0:  
        当前目标x坐标 = 目标列表X中当前目标ID减1的索引  
        当前目标y坐标 = 目标列表Y中当前目标ID减1的索引  
    否则:  
        当前目标x坐标 = 目标列表X中当前目标ID的索引  
        当前目标y坐标 = 目标列表Y中当前目标ID的索引  
  
    # 计算当前位置到目标位置的距离  
    当前距离 = 计算距离(当前位置x, 当前位置y, 当前目标x, 当前目标y)  
  
    # 如果当前距离小于最小距离且标志为1  
    如果 当前距离 < 最小距离 且 标志 == 1:  
        记录完成时间 = 获取当前时间  
        时间间隔 = 完成时间 - 开始时间  
        打印 时间间隔  
  
        # 如果当前是最后一个目标（这里假设LONG是最后一个目标的ID）  
        如果 当前目标ID == 最后一个目标ID:  
            重置当前目标ID为0  
  
        # 准备并发布下一个目标  
        设置目标消息的时间戳为当前时间  
        设置目标消息的x坐标为下一个目标的x坐标  
        设置目标消息的y坐标为下一个目标的y坐标  
        设置目标消息的z方向（假设是欧拉角或四元数的z部分）为下一个目标的z方向  
        设置目标消息的四元数w部分为下一个目标的w部分  
        发布目标消息  
        记录日志信息，表示初始目标已发布，并打印目标ID  
        记录日志信息，表示进入statusCB  
        目标计数增加  
        打印目标计数  
  
        # 更新当前目标ID  
        如果 当前目标ID小于目标列表X的长度:  
            当前目标ID增加1  
        否则:  
            重置当前目标ID为0  
            打印 "final"（最终）