思路:采用距离判断,等到车与目标点之间距离小于一定值的时候 才发布下一个目标点
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"(最终)