def Update(self, contourinfo):
if (self.initialized):
self.count += 1
#rospy.logwarn(contourinfo)
if (contourinfo is not None) and (not np.isnan(contourinfo.x)) and (not np.isnan(contourinfo.y)) and (not np.isnan(contourinfo.angle)):
bValidContourinfo = True
else:
bValidContourinfo = False
# Use null contourinfo.
contourinfoNone = Contourinfo()
contourinfoNone.header = Header()
contourinfoNone.x = None
contourinfoNone.y = None
contourinfoNone.angle = None
contourinfoNone.area = None
contourinfoNone.ecc = None
contourinfoNone.imgRoi = None
contourinfo = contourinfoNone
self.contourinfo = contourinfo
self.dt = self.contourinfo.header.stamp - self.stampPrev
self.stampPrev = self.contourinfo.header.stamp
#with self.lock:
# SetDict.SetWithOverwrite(self.params, rospy.get_param('/',{}))
# Update the position & orientation filters
self.isVisible = False
if (bValidContourinfo):
# Update min/max ecc & area.
if contourinfo.ecc < self.eccMin:
self.eccMin = contourinfo.ecc
if self.eccMax < contourinfo.ecc:
self.eccMax = contourinfo.ecc
if contourinfo.area < self.areaMin:
self.areaMin = contourinfo.area
if self.areaMax < contourinfo.area:
self.areaMax = contourinfo.area
a = self.dt.to_sec() / 60
if (self.updated):
self.areaMean = (1-a)*self.areaMean + a*contourinfo.area
self.eccMean = (1-a)*self.eccMean + a*contourinfo.ecc
else:
#if (not np.isnan(contourinfo.area)):
self.areaMean = contourinfo.area
#if (not np.isnan(contourinfo.ecc)):
self.eccMean = contourinfo.ecc
self.isVisible = True
(xKalman,yKalman,vxKalman,vyKalman) = self.kfState.Update((self.contourinfo.x, self.contourinfo.y), contourinfo.header.stamp.to_sec())
(zKalman, vzKalman) = (0.0, 0.0)
#(xKalman,yKalman) = (self.contourinfo.x,self.contourinfo.y) # Unfiltered.
self.lpAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec())
self.apAngleContour.Update(contourinfo.angle, contourinfo.header.stamp.to_sec())
(angleLeft, angleRight) = self.GetWingAngles(contourinfo)
if (np.abs(self.contourinfo.x)>9999) or (np.abs(xKalman)>9999):
rospy.logwarn ('FLY LARGE CONTOUR, x,x=%s, %s. Check your background image, lighting, and the parameter tracking/diff_threshold.' % (self.contourinfo.x, xKalman))
else: # We don't have a contourinfo.
#rospy.logwarn('FLY No contour seen for %s; check your nFlies parameter.' % self.name)
(xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update(None, contourinfo.header.stamp.to_sec())
(zKalman, vzKalman) = (0.0, 0.0)
(angleLeft, angleRight) = (np.pi, np.pi)
if (contourinfo.angle is None):
contourinfo.angle = 0.0
# If good data, then use it.
if (xKalman is not None) and (yKalman is not None):
x = xKalman
y = yKalman
z = zKalman
vx = vxKalman
vy = vyKalman
vz = vzKalman
else: # Use the unfiltered data for the case where the filters return None results.
rospy.logwarn('FLY Object %s not yet initialized, %s' % (self.name, [self.contourinfo.x, self.contourinfo.y]))
x = 0.0#self.contourinfo.x
y = 0.0#self.contourinfo.y
z = 0.0
vx = 0.0
vy = 0.0
vz = 0.0
# Store the latest state.
self.state.header.stamp = self.contourinfo.header.stamp
self.state.pose.position.x = x
self.state.pose.position.y = y
self.state.pose.position.z = z
#self.state.pose.orientation comes from self.GetResolvedAngleFiltered() later.
self.state.velocity.linear.x = vx
self.state.velocity.linear.y = vy
self.state.velocity.linear.z = vz
self.state.wings.left.angle = angleLeft
self.state.wings.right.angle = angleRight
# HACK
#self.state.pose.position.x = -4.1
#self.state.pose.position.y = -4.0
# HACK
if 'Robot' not in self.name:
angle = self.GetResolvedAngleFiltered()
else:
angle = contourinfo.angle
(self.state.pose.orientation.x, self.state.pose.orientation.y, self.state.pose.orientation.z, self.state.pose.orientation.w) = tf.transformations.quaternion_about_axis(angle, (0,0,1))
# Get the angular velocities.
if self.isVisible:
try:
((vx2,vy2,vz2),(wx,wy,wz)) = self.tfrx.lookupTwist(self.name, self.state.header.frame_id, self.state.header.stamp-self.dtVelocity, self.dtVelocity)
except (tf.Exception, AttributeError), e:
((vx2,vy2,vz2),(wx,wy,wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
else:
((vx2,vy2,vz2),(wx,wy,wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0, 0.0))
# Fix glitches due to flip orientation.
if wz>6.28:
wz = self.lpWz.GetValue()
self.state.velocity.angular.x = self.lpWx.Update(wx, self.state.header.stamp.to_sec())
self.state.velocity.angular.y = self.lpWy.Update(wy, self.state.header.stamp.to_sec())
self.state.velocity.angular.z = self.lpWz.Update(wz, self.state.header.stamp.to_sec())
speedPre = np.linalg.norm([self.state.velocity.linear.x, self.state.velocity.linear.y, self.state.velocity.linear.z])
self.speed = self.lpSpeed.Update(speedPre, self.state.header.stamp.to_sec())
# Update the most recent angle of travel, and flip state.
self.SetAngleOfTravel()
self.UpdateFlipState()
if (bValidContourinfo):
# Send the Raw transform.
if self.isVisible:
self.tfbx.sendTransform((self.contourinfo.x,
self.contourinfo.y,
0.0),
tf.transformations.quaternion_about_axis(self.contourinfo.angle, (0,0,1)),
self.contourinfo.header.stamp,
self.name+'Contour',
'Arena')
# Send the Filtered transform.
if self.state.pose.position.x is not None:
q = self.state.pose.orientation
self.tfbx.sendTransform((self.state.pose.position.x,
self.state.pose.position.y,
self.state.pose.position.z),
(q.x, q.y, q.z, q.w),
self.state.header.stamp,
self.name,
self.state.header.frame_id)
# Send the Forecast transform.
if self.state.pose.position.x is not None:
poseForecast = Pose()#copy.copy(self.state.pose)
poseForecast.position.x = self.state.pose.position.x + self.state.velocity.linear.x * self.params['tracking']['dtForecast']
poseForecast.position.y = self.state.pose.position.y + self.state.velocity.linear.y * self.params['tracking']['dtForecast']
poseForecast.position.z = self.state.pose.position.z + self.state.velocity.linear.z * self.params['tracking']['dtForecast']
q = self.state.pose.orientation
self.tfbx.sendTransform((poseForecast.position.x,
poseForecast.position.y,
poseForecast.position.z),
(q.x, q.y, q.z, q.w),
self.state.header.stamp,
self.name+'Forecast',
self.state.header.frame_id)
# Publish a 3D model marker for the robot.
if 'Robot' in self.name:
markerRobot = Marker(header=Header(stamp = self.state.header.stamp,
frame_id='Arena'),
ns=self.name,
id=1,
type=Marker.CYLINDER,
action=0,
pose=Pose(position=Point(x=self.state.pose.position.x,
y=self.state.pose.position.y,
z=self.state.pose.position.z),
orientation=self.state.pose.orientation),
scale=Vector3(x=self.params['tracking']['robot']['width'],
y=self.params['tracking']['robot']['length'],
z=self.params['tracking']['robot']['height']),
color=ColorRGBA(a=0.7,
r=0.5,
g=0.5,
b=0.5),
lifetime=rospy.Duration(0.5))
self.pubMarker.publish(markerRobot)
self.updated = True
def ProcessContourinfoLists(self):
contourinfolistsPixels = None
with self.lockBuffer:
if (self.bufferContourinfolists[self.iBufferWorking] is not None):
contourinfolistsPixels = self.bufferContourinfolists[self.iBufferWorking]
# Mark this buffer entry as available for loading.
self.bufferContourinfolists[self.iBufferWorking] = None
# Go to the next slot.
self.iBufferWorking = (self.iBufferWorking+1) % len(self.bufferContourinfolists)
if (contourinfolistsPixels is not None):
if (self.initialized):
with self.lockThreads:
try:
# Publish state of the EndEffector for ourselves (so we get the EE via bag-recordable message rather than via tf.
if (0 < self.nRobots):
try:
stamp = self.tfrx.getLatestCommonTime('Arena', 'EndEffector')
(translationEE,rotationEE) = self.tfrx.lookupTransform('Arena', 'EndEffector', stamp)
except tf.Exception, e:
pass # No transform - Either the EE is still initializing, or because we're replaying a bag file.
#rospy.logwarn ('CI EndEffector not yet initialized: %s' % e)
else:
self.pubStateEE.publish(MsgFrameState(header=Header(stamp=contourinfolistsPixels.header.stamp,
frame_id='Arena'),
pose=Pose(position=Point(translationEE[0],translationEE[1],translationEE[2]),
orientation=Quaternion(rotationEE[0],rotationEE[1],rotationEE[2],rotationEE[3]))
)
)
# Apply the arena mask, and transform to arena frame.
contourinfolistsPixels = self.FilterContourinfolistsWithinMask(contourinfolistsPixels)
contourinfolists = self.TransformContourinfolistsArenaFromCamera(contourinfolistsPixels)
# Repackage the contourinfolists as a list of contourinfos, ignoring any that are in an exclusion zone.
self.contourinfo_list = []
for i in range(len(contourinfolists.x)):
inExclusionzone = False
if (self.enabledExclusionzone):
# See if the contourinfo is in any of the exclusionzones.
for k in range(len(self.pointExclusionzone_list)):
inExclusionzone = inExclusionzone or (np.linalg.norm([contourinfolists.x[i]-self.pointExclusionzone_list[k].x,
contourinfolists.y[i]-self.pointExclusionzone_list[k].y]) < self.radiusExclusionzone_list[k])
if (not inExclusionzone):
contourinfo = Contourinfo()
contourinfo.header = contourinfolists.header
contourinfo.x = contourinfolists.x[i]
contourinfo.y = contourinfolists.y[i]
if (not np.isnan(contourinfolists.angle[i])):
contourinfo.angle = contourinfolists.angle[i]
else:
contourinfo.angle = self.contouranglePrev
self.contouranglePrev = contourinfo.angle
contourinfo.area = contourinfolists.area[i]
contourinfo.ecc = contourinfolists.ecc[i]
contourinfo.imgRoi = contourinfolists.imgRoi[i]
self.contourinfo_list.append(contourinfo)
# Figure out who is who in the camera image.
try:
self.mapContourinfoFromObject = self.MapContoursFromObjects()
except IndexError:
self.mapContourinfoFromObject = None
if (self.mapContourinfoFromObject is not None):
# Update the robot state w/ the contourinfo and end-effector positions.
for iRobot in self.iRobot_list:
# If the map points to a contourinfo, then use it.
if (self.mapContourinfoFromObject[iRobot] is not None):
# For the robot, use the end-effector angle instead of the contourinfo angle.
if (False):#(self.stateKinematic is not None):
q = self.stateKinematic.pose.orientation
rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].angle = rpy[2]
# Use this contourinfo.
contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iRobot]]
# Else use the kinematic state, if available.
elif (self.stateKinematic is not None):
q = self.stateKinematic.pose.orientation
rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
area = np.pi * (self.params['tracking']['robot']['width']/2)**2
# Make a 'fake' contourinfo from kinematics.
contourinfo = Contourinfo(header=self.stateKinematic.header,
x=self.stateKinematic.pose.position.x,
y=self.stateKinematic.pose.position.y,
angle=rpy[2],
area=area,
ecc=1.0,
imgRoi=None
)
else:
contourinfo = None
# Update the object.
if (iRobot < len(self.objects)):
self.objects[iRobot].Update(contourinfo)
# Write a file (for getting Kalman covariances, etc).
#data = '%s, %s, %s, %s, %s, %s\n' % (self.stateKinematic.pose.position.x,
# self.stateKinematic.pose.position.y,
# self.objects[iRobot].state.pose.position.x,
# self.objects[iRobot].state.pose.position.y,
# self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].x,
# self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].y)
#self.fidRobot.write(data)
# Update the flies' states.
for iFly in self.iFly_list:
if (self.mapContourinfoFromObject[iFly] is not None):
contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iFly]]
else:
contourinfo = None
if (iFly < len(self.objects)):
self.objects[iFly].Update(contourinfo)
# Write a file.
#if self.mapContourinfoFromObject[1] is not None:
# data = '%s, %s, %s, %s\n' % (self.contourinfo_list[self.mapContourinfoFromObject[1]].x,
# self.contourinfo_list[self.mapContourinfoFromObject[1]].y,
# self.objects[1].state.pose.position.x,
# self.objects[1].state.pose.position.y)
# self.fidFly.write(data)
self.PublishArenaStateFromObjects()
# Publish the visual_state.
if (self.bUseVisualServoing) and (0 in self.iRobot_list) and (0 < len(self.objects)):
self.pubVisualState.publish(self.objects[0].state)
# Publish a marker to indicate the size of the arena.
self.markerArenaOuter.header.stamp = contourinfolists.header.stamp
self.markerArenaInner.header.stamp = contourinfolists.header.stamp
self.pubMarker.publish(self.markerArenaOuter)
self.pubMarker.publish(self.markerArenaInner)
# Publish markers for all the exclusionzones.
if self.enabledExclusionzone:
for marker in self.markerExclusionzone_list:
marker.header.stamp = contourinfolists.header.stamp
self.pubMarker.publish(marker)
except rospy.exceptions.ROSException, e:
rospy.loginfo('CI ROSException: %s' % e)
def __init__(self, name=None, tfrx=None, lock=None):
global globalLock2
if (lock is not None):
self.lock = lock
else:
self.lock = globalLock2
self.initialized = False
self.updated = False
self.name = name
# Read all the params.
with self.lock:
self.params = rospy.get_param('/', {})
defaults = {
'tracking': {
'dtVelocity': 0.2,
'dtForecast': 0.15,
'rcFilterFlip': 3.0,
'rcFilterSpeed': 0.2,
'rcFilterAngle': 0.1,
'rcFilterAngularVel': 0.1,
'rcForeground': 1.0,
'speedThresholdForTravel': 5.0,
'robot': {
'width': 1.0,
'length': 1.0,
'height': 1.0
},
'roi': {
'width': 15,
'height': 15
},
'lengthBody': 9,
'widthBody': 5,
'thresholdBody': 40.0,
'scalarMeanFlySubtraction': 1.0,
'nStdDevWings': 2.0,
'rcWingMean':
10.0, # The time constant for exponentially weighted wing mean.
'rcWingStdDev':
100.0, # The time constant for exponentially weighted wing std dev.
'nonessentialPublish': False,
'nonessentialCalcSuper': False
}
}
SetDict.SetWithPreserve(self.params, defaults)
self.cvbridge = CvBridge()
self.tfrx = tfrx
self.tfbx = tf.TransformBroadcaster()
self.pubMarker = rospy.Publisher('visualization_marker', Marker)
# Nonessential stuff to publish.
if self.params['tracking']['nonessentialPublish']:
self.pubImageRoiMean = rospy.Publisher(self.name + '/image_mean',
Image)
self.pubImageRoi = rospy.Publisher(self.name + '/image', Image)
self.pubImageRoiReg = rospy.Publisher(self.name + '/image_reg',
Image)
self.pubImageRoiWings = rospy.Publisher(self.name + '/image_wings',
Image)
self.pubImageMaskWings = rospy.Publisher(
self.name + '/image_mask_wings', Image)
self.pubImageMaskBody = rospy.Publisher(
self.name + '/image_mask_body', Image)
self.pubLeft = rospy.Publisher(self.name + '/leftwing', Float32)
self.pubLeftIntensity = rospy.Publisher(
self.name + '/leftintensity', Float32)
self.pubLeftMeanIntensity = rospy.Publisher(
self.name + '/leftmean', Float32)
self.pubLeftMeanPlusStdDev = rospy.Publisher(
self.name + '/leftmeanplusstddev', Float32)
self.pubLeftMeanMinusStdDev = rospy.Publisher(
self.name + '/leftmeanminusstddev', Float32)
self.pubLeftDev = rospy.Publisher(self.name + '/leftdev', Float32)
self.pubRight = rospy.Publisher(self.name + '/rightwing', Float32)
self.pubRightIntensity = rospy.Publisher(
self.name + '/rightintensity', Float32)
self.pubRightMeanIntensity = rospy.Publisher(
self.name + '/rightmean', Float32)
self.pubRightMeanPlusStdDev = rospy.Publisher(
self.name + '/rightmeanplusstddev', Float32)
self.pubRightMeanMinusStdDev = rospy.Publisher(
self.name + '/rightmeanminusstddev', Float32)
self.pubRightDev = rospy.Publisher(self.name + '/rightdev',
Float32)
self.pubImageSuper = rospy.Publisher(self.name + '/image_super',
Image)
self.pubImageSuperCount = rospy.Publisher(
self.name + '/image_supercount', Image)
self.pubImageSuperGrad = rospy.Publisher(
self.name + '/image_supergrad', Image)
rospy.logwarn(
'(%s) Filter time constants: rcFilterAngle=%s, rcFilterAngularVel=%s'
% (self.name, self.params['tracking']['rcFilterAngle'],
self.params['tracking']['rcFilterAngularVel']))
self.kfState = filters.KalmanFilter()
self.lpAngle = filters.LowPassCircleFilter(
RC=self.params['tracking']['rcFilterAngle'])
self.lpAngle.SetValue(0.0)
self.lpAngleContour = filters.LowPassHalfCircleFilter(
RC=self.params['tracking']['rcFilterAngle'])
self.lpAngleContour.SetValue(0.0)
self.apAngleContour = filters.LowPassHalfCircleFilter(RC=0.0)
self.apAngleContour.SetValue(0.0)
self.stampPrev = rospy.Time.now()
self.dtVelocity = rospy.Duration(
self.params['tracking']
['dtVelocity']) # Interval over which to calculate velocity.
self.stampPrev = rospy.Time.now()
# Orientation detection stuff.
self.lpFlip = filters.LowPassFilter(
RC=self.params['tracking']['rcFilterFlip'])
self.lpFlip.SetValue(0.0) # (0.5) TEST
self.lpSpeed = filters.LowPassFilter(
RC=self.params['tracking']['rcFilterSpeed'])
self.lpSpeed.SetValue(0.0)
self.angleOfTravelRecent = 0.0
self.contourinfo = Contourinfo()
self.speed = 0.0
self.lpWx = filters.LowPassFilter(
RC=self.params['tracking']['rcFilterAngularVel'])
self.lpWy = filters.LowPassFilter(
RC=self.params['tracking']['rcFilterAngularVel'])
self.lpWz = filters.LowPassFilter(
RC=self.params['tracking']['rcFilterAngularVel'])
self.lpWx.SetValue(0.0)
self.lpWy.SetValue(0.0)
self.lpWz.SetValue(0.0)
self.isVisible = False
self.isDead = False # TO DO: dead fly detection.
self.state = MsgFrameState()
self.state.header.frame_id = 'Arena'
self.state.pose.position.x = 0.0
self.state.pose.position.y = 0.0
self.state.pose.position.z = 0.0
q = tf.transformations.quaternion_about_axis(0.0, (0, 0, 1))
self.state.pose.orientation.x = q[
0] # Note: The orientation is ambiguous as to head/tail.
self.state.pose.orientation.y = q[
1] # Check the self.lpFlip sign to resolve it: use self.ResolvedAngle().
self.state.pose.orientation.z = q[2] #
self.state.pose.orientation.w = q[3] #
self.state.velocity.linear.x = 0.0
self.state.velocity.linear.y = 0.0
self.state.velocity.linear.z = 0.0
self.state.velocity.angular.x = 0.0
self.state.velocity.angular.y = 0.0
self.state.velocity.angular.z = 0.0
self.state.wings.left.angle = 0.0
self.state.wings.right.angle = 0.0
self.eccMin = 999.9
self.eccMean = 0.0
self.eccMax = 0.0
self.areaMin = 999.9
self.areaMean = 1.0
self.areaMax = 0.0
# Wing angle stuff.
self.npfRoiMean = None
self.npfRoiSum = None
self.npMaskWings = None
self.npMaskCircle = np.zeros([
self.params['tracking']['roi']['height'],
self.params['tracking']['roi']['width']
],
dtype=np.uint8)
cv2.circle(self.npMaskCircle,
(int(self.params['tracking']['roi']['height'] / 2),
int(self.params['tracking']['roi']['width'] / 2)),
int(self.params['tracking']['roi']['height'] / 2), 255,
cv.CV_FILLED)
self.meanIntensityLeft = None
self.meanIntensityRight = None
self.varIntensityLeft = None
self.varIntensityRight = None
self.sumIntensityLeft = 0.0
self.sumIntensityRight = 0.0
self.sumSqDevIntensityLeft = 0.0
self.stddevIntensityLeft = 0.0
self.sumSqDevIntensityRight = 0.0
self.stddevIntensityRight = 0.0
self.count = 0
# Super-resolution image.
if self.params['tracking']['nonessentialCalcSuper']:
self.heightSuper = 16 * self.params['tracking']['roi'][
'height'] # The resolution increase for super-res.
self.widthSuper = 16 * self.params['tracking']['roi']['width']
self.npfSuper = np.zeros([self.heightSuper, self.widthSuper],
dtype=np.float)
self.npfSuperSum = np.zeros([self.heightSuper, self.widthSuper],
dtype=np.float)
self.npfSuperCount = np.zeros([self.heightSuper, self.widthSuper],
dtype=np.float)
rospy.logwarn('FLY object added, name=%s' % name)
self.timePrev = rospy.Time.now().to_sec()
self.theta = 0.0
self.initialized = True
def Update(self, contourinfo):
if (self.initialized):
self.count += 1
#rospy.logwarn(contourinfo)
if (contourinfo is not None) and (not np.isnan(
contourinfo.x)) and (not np.isnan(
contourinfo.y)) and (not np.isnan(contourinfo.angle)):
bValidContourinfo = True
else:
bValidContourinfo = False
# Use null contourinfo.
contourinfoNone = Contourinfo()
contourinfoNone.header = Header()
contourinfoNone.x = None
contourinfoNone.y = None
contourinfoNone.angle = None
contourinfoNone.area = None
contourinfoNone.ecc = None
contourinfoNone.imgRoi = None
contourinfo = contourinfoNone
self.contourinfo = contourinfo
self.dt = self.contourinfo.header.stamp - self.stampPrev
self.stampPrev = self.contourinfo.header.stamp
#with self.lock:
# SetDict.SetWithOverwrite(self.params, rospy.get_param('/',{}))
# Update the position & orientation filters
self.isVisible = False
if (bValidContourinfo):
# Update min/max ecc & area.
if contourinfo.ecc < self.eccMin:
self.eccMin = contourinfo.ecc
if self.eccMax < contourinfo.ecc:
self.eccMax = contourinfo.ecc
if contourinfo.area < self.areaMin:
self.areaMin = contourinfo.area
if self.areaMax < contourinfo.area:
self.areaMax = contourinfo.area
a = self.dt.to_sec() / 60
if (self.updated):
self.areaMean = (1 -
a) * self.areaMean + a * contourinfo.area
self.eccMean = (1 - a) * self.eccMean + a * contourinfo.ecc
else:
#if (not np.isnan(contourinfo.area)):
self.areaMean = contourinfo.area
#if (not np.isnan(contourinfo.ecc)):
self.eccMean = contourinfo.ecc
self.isVisible = True
(xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update(
(self.contourinfo.x, self.contourinfo.y),
contourinfo.header.stamp.to_sec())
(zKalman, vzKalman) = (0.0, 0.0)
#(xKalman,yKalman) = (self.contourinfo.x,self.contourinfo.y) # Unfiltered.
self.lpAngleContour.Update(contourinfo.angle,
contourinfo.header.stamp.to_sec())
self.apAngleContour.Update(contourinfo.angle,
contourinfo.header.stamp.to_sec())
(angleLeft, angleRight) = self.GetWingAngles(contourinfo)
if (np.abs(self.contourinfo.x) > 9999) or (np.abs(xKalman) >
9999):
rospy.logwarn(
'FLY LARGE CONTOUR, x,x=%s, %s. Check your background image, lighting, and the parameter tracking/diff_threshold.'
% (self.contourinfo.x, xKalman))
else: # We don't have a contourinfo.
#rospy.logwarn('FLY No contour seen for %s; check your nFlies parameter.' % self.name)
(xKalman, yKalman, vxKalman, vyKalman) = self.kfState.Update(
None, contourinfo.header.stamp.to_sec())
(zKalman, vzKalman) = (0.0, 0.0)
(angleLeft, angleRight) = (np.pi, np.pi)
if (contourinfo.angle is None):
contourinfo.angle = 0.0
# If good data, then use it.
if (xKalman is not None) and (yKalman is not None):
x = xKalman
y = yKalman
z = zKalman
vx = vxKalman
vy = vyKalman
vz = vzKalman
else: # Use the unfiltered data for the case where the filters return None results.
rospy.logwarn(
'FLY Object %s not yet initialized, %s' %
(self.name, [self.contourinfo.x, self.contourinfo.y]))
x = 0.0 #self.contourinfo.x
y = 0.0 #self.contourinfo.y
z = 0.0
vx = 0.0
vy = 0.0
vz = 0.0
# Store the latest state.
self.state.header.stamp = self.contourinfo.header.stamp
self.state.pose.position.x = x
self.state.pose.position.y = y
self.state.pose.position.z = z
#self.state.pose.orientation comes from self.GetResolvedAngleFiltered() later.
self.state.velocity.linear.x = vx
self.state.velocity.linear.y = vy
self.state.velocity.linear.z = vz
self.state.wings.left.angle = angleLeft
self.state.wings.right.angle = angleRight
# HACK
#self.state.pose.position.x = -4.1
#self.state.pose.position.y = -4.0
# HACK
if 'Robot' not in self.name:
angle = self.GetResolvedAngleFiltered()
else:
angle = contourinfo.angle
(self.state.pose.orientation.x, self.state.pose.orientation.y,
self.state.pose.orientation.z, self.state.pose.orientation.w
) = tf.transformations.quaternion_about_axis(angle, (0, 0, 1))
# Get the angular velocities.
if self.isVisible:
try:
((vx2, vy2, vz2), (wx, wy, wz)) = self.tfrx.lookupTwist(
self.name, self.state.header.frame_id,
self.state.header.stamp - self.dtVelocity,
self.dtVelocity)
except (tf.Exception, AttributeError), e:
((vx2, vy2, vz2), (wx, wy, wz)) = ((0.0, 0.0, 0.0),
(0.0, 0.0, 0.0))
else:
((vx2, vy2, vz2), (wx, wy, wz)) = ((0.0, 0.0, 0.0), (0.0, 0.0,
0.0))
# Fix glitches due to flip orientation.
if wz > 6.28:
wz = self.lpWz.GetValue()
self.state.velocity.angular.x = self.lpWx.Update(
wx, self.state.header.stamp.to_sec())
self.state.velocity.angular.y = self.lpWy.Update(
wy, self.state.header.stamp.to_sec())
self.state.velocity.angular.z = self.lpWz.Update(
wz, self.state.header.stamp.to_sec())
speedPre = np.linalg.norm([
self.state.velocity.linear.x, self.state.velocity.linear.y,
self.state.velocity.linear.z
])
self.speed = self.lpSpeed.Update(speedPre,
self.state.header.stamp.to_sec())
# Update the most recent angle of travel, and flip state.
self.SetAngleOfTravel()
self.UpdateFlipState()
if (bValidContourinfo):
# Send the Raw transform.
if self.isVisible:
self.tfbx.sendTransform(
(self.contourinfo.x, self.contourinfo.y, 0.0),
tf.transformations.quaternion_about_axis(
self.contourinfo.angle,
(0, 0, 1)), self.contourinfo.header.stamp,
self.name + 'Contour', 'Arena')
# Send the Filtered transform.
if self.state.pose.position.x is not None:
q = self.state.pose.orientation
self.tfbx.sendTransform((self.state.pose.position.x,
self.state.pose.position.y,
self.state.pose.position.z),
(q.x, q.y, q.z, q.w),
self.state.header.stamp, self.name,
self.state.header.frame_id)
# Send the Forecast transform.
if self.state.pose.position.x is not None:
poseForecast = Pose() #copy.copy(self.state.pose)
poseForecast.position.x = self.state.pose.position.x + self.state.velocity.linear.x * self.params[
'tracking']['dtForecast']
poseForecast.position.y = self.state.pose.position.y + self.state.velocity.linear.y * self.params[
'tracking']['dtForecast']
poseForecast.position.z = self.state.pose.position.z + self.state.velocity.linear.z * self.params[
'tracking']['dtForecast']
q = self.state.pose.orientation
self.tfbx.sendTransform(
(poseForecast.position.x, poseForecast.position.y,
poseForecast.position.z), (q.x, q.y, q.z, q.w),
self.state.header.stamp, self.name + 'Forecast',
self.state.header.frame_id)
# Publish a 3D model marker for the robot.
if 'Robot' in self.name:
markerRobot = Marker(
header=Header(stamp=self.state.header.stamp,
frame_id='Arena'),
ns=self.name,
id=1,
type=Marker.CYLINDER,
action=0,
pose=Pose(position=Point(x=self.state.pose.position.x,
y=self.state.pose.position.y,
z=self.state.pose.position.z),
orientation=self.state.pose.orientation),
scale=Vector3(
x=self.params['tracking']['robot']['width'],
y=self.params['tracking']['robot']['length'],
z=self.params['tracking']['robot']['height']),
color=ColorRGBA(a=0.7, r=0.5, g=0.5, b=0.5),
lifetime=rospy.Duration(0.5))
self.pubMarker.publish(markerRobot)
self.updated = True
def ContourinfoLists_callback(self, contourinfolistsPixels):
if self.initialized:
if self.nRobots>0:
# Publish state of the EndEffector for ourselves (so we get the EE via bag-recordable message rather than via tf.
try:
stamp = self.tfrx.getLatestCommonTime('Arena', 'EndEffector')
(translationEE,rotationEE) = self.tfrx.lookupTransform('Arena', 'EndEffector', stamp)
except tf.Exception, e:
pass # No transform - Either the EE is still initializing, or because we're replaying a bag file.
#rospy.logwarn ('CI EndEffector not yet initialized: %s' % e)
else:
self.pubStateEE.publish(MsgFrameState(header=Header(stamp=contourinfolistsPixels.header.stamp,
frame_id='Arena'),
pose=Pose(position=Point(translationEE[0],translationEE[1],translationEE[2]),
orientation=Quaternion(rotationEE[0],rotationEE[1],rotationEE[2],rotationEE[3]))
)
)
contourinfolistsPixels = self.FilterContourinfoWithinMask(contourinfolistsPixels)
contourinfolists = self.TransformContourinfoArenaFromCamera(contourinfolistsPixels)
# Create a null contourinfo.
contourinfoNone = Contourinfo()
contourinfoNone.header = contourinfolists.header
contourinfoNone.x = None
contourinfoNone.y = None
contourinfoNone.angle = None
contourinfoNone.area = None
contourinfoNone.ecc = None
contourinfoNone.imgRoi = None
# Repackage the contourinfolists into a list of contourinfos, ignoring any that are in the exclusion zone.
self.contourinfo_list = []
for i in range(len(contourinfolists.x)):
inExclusionzone = False
if (self.enabledExclusionzone):
# See if the contourinfo is in any of the exclusionzones.
for k in range(len(self.pointExclusionzone_list)):
inExclusionzone = inExclusionzone or (N.linalg.norm([contourinfolists.x[i]-self.pointExclusionzone_list[k].x,
contourinfolists.y[i]-self.pointExclusionzone_list[k].y]) < self.radiusExclusionzone_list[k])
if (not inExclusionzone):
contourinfo = Contourinfo()
contourinfo.header = contourinfolists.header
contourinfo.x = contourinfolists.x[i]
contourinfo.y = contourinfolists.y[i]
if (contourinfolists.angle[i] != 99.9) and (not N.isnan(contourinfolists.angle[i])):
contourinfo.angle = contourinfolists.angle[i]
else:
contourinfo.angle = self.contouranglePrev
self.contouranglePrev = contourinfo.angle
contourinfo.area = contourinfolists.area[i]
contourinfo.ecc = contourinfolists.ecc[i]
contourinfo.imgRoi = contourinfolists.imgRoi[i]
self.contourinfo_list.append(contourinfo)
# Figure out who is who in the camera image.
try:
self.mapContourinfoFromObject = self.MapContoursFromObjects()
except IndexError:
self.mapContourinfoFromObject = None
if self.mapContourinfoFromObject is not None:
# Update the robot state w/ the contourinfo and end-effector positions.
for iRobot in self.iRobot_list:
if self.mapContourinfoFromObject[iRobot] is not None:
# For the robot, use the end-effector angle instead of the contourinfo angle.
if self.stateEndEffector is not None:
q = self.stateEndEffector.pose.orientation
rpy = tf.transformations.euler_from_quaternion((q.x, q.y, q.z, q.w))
self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].angle = rpy[2]
contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iRobot]]
else:
contourinfo = contourinfoNone
# Use the computed pose if one exists.
if self.stateEndEffector is not None:
poses = PoseStamped(header=self.stateEndEffector.header, pose=self.stateEndEffector.pose)
else:
poses = None
# Update the object.
if (iRobot < len(self.objects)):
self.objects[iRobot].Update(contourinfo, poses)
# Write a file (for getting Kalman covariances, etc).
#data = '%s, %s, %s, %s, %s, %s\n' % (self.stateEndEffector.pose.position.x,
# self.stateEndEffector.pose.position.y,
# self.objects[iRobot].state.pose.position.x,
# self.objects[iRobot].state.pose.position.y,
# self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].x,
# self.contourinfo_list[self.mapContourinfoFromObject[iRobot]].y)
#self.fidRobot.write(data)
# Update the flies' states.
for iFly in self.iFly_list:
if self.mapContourinfoFromObject[iFly] is not None:
contourinfo = self.contourinfo_list[self.mapContourinfoFromObject[iFly]]
else:
contourinfo = contourinfoNone
#rospy.logwarn ('No contourinfo for fly %d' % iFly)
if (iFly < len(self.objects)):
self.objects[iFly].Update(contourinfo, None)
# Write a file.
#if self.mapContourinfoFromObject[1] is not None:
# data = '%s, %s, %s, %s\n' % (self.contourinfo_list[self.mapContourinfoFromObject[1]].x,
# self.contourinfo_list[self.mapContourinfoFromObject[1]].y,
# self.objects[1].state.pose.position.x,
# self.objects[1].state.pose.position.y)
# self.fidFly.write(data)
self.PublishArenaStateFromObjects()
# Publish the VisualState.
if (0 in self.iRobot_list) and (0 < len(self.objects)):
self.pubVisualState.publish(self.objects[0].state)
# Publish a marker to indicate the size of the arena.
self.markerArenaOuter.header.stamp = contourinfolists.header.stamp
self.markerArenaInner.header.stamp = contourinfolists.header.stamp
self.pubMarker.publish(self.markerArenaOuter)
self.pubMarker.publish(self.markerArenaInner)
# Publish markers for all the exclusionzones.
if self.enabledExclusionzone:
for marker in self.markerExclusionzone_list:
marker.header.stamp = contourinfolists.header.stamp
self.pubMarker.publish(marker)
