class Party:
def __init__(self):
# self.bridge = CvBridge()
# self.image_received = False
self.encLeft_received = False
self.encRight_received = False
self.apriltagStatus_received = False
self.apriltagID_received = False
self.apriltag_detection_ID = None
self.taskONE = False
self.taskTWO = False
self.MAX_LIN_VEL = 0.02
self.MAX_ANG_VEL = 0.03
# set PID values for panning
self.panP = 0.5
self.panI = 0
self.panD = 0
# set PID values for tilting
self.tiltP = 0.5
self.tiltI = 0
self.tiltD = 0.5
# create a PID and initialize it
self.panPID = PID(self.panP, self.panI, self.panD)
self.tiltPID = PID(self.tiltP, self.tiltI, self.tiltD)
self.panPID.initialize()
self.tiltPID.initialize()
self.partyTwist = Twist()
self.resetLeft = Bool()
self.resetRight = Bool()
rospy.logwarn("Party Node [ONLINE]...")
# rospy shutdown
rospy.on_shutdown(self.cbShutdown)
# Subscribe to Int64 msg
self.encLeft_topic = "/val_encLeft"
self.encLeft_sub = rospy.Subscriber(self.encLeft_topic, Int64,
self.cbEncoderLeft)
# Subscribe to Int64 msg
self.encRight_topic = "/val_encRight"
self.encRight_sub = rospy.Subscriber(self.encRight_topic, Int64,
self.cbEncoderRight)
# Subscribe to Bool msg
self.apriltagStatus_topic = "/apriltag_detection_status"
self.apriltagStatus_sub = rospy.Subscriber(
self.apriltagStatus_topic, Bool, self.cbAprilTagDetectionStatus)
# Subscribe to Int64 msg
self.apriltagID_topic = "/apriltag_detection_ID"
self.apriltagID_sub = rospy.Subscriber(self.apriltagID_topic, Int64,
self.cbAprilTagDetectionID)
# Subscribe to objCenter msg
self.objCoord_topic = "/objCoord"
self.objCoord_sub = rospy.Subscriber(self.objCoord_topic, objCoord,
self.cbObjCoord)
# Subscribe to CameraInfo msg
self.telloCameraInfo_topic = "/cv_camera/camera_info_converted"
self.telloCameraInfo_sub = rospy.Subscriber(self.telloCameraInfo_topic,
CameraInfo,
self.cbCameraInfo)
# Publish to Twist msg
self.partyTwist_topic = "/cmd_vel"
self.partyTwist_pub = rospy.Publisher(self.partyTwist_topic,
Twist,
queue_size=10)
# Subscribe to Int64 msg
self.rstEncLeft_topic = "/rstEncLeft"
self.rstEncLeft_pub = rospy.Publisher(self.rstEncLeft_topic,
Bool,
queue_size=10)
# Subscribe to Int64 msg
self.rstEncRight_topic = "/rstEncRight"
self.rstEncRight_pub = rospy.Publisher(self.rstEncRight_topic,
Bool,
queue_size=10)
# Allow up to one second to connection
rospy.sleep(1)
# Get Encoder reading
def cbEncoderLeft(self, msg):
try:
self.val_encLeft = msg.data
except KeyboardInterrupt as e:
print(e)
if self.val_encLeft is not None:
self.encLeft_received = True
else:
self.encLeft_received = False
# Get Encoder reading
def cbEncoderRight(self, msg):
try:
self.val_encRight = msg.data
except KeyboardInterrupt as e:
print(e)
if self.val_encRight is not None:
self.encRight_received = True
else:
self.encRight_received = False
# Get AprilTagDetectionStatus reading
def cbAprilTagDetectionStatus(self, msg):
try:
self.apriltag_detection_status = msg.data
except KeyboardInterrupt as e:
print(e)
if self.apriltag_detection_status is not None:
self.apriltagStatus_received = True
else:
self.apriltagStatus_received = False
# Get AprilTagDetectionID reading
def cbAprilTagDetectionID(self, msg):
try:
self.apriltag_detection_ID = msg.data
except KeyboardInterrupt as e:
print(e)
if self.apriltag_detection_ID is not None:
self.apriltagID_received = True
else:
self.apriltagID_received = False
# Convert image to OpenCV format
def cbCameraInfo(self, msg):
self.imgWidth = msg.width
self.imgHeight = msg.height
# Convert image to OpenCV format
def cbObjCoord(self, msg):
self.objectCoordX = msg.centerX
self.objectCoordY = msg.centerY
# Main #
def cbParty(self):
if self.apriltag_detection_ID == 0:
self.taskONE = True
elif self.apriltag_detection_ID == 1 and self.taskONE == False:
self.taskTWO = True
else:
self.cbStop()
rospy.logwarn("Waiting For Instruction!")
# Task 1: Search for the ArptilTag
if self.taskONE == True:
if (self.apriltag_detection_status == True
and self.apriltag_detection_ID != 0
) or self.val_encLeft >= 1000:
self.cbStop
self.resetLeft.data = True
self.rstEncLeft_pub.publish(self.resetLeft)
self.resetRight.data = True
self.rstEncRight_pub.publish(self.resetRight)
self.taskONE = False
else:
self.cbRotateR()
# Task 2: Detected AprilTag ID: 1 and Tracking
if self.taskTWO == True:
if self.apriltag_detection_status == True and self.apriltag_detection_ID == 1:
self.cbCallErr()
else:
self.taskTWO = False
def cbAprilTag(self):
self.cbPIDerr()
# show information callback
def cbPIDerr(self):
self.panErr, self.panOut = self.cbPIDprocess(self.panPID,
self.objectCoordX,
self.imgWidth // 2)
self.tiltErr, self.tiltOut = self.cbPIDprocess(self.tiltPID,
self.objectCoordY,
self.imgHeight // 2)
def cbPIDprocess(self, pid, objCoord, centerCoord):
# calculate the error
error = centerCoord - objCoord
# update the value
output = pid.update(error)
return error, output
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.partyTwist.linear.x = 0.0
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = -tiltSpeed
elif self.tiltErr < -10:
self.partyTwist.linear.x = 0.0
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = tiltSpeed
else:
self.partyTwist.linear.x = 0.03
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = 0.0
self.partyTwist_pub.publish(self.partyTwist)
def constrain(self, input, low, high):
if input < low:
input = low
elif input > high:
input = high
else:
input = input
return input
def cbMove(self):
self.partyTwist.linear.x = 0.02
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = 0.0
self.partyTwist_pub.publish(self.partyTwist)
def cbStop(self):
self.partyTwist.linear.x = 0.0
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = 0.0
self.partyTwist_pub.publish(self.partyTwist)
# rospy.logwarn("STOP!")
def cbRotateR(self):
self.partyTwist.linear.x = 0.0
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = -0.02
self.partyTwist_pub.publish(self.partyTwist)
# rospy.logwarn("TURN RIGHT!")
# rospy shutdown callback
def cbShutdown(self):
rospy.logerr("Party Node [OFFLINE]...")
self.partyTwist.linear.x = 0.0
self.partyTwist.linear.y = 0.0
self.partyTwist.linear.z = 0.0
self.partyTwist.angular.x = 0.0
self.partyTwist.angular.y = 0.0
self.partyTwist.angular.z = 0.0
self.partyTwist_pub.publish(self.partyTwist)
class CameraAprilTag:
def __init__(self):
self.bridge = CvBridge()
self.detector = apriltag.Detector()
self.objectCoord = objCoord()
self.telloCmdVel = Twist()
self.image_received = False
self.MAX_LIN_VEL = 0.005
self.MAX_ANG_VEL = 0.03
# set PID values for panning
self.panP = 0.5
self.panI = 0
self.panD = 0
# set PID values for tilting
self.tiltP = 1
self.tiltI = 0
self.tiltD = 0
# create a PID and initialize it
self.panPID = PID(self.panP, self.panI, self.panD)
self.tiltPID = PID(self.tiltP, self.tiltI, self.tiltD)
self.panPID.initialize()
self.tiltPID.initialize()
rospy.logwarn("AprilTag Tracking Node [ONLINE]...")
# rospy shutdown
rospy.on_shutdown(self.cbShutdown)
# Subscribe to CompressedImage msg
self.cameraInfo_topic = "/cv_camera/camera_info_converted"
self.cameraInfo_sub = rospy.Subscriber(self.cameraInfo_topic,
CameraInfo, self.cbCameraInfo)
# Subscribe to objCenter msg
self.objCoord_topic = "/objCoord"
self.objCoord_sub = rospy.Subscriber(self.objCoord_topic, objCoord,
self.cbObjCoord)
# Publish to Twist msg
self.telloCmdVel_topic = "/cmd_vel"
self.telloCmdVel_pub = rospy.Publisher(self.telloCmdVel_topic,
Twist,
queue_size=10)
# Allow up to one second to connection
rospy.sleep(1)
# Convert image to OpenCV format
def cbCameraInfo(self, msg):
self.imgWidth = msg.width
self.imgHeight = msg.height
# Convert image to OpenCV format
def cbObjCoord(self, msg):
self.objectCoordX = msg.centerX
self.objectCoordY = msg.centerY
def cbAprilTag(self):
self.cbPIDerr()
# show information callback
def cbPIDerr(self):
self.panErr, self.panOut = self.cbPIDprocess(self.panPID,
self.objectCoordX,
self.imgWidth // 2)
self.tiltErr, self.tiltOut = self.cbPIDprocess(self.tiltPID,
self.objectCoordY,
self.imgHeight // 2)
def cbPIDprocess(self, pid, objCoord, centerCoord):
# calculate the error
error = centerCoord - objCoord
# update the value
output = pid.update(error)
return error, output
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)
# rospy shutdown callback
def cbShutdown(self):
rospy.logerr("AprilTag Tracking Node [OFFLINE]...")