def __init__(self, poseTracker, target, yaw, platformNumber, waitDist=0.1):
#self.Kp,self.Ki,self.Kd = 0.1,20.0,0.0005 #best
self.Kp, self.Ki, self.Kd = 0.2, 0.0, 0.0005
self.moveTime = 0.2
self.waitTime = 0.0
self.tracker = poseTracker
self.target = target
self.waitDist = waitDist
self.worldTarget = self.tracker.body2World(target)[:, 0]
self.processVideo = ProcessVideo()
self.platformNumber = platformNumber
self.centery = 360 / 2.0
self.centerx = 640 / 2.0
self.pub = rospy.Publisher('ardrone/tracker', tracker)
self.track = tracker()
self.platform = [0, 0, 0]
self.filterSize = 50
self.buff = np.repeat(np.asarray([self.worldTarget]).T,
self.filterSize,
axis=1)
self.KpYaw, self.KiYaw, self.KdYaw = (1 / 180.0) * 0.8, 0, 0
self.targetYaw = (yaw + 360) % 360
self.worldPoint = np.asarray([[0, 0, 0]]).T
#the amount of weight we would like to put towards correcting the drones drift by recognizing landmarks
self.correctionRatio = 0.9999
def __init__(self, platformColor, hoverAltitude):
self.platformColor = platformColor
self.hoverAltitude = hoverAltitude
self.processVideo = ProcessVideo()
self.moveTime = 0
self.waitTime = 0.10
def __init__(self, platformColor):
self.platformColor = platformColor
self.hoverAltitude = hoverAltitude
self.processVideo = ProcessVideo()
self.moveTime = 0.2
self.waitTime = 0
rospy.logwarn("test4")
def __init__(self, lineColor, speedModifier=0.5, radiusThresh=255):
self.lineColor = lineColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.moveTime = 0.25
self.waitTime = 0.10
def __init__(self, platformColor, speedModifier=0.3, radiusThresh=155):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.moveTime = 0.20
self.waitTime = 0.10
def __init__(self, platformColor, settingsPath):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, Kp=P, Ki=I, Kd=D)
self.moveTime = 0.2
self.waitTime = 0.1
def __init__(self, lineColor, speed=0.4):
self.lineColor = lineColor
self.speed = speed
self.processVideo = ProcessVideo()
self.moveTime = 0.45
self.waitTime = 0.1
self.prevAngle = None
self.prevAngleCount = 0
self.prevAngleCountMax = 185
def __init__(self, platformColor, lineColor, speedModifier = 0.6, radiusThresh = 170):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
self.speedModifier = speedModifier
self.radiusThresh = radiusThresh
self.lineColor = lineColor
self.moveTime = 0.35
self.waitTime = 0.10
self.bestPlatformFound = None
def __init__(self, platformColor):
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P,I,D = 0.0054, 0.006352, 0.0011475
#P,I,D = 0.00405, 0.00285, 0
self.pid = PIDController(Kp = P, Ki = I, Kd = D)
self.moveTime = 0.2
self.waitTime = 0
self.pub_pid_xspeed = rospy.Publisher('pid_xspeed', Float32, queue_size = 10)
self.pub_pid_yspeed = rospy.Publisher('pid_yspeed', Float32, queue_size = 10)
self.pub_pid_in_alt = rospy.Publisher('pid_in_alt', Int32, queue_size = 10)
rate = rospy.Rate(5)
def __init__(self, orientation, lineColor, platformColor, settingsPath):
if orientation != "PARALLEL" and orientation != "PERPENDICULAR":
raise Exception("Orientation not recognized.")
else:
self.orientation = orientation
self.lineColor = lineColor
self.platformColor = platformColor
self.processVideo = ProcessVideo()
P, I, D = self.GetSettings(settingsPath)
self.pid = PIDController(360, 640, P, I, D)
self.moveTime = 0.2
self.waitTime = 0.1
def __init__(self, image):
self.pid = PIDController()
self.process = ProcessVideo()
#take input as bgr image, given orange is visible, will command to hover ontop of it
def HoverOnCheckpoint(self, image):
# calculating cx,cy,xspeed,yspeed
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
self.pid.UpdateDeltaTime()
self.pid.SetPoint(orang_image)
self.pid.UpdateError(self.cx, self.cy)
self.pid.SetPIDTerm()
xspeed, yspeed = self.pid.GetPIDValues()
self.pid.DrawArrow(orange_image, xspeed, yspeed)
return xspeed, yspeed
def OrienttoLine(self, image):
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
blue_image = self.process.DetectColor(image, 'blue')
angle = self.process.ShowLine(blue_image)
yawspeed = self.process.LineOrientation(blue_image, angle, 5)
return yawspeed
def OrienttoObject(self, image):
orange_image = self.process.DetectColor(image, 'orange')
self.cx, self.cy = self.process.CenterofMass(orange_image)
green_image = self.process.DetectColor(image, 'green')
angle = self.process.ShowLine(green_image)
yawspeed = self.process.ObjectOrientation(green_image, angle, 5)
return yawspeed
def __init__(self,
poseTracker,
target,
yaw,
platformNumber,
waitDist=0.13):
#self.Kp,self.Ki,self.Kd = 0.1,20.0,0.0005 #best
#self.Kp,self.Ki,self.Kd = 0.2,0.0,0.0005
#self.Kp,self.Ki,self.Kd = 0.21,0.0,0.0006
self.Kp, self.Ki, self.Kd = 0.17, 10, 0.0005
self.Kpz = 0.1
self.KpYaw, self.KiYaw, self.KdYaw = (2 / 90.), 0, 0
self.targetYaw = -yaw
self.moveTime = 0.2
self.waitTime = 0.0
self.tracker = poseTracker
self.target = target
self.target[2] = target[2] - self.tracker.translation[2]
self.waitDist = waitDist
self.worldTarget = self.tracker.body2World(target)[:, 0]
self.processVideo = ProcessVideo()
self.platformNumber = platformNumber
self.centery = 360 / 2.0
self.centerx = 640 / 2.0
self.pub = rospy.Publisher('ardrone/tracker', tracker)
self.track = tracker()
self.platform = [0, 0, 0]
self.filterSize = 30
self.platformBuff = np.repeat(np.asarray([self.worldTarget]).T,
self.filterSize,
axis=1)
self.heightBuff = np.zeros(4)
self.yawBuff = np.zeros(4)
self.worldPoint = np.asarray([[0, 0, 0]]).T
self.lastLocation = self.tracker.translation
#the amount of weight we would like to put towards correcting the drones drift by recognizing landmarksgggs
self.correctionRatio = 0.9999
def __init__(self,
orientation,
lineColor,
platformColor,
hoverAltitude,
heightTolerance=50,
yOffset=0,
ySizeOffset=0):
if orientation != "PARALLEL" and orientation != "PERPENDICULAR":
raise Exception("Orientation not recognized.")
else:
self.orientation = orientation
self.lineColor = lineColor
self.platformColor = platformColor
self.hoverAltitude = hoverAltitude
self.processVideo = ProcessVideo()
self.prevCenter = None
self.forceCenter = None
self.prevAngle = None
self.heightTolerance = heightTolerance
self.yOffset = yOffset
self.ySizeOffset = ySizeOffset
def __init__(self, platformColor, tolerance):
self.platformColor = platformColor
self.tolerance = tolerance
self.processVideo = ProcessVideo()
def __init__(self):
#self.Kp,self.Ki,self.Kd = 0.11,0.0,0.0004
#self.Kp,self.Ki,self.Kd = 0.1,20.0,0.0005 #best
self.processVideo = ProcessVideo()
def __init__(self):
super(FlightstatsReceiver, self).__init__()
# Subscribe to the navdata topic, and call self.ProcessNavdata whenever
# update navdata messages are recieved
self.navdataSub = rospy.Subscriber('/ardrone/navdata', Navdata, self.UpdateNavdata)
self.altitudeSub = rospy.Subscriber('/ardrone/navdata_altitude', navdata_altitude, self.UpdateAltitude)
#self.video=rospy.Subscriber('/ardrone/image_raw', Image, self.VideoUpdate )
# dictionary will hold a useful subset of available flight info
# Key is the variable name; value is (description, value, units, (optional) direction string following units)
self.defaultValue = "?"
self.flightInfo = collections.OrderedDict()
self.flightInfo["batteryPercent"]=["Battery Left: ", self.defaultValue, "%", ""]
self.flightInfo["state"]=["Status: ", self.defaultValue, "", ""]
self.flightInfo["altitude"]=["Drone Altitude: ", self.defaultValue, "mm", ""]
self.flightInfo["altitude_raw"]=["Drone Raw Altitude: ", self.defaultValue, "mm", ""]
self.flightInfo["SVCLAltitude"] = ["SVCL Altitude: ", -1, "mm", ""]
self.flightInfo["center"] = ["Inferred Center: ", self.defaultValue, "", ""]
self.flightInfo["lastCenter"] = ["Previous Center: ", (None,None), "", ""]
self.flightInfo["lastRecordedCenter"] = ["Last Recorded Algorithm Center: ", (None,None), "", ""]
self.flightInfo["allCenters"] = ["All Centers: ", self.defaultValue, "", ""]
self.flightInfo["rotX"]=["Left/Right Tilt: ", self.defaultValue, u'\N{DEGREE SIGN}', ""]
self.flightInfo["rotY"]=["Front/Back Tilt: ", self.defaultValue, u'\N{DEGREE SIGN}', ""]
self.flightInfo["rotZ"]=["Rotation Amount: ", self.defaultValue, u'\N{DEGREE SIGN}', ""]
self.flightInfo["velY"]=["Left/Right Velocity: ", self.defaultValue, "mm/s", ""]
self.flightInfo["velX"]=["Forwards/Backwards Velocity: ", self.defaultValue, "mm/s", ""]
self.flightInfo["velZ"]=["Up/Down Velocity: ", self.defaultValue, "mm/s", ""]
self.flightInfo["accelZ"]=["Up/Down Acceleration: ", self.defaultValue, "mm/s", ""]
self.flightInfo["dispLR"]=["Left/Right Displacement: ", self.defaultValue, "mm", ""]
self.flightInfo["dispFB"]=["Forwards/Backwards Displacement: ", self.defaultValue, "mm", ""]
self.flightInfo["dispUD"]=["Up/Down Displacement: ", self.defaultValue, "mm", ""]
self.flightInfo["segImage"] = [None]
self.LRDisplacement = 0.0
self.FBDisplacement = 0.0
self.UDDisplacement = 0.0
self.oldTime = rospy.Time.now()
self.bridge = CvBridge()
self.processVideo = ProcessVideo()
# sometimes the altitude doesn't start at 0; "zero balances" the drone such that where it started is considered 0 altitude
self.zeroBalanced = False
self.zeroAltitude = 0
# counter is designed to throttle the lag that comes with executing videoupdate too often
# a higher videoUpdateMax will make everything run faster, but getting height/center updates will be slower
# describes a ratio: compute/rest
self.computeMax = 1
self.restMax = 0
self.counter = 0
self.lastLocation = (None,None)
self.lastLoc = (None,None)
self.lastCenter = (None,None)
self.lastCenterCount = 0
self.lastCenterMax = 8
def __init__(self, color):
self.color = color
self.processVideo = ProcessVideo()