def cbCallErr(self):
self.cbAprilTag()
panSpeed = mapped(abs(self.panOut), 0, self.imgWidth // 2, 0,
self.MAX_ANG_VEL)
tiltSpeed = mapped(abs(self.tiltOut), 0, self.imgHeight // 2, 0,
self.MAX_LIN_VEL)
# if self.panOut < 0:
# self.telloCmdVel.linear.x = panSpeed
# elif self.panOut > 0:
# self.telloCmdVel.linear.x = -panSpeed
# else:
# self.telloCmdVel.linear.x = 0
if self.tiltOut > 0.04:
self.telloCmdVel.angular.z = -panSpeed
elif self.tiltOut < -0.04:
self.telloCmdVel.angular.z = panSpeed
else:
self.telloCmdVel.angular.z = 0
self.telloCmdVel.linear.x = 0.0
self.telloCmdVel.linear.y = 0.0
self.telloCmdVel.linear.z = 0.0
self.telloCmdVel.angular.x = 0.0
self.telloCmdVel.angular.y = 0.0
self.telloCmdVel_pub.publish(self.telloCmdVel)
def cbCallErr(self):
self.cbAprilTag()
panSpeed = mapped(abs(self.panOut), 0, self.imgWidth // 2, 0,
self.MAX_LIN_VEL)
tiltSpeed = mapped(abs(self.tiltOut), 0, self.imgHeight // 2, 0,
self.MAX_ANG_VEL)
panSpeed = self.constrain(panSpeed, -self.MAX_LIN_VEL,
self.MAX_LIN_VEL)
tiltSpeed = self.constrain(tiltSpeed, -self.MAX_ANG_VEL,
self.MAX_ANG_VEL)
if self.tiltErr > 10:
self.telloCmdVel.angular.z = -tiltSpeed
elif self.tiltErr < -10:
self.telloCmdVel.angular.z = tiltSpeed
else:
self.telloCmdVel.angular.z = 0.0
self.telloCmdVel.linear.x = 0.0
self.telloCmdVel.linear.y = 0.0
self.telloCmdVel.linear.z = 0.0
self.telloCmdVel.angular.x = 0.0
self.telloCmdVel.angular.y = 0.0
self.telloCmdVel_pub.publish(self.telloCmdVel)