案例设计

pid4.png
通过指定目标点的X和Y值，让小乌龟到达指定点。

运动分析

小乌龟如何到达目标点?

数据获取

小乌龟当前的坐标和角度
可以通过订阅/turtle1/pose获得相关的信息

控制操作

通过设置小乌龟的线速度和角速度可以让小乌龟动起来
可以通过发布数据到/turtle1/cmd_vel控制小乌龟移动

示例代码

发布者与订阅者

  1. # 订阅位置
  2. self.sub = self.node.create_subscription(Pose,'/turtle1/pose',self.pose_callback,10)
  3. # 小乌龟控制发布者
  4. self.control_pub = self.node.create_publisher(Twist,'/turtle1/cmd_vel',10)

获取小乌龟实时位置信息的回调

  1. def pose_callback(self,pose):
  2.         self.curX = pose.x
  3.         self.curY = pose.y
  4.         self.curTheta = pose.theta
  5.         # 设置当前位置
  6.         self.xL.setText(str(self.curX))
  7.         self.yL.setText(str(self.curY))
  8.         self.thetaL.setText(str(self.curTheta))

直线运动计算

pid3.png
通过最简单的示例，先解决指线运动。

  1. 距离 = 速度 * 时间
  2. 速度 = 距离 / 时间

我们已知的是当前小乌龟的坐标turtle(x, y) ，和目标点dist(x, y)，我们要去得到是小乌龟的速度。
首先我们需要计算出两点间的距离：

  1. def distance(self,curX,curY,targetX,targetY):
  2.         return math.sqrt(pow(targetX-curX,2)+pow(targetY-curY,2))

其次我们需要确定的是时间time，我们可以给定一个预期的值。
我们可以将计算的结果进行运行测试。
测试发现，小乌龟默认运行的时间是1s。没有提供给我们设置时间接口。

解决时间不可控的问题

方式一:

  1. rate = self.node.create_rate(1)
  2. for i in range(5):
  3.     data = Twist()
  4.     data.linear.x = linearVel
  5.     data.angular.z = 0
  6.     publisher.publish(twist)

方式二:

  1. runDistance = 0
  2. while runDistance<distance:
  3.     data = Twist()
  4.     data.linear.x = linearVel
  5.     data.angular.z = 0
  6.     publisher.publish(twist)
  7.     runDistance += linearVel / hz

方式三:

  1. while calcDistance(srcX, srcY, distX, distY) > 0.1:
  2.       // 获取srcX，srcY
  3.       srcX = pose->x;
  4.       srcY = pose->y;
  6.       //设置速度
  7.       data = Twist()
  8.       data.linear.x = linearVel
  9.       data.angular.z = 0
  10.       publisher.publish(twist)
  11.       rate.sleep();
  12. }

思考：是否存在完美的事物

  • 速度是否是绝对平均
  • 距离差值是否是绝对为0
  • 时间是否绝对为预期