def ContourInfo_callback(self, contourinfo):
#rospy.logwarn ('contourinfo callback, stamp=%s, initialized=%s' % (contourinfo.header.stamp, self.initialized))
if self.initialized:
try:
#rospy.logwarn ('CI contourinfo0 %s' % contourinfo)
contourinfo = self.TransformContourinfoPlateFromCamera(contourinfo)
#rospy.logwarn ('CI contourinfo1 %s' % contourinfo)
contourinfo = self.FilterContourinfoWithinRadius(contourinfo, self.radiusMask)
#rospy.logwarn ('CI contourinfo2 %s' % contourinfo)
# Repackage the contourinfo into a list of contours
self.contours = []
for i in range(len(contourinfo.x)):
contour = Contour()
contour.header = contourinfo.header
contour.x = contourinfo.x[i]
contour.y = contourinfo.y[i]
contour.angle = contourinfo.angle[i]
contour.area = contourinfo.area[i]
contour.ecc = contourinfo.ecc[i]
self.contours.append(contour)
#rospy.logwarn('contour.angle=%0.2f' % (contour.angle))
# Send the contour transforms.
try:
self.tfbx.sendTransform((contour.x,
contour.y,
0.0),
tf.transformations.quaternion_about_axis(contour.angle, (0,0,1)),
contour.header.stamp,
"contour"+str(i),
"ImageRect")
except tf.Exception, e:
rospy.logwarn ('Exception in sendTransform(%s->%s): %s' % ("contour"+str(i),"ImageRect",e))
# Figure out who is who in the camera image.
try:
self.mapContourFromObject = self.MapContoursFromObjects()
except IndexError:
self.mapContourFromObject = None
#rospy.logwarn ('CI map=%s' % self.mapContourFromObject)
#for i in range(len(self.contours)):
# rospy.logwarn ('CI contour[%d].x,y=%s' % (i,[self.contours[i].x,self.contours[i].y]))
# Update the robot state w/ the contour and end-effector positions.
if self.mapContourFromObject is not None:
if (self.nRobots==1) and (self.stateEndEffector is not None):
if self.mapContourFromObject[0] is not None:
# For the robot, use the end-effector angle instead of the contour 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.contours[self.mapContourFromObject[0]].angle = rpy[2]
self.objects[0].Update(self.contours[self.mapContourFromObject[0]], self.stateEndEffector.pose.position)
else:
self.objects[0].Update(None, self.stateEndEffector.pose.position)
# 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[0].state.pose.position.x,
# self.objects[0].state.pose.position.y,
# self.contours[self.mapContourFromObject[0]].x,
# self.contours[self.mapContourFromObject[0]].y)
#self.fidRobot.write(data)
#rospy.loginfo ('CI update robot contour=%s' % contour)
# Update the flies' states.
for iFly in self.iFly_list:
if self.mapContourFromObject[iFly] is not None:
#rospy.logwarn ('self.contours[self.mapContourFromObject[%d]]=%s' % (iFly, self.contours[self.mapContourFromObject[iFly]]))
try:
self.objects[iFly].Update(self.contours[self.mapContourFromObject[iFly]], None)
except IndexError, e:
#rospy.logwarn('iFly=%s' % iFly)
#rospy.logwarn('self.mapContourFromObject=%s' % self.mapContourFromObject)
#rospy.logwarn('self.contours=%s' % self.contours)
#rospy.logwarn('len(self.objects)=%s' % len(self.objects))
rospy.logwarn('Exception on objects[%d].Update(): %s' % (iFly, e))
else:
self.objects[iFly].Update(None, None)
#self.stateEndEffector.header.stamp,#rospy.Time.now()
#rospy.loginfo ('CI update state %s contour=%s' % (iFly,contour))
# Write a file.
#if self.mapContourFromObject[1] is not None:
# data = '%s, %s, %s, %s\n' % (self.contours[self.mapContourFromObject[1]].x,
# self.contours[self.mapContourFromObject[1]].y,
# self.objects[1].state.pose.position.x,
# self.objects[1].state.pose.position.y)
# self.fidFly.write(data)
# Construct the ArenaState message.
arenastate = ArenaState()
#if self.objects[0].state.pose.position.x is not None:
if (self.nRobots==1) and (len(self.objects)>0):
arenastate.robot.header.stamp = self.objects[0].state.header.stamp
arenastate.robot.header.frame_id = self.objects[0].state.header.frame_id
arenastate.robot.pose = self.objects[0].state.pose
arenastate.robot.velocity = self.objects[0].state.velocity
#rospy.logwarn ('CI robot.position=%s, ptOffset=%s' % ([self.objects[0].state.pose.position.x,
# self.objects[0].state.pose.position.y],
# [self.objects[0].ptOffset.x,
# self.objects[0].ptOffset.y]))
for iFly in self.iFly_list:
#rospy.logwarn ('iFly=%d, self.mapContourFromObject=%s, len(self.objects)=%d' % (iFly, self.mapContourFromObject, len(self.objects)))
if (self.mapContourFromObject[iFly] is not None) and (self.objects[iFly].state.pose.position.x is not None):
arenastate.flies.append(MsgFrameState(header = self.objects[iFly].state.header,
pose = self.objects[iFly].state.pose,
velocity = self.objects[iFly].state.velocity))
#rospy.logwarn('arenastate.flies.append(%s)' % self.objects[iFly].name)
# Publish the ArenaState.
self.pubArenaState.publish(arenastate)
# Publish the EndEffectorOffset.
if len(self.objects)>0:
self.pubEndEffectorOffset.publish(self.objects[0].ptOffset)
# Publish a disc to indicate the arena extent.
self.markerArena.header.stamp = contourinfo.header.stamp
self.pubMarker.publish(self.markerArena)
def MapContoursFromObjects(self):
# Make a list of object positions (i.e. robots & flies).
xyObjects = []
# Robots.
if (self.nRobots==1):
if (self.stateEndEffector is not None):
t = self.stateEndEffector.header.stamp
xyRobotComputed = [self.stateEndEffector.pose.position.x + self.objects[0].ptOffset.x,
self.stateEndEffector.pose.position.y + self.objects[0].ptOffset.y]
#rospy.logwarn ('CI Robot image at %s' % ([self.objects[0].state.pose.position.x,
# self.objects[0].state.pose.position.y]))
elif (len(self.objects)>0) and (self.objects[0].isVisible):
t = self.objects[0].state.header.stamp
xyRobotComputed = [self.objects[0].state.pose.position.x,
self.objects[0].state.pose.position.y]
else:
t = rospy.Time.now()
xyRobotComputed = [0.0, 0.0]
xyObjects.append(xyRobotComputed)
self.tfbx.sendTransform((xyRobotComputed[0], xyRobotComputed[1], 0.0),
tf.transformations.quaternion_about_axis(0, (0,0,1)),
t,
"RobotComputed",
"Plate")
else:
t = rospy.Time.now()
# Flies.
for iFly in self.iFly_list:
#if self.objects[iFly].isVisible:
xyObjects.append([self.objects[iFly].state.pose.position.x,
self.objects[iFly].state.pose.position.y])
#rospy.loginfo ('CI Object %s at x,y=%s' % (iFly,[self.objects[iFly].state.pose.position.x,
# self.objects[iFly].state.pose.position.y]))
# Make a list of contour positions.
xyContours = []
for iContour in range(len(self.contours)):
if self.contours[iContour] is not None:
xyContours.append([self.contours[iContour].x,
self.contours[iContour].y])
#rospy.loginfo ('CI contour %s at x,y=%s' % (iContour,[self.contours[iContour].x,
# self.contours[iContour].y]))
#rospy.loginfo ('CI flies[0,1].isVisible=%s' % [self.objects[0].isVisible, self.objects[1].isVisible])
# Construct two lists of contour stats, to describe the range of good robot/fly properties.
contoursMin = []
contoursMax = []
contoursMean = []
# Append the robot contour stats.
contour = Contour()
if (self.nRobots==1):
contour.area = self.objects[0].areaMin #self.areaMinRobot
contour.ecc = self.objects[0].eccMin #self.eccMinRobot
contoursMin.append(contour)
contour.area = self.objects[0].areaMax #self.areaMaxRobot
contour.ecc = self.objects[0].eccMax #self.eccMaxRobot
contoursMax.append(contour)
contour.area = self.objects[0].areaSum / self.objects[0].areaCount
contour.ecc = self.objects[0].eccSum / self.objects[0].eccCount
contoursMean.append(contour)
# Append the fly contour stats.
for iFly in self.iFly_list:
contour.area = self.objects[iFly].areaMin #self.areaMinFly
contour.ecc = self.objects[iFly].eccMin #self.eccMinFly
contoursMin.append(contour)
contour.area = self.objects[iFly].areaMax #self.areaMaxFly
contour.ecc = self.objects[iFly].eccMax #self.eccMaxFly
contoursMax.append(contour)
contour.area = self.objects[iFly].areaSum / self.objects[iFly].areaCount
contour.ecc = self.objects[iFly].eccSum / self.objects[iFly].eccCount
contoursMean.append(contour)
# Print ecc/area stats.
#rospy.logwarn ('robot,fly ecc=[%0.2f, %0.2f], area=[%0.2f, %0.2f]' % (self.objects[0].eccSum/self.objects[0].eccCount,
# self.objects[1].eccSum/self.objects[1].eccCount,
# self.objects[0].areaSum/self.objects[0].areaCount,
# self.objects[1].areaSum/self.objects[1].areaCount))
# Create a list of computed object positions, if any.
ptComputed = [None for k in self.iAll_list] #[None for k in range(self.nRobots+len(self.objects))]
if (self.nRobots==1):
ptComputed[0] = Point(x=xyRobotComputed[0], y=xyRobotComputed[1])
# Augment the contours list, if necessary, so there are as many contours as objects.
contoursAug = copy.copy(self.contours)
while len(contoursAug)<len(xyObjects):
contoursAug.append(ContourInfo(x=55555, y=55555, ecc=1.0, area=1.0)) # norm(x,y) must be less than 999999!
#rospy.logwarn ('len(contoursAug),len(xyObjects)=%s' % [len(contoursAug),len(xyObjects)])
# Match objects with contours.
#rospy.logwarn ('GetDistanceMatrixFromContours()')
for m in range(len(xyObjects)):
self.tfbx.sendTransform((xyObjects[m][0],xyObjects[m][1],0.0),
(0,0,0,1),
t,
"xyObjects"+str(m),
"Plate")
for n in range(len(contoursAug)):
self.tfbx.sendTransform((contoursAug[n].x,contoursAug[n].y,0.0),
(0,0,0,1),
t,
"contours"+str(n),
"Plate")
d = self.GetDistanceMatrixFromContours(xyObjects, contoursAug, contoursMin, contoursMax, contoursMean, ptComputed)
if d is not []:
# Choose the algorithm.
#alg = 'galeshapely'
alg = 'munkres'
if alg=='galeshapely':
(mapObjectsGaleShapely, mapContours) = self.GetMatchGaleShapely(d)
mapContoursFromObjects = mapObjectsGaleShapely
if alg=='munkres':
mapObjectsMunkres = self.GetMatchMunkres(d)
mapContoursFromObjects = mapObjectsMunkres
#rospy.logwarn('mapContoursFromObjects=%s'%mapContoursFromObjects)
#rospy.logwarn('len(mapContoursFromObjects)=%d'%len(mapContoursFromObjects))
#rospy.logwarn('len(xyContours)=%d'%len(xyContours))
# Set the augmented entries to None.
for m in range(len(mapContoursFromObjects)):
#rospy.logwarn('mapContoursFromObjects[m], len(xyContours)=%s'%[mapContoursFromObjects[m],len(xyContours)])
if (mapContoursFromObjects[m])>=len(xyContours):
mapContoursFromObjects[m] = None
#rospy.logwarn ('CI mapObjectsGaleShapely =%s' % (mapObjectsGaleShapely))
#rospy.logwarn ('CI mapObjectsMunkres =%s' % mapObjectsMunkres)
#rospy.logwarn ('CI mapContoursFromObjects=%s' % mapContoursFromObjects)
# Prepend a None for a missing robot.
#if self.stateEndEffector is None:
# mapContoursFromObjects.insert(0,None)
# Append a None for missing flies.
while len(mapContoursFromObjects) < self.nRobots+self.nFlies:
mapContoursFromObjects.append(None)
else:
mapContoursFromObjects = [None for k in self.iAll_list]
# rospy.logwarn ('----------------------------------')
# if (self.nRobots==1):
# rospy.logwarn ('CI xyRobotComputed=%s' % xyRobotComputed)
# rospy.logwarn ('CI xyObjects=%s' % xyObjects)
# rospy.logwarn ('CI xyContours=%s' % xyContours)
# if self.stateEndEffector is not None:
# rospy.logwarn ('CI ptComputed=(%0.2f+%0.2f, %0.2f+%0.2f)' % (self.stateEndEffector.pose.position.x,
# self.objects[0].ptOffset.x,
# self.stateEndEffector.pose.position.y,
# self.objects[0].ptOffset.y))
# rospy.logwarn ('CI mapObjects=%s' % (mapContoursFromObjects))
# rospy.logwarn('d=\n%s'%d)
return mapContoursFromObjects
