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)
