def find_checkerboard_service(self, req):
poses = []
min_x = self.ros_image.width
min_y = self.ros_image.height
max_x = 0
max_y = 0
for i in range(10):
rospy.sleep(0.5)
#copy the image over
with self.mutex:
image = self.ros_image
result = self.find_checkerboard_pose(image, req.corners_x,
req.corners_y, req.spacing_x,
req.spacing_y,
self.width_scaling,
self.height_scaling)
if result is None:
rospy.logerr("Couldn't find a checkerboard")
continue
p, corners = result
poses.append(p)
for j in range(corners.cols):
x = cv.GetReal2D(corners, 0, j)
y = cv.GetReal2D(corners, 1, j)
min_x = min(min_x, x)
max_x = max(max_x, x)
min_y = min(min_y, y)
max_y = max(max_y, y)
# convert all poses to twists
twists = []
for p in poses:
twists.append(
PyKDL.diff(PyKDL.Frame.Identity(), posemath.fromMsg(p.pose)))
# get average twist
twist_res = PyKDL.Twist()
PyKDL.SetToZero(twist_res)
for t in twists:
for i in range(3):
twist_res.vel[i] += t.vel[i] / len(poses)
twist_res.rot[i] += t.rot[i] / len(poses)
# get noise
noise_rot = 0
noise_vel = 0
for t in twists:
n_vel = (t.vel - twist_res.vel).Norm()
n_rot = (t.rot - twist_res.rot).Norm()
if n_vel > noise_vel:
noise_vel = n_vel
if n_rot > noise_rot:
noise_rot = n_rot
# set service resul
pose_res = PyKDL.addDelta(PyKDL.Frame.Identity(), twist_res, 1.0)
pose_msg = PoseStamped()
pose_msg.header = poses[0].header
pose_msg.pose = posemath.toMsg(pose_res)
return GetCheckerboardPoseResponse(pose_msg, min_x, max_x, min_y,
max_y, noise_vel, noise_rot)
