def get_pose(self, topic):
image = self.images[topic]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
camera_matrix = np.matrix(
np.reshape(self.camera_infos[topic].K, (3, 3)))
dist_coeffs = np.matrix(self.camera_infos[topic].D)
return self.detector.calculate_object_pose(img_raw, camera_matrix,
dist_coeffs, False)
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now() - self.latest_image.header.stamp > rospy.Duration(1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat, tvec, image_points) = self.detector.calculate_object_pose_ransac(image_raw, self.cm, self.dc, True)
#print tvec[1]
return tvec[0][0], tvec[1][0], tvec[2][0]
def _checkHandle(self, req):
'''
Service handle for "Capture" Service
Grabs images, converts them and saves them to files
@param req: service request
@type req: CaptureRequest() message
@return: CaptureResponse() message
'''
self.image_received = [False] * self.numCams
visible = []
while not all(self.image_received):
print "waiting for images"
rospy.sleep(1)
if all(self.image_received):
print "=== ALL IMAGES RECEIVED ==="
self.images_received = [False] * self.numCams
for id in range(self.numCams):
image = self.image[id]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
try:
self.detector.detect_image_points( img_raw, False, True)
visible.append(True)
except self.detector.NoPatternFoundException:
rospy.logwarn("No cb found")
visible.append(False)
# grab image messages
#print '%s checkerboards found --> return %s'%(sum(visible),all(visible))
response = VisibleResponse()
response.every = False
response.master = False
if all(visible):
response.every = True
response.master = True
#elif visible[0]:
# response.master = True
return response
def get_position(self):
'''
returns the Y value in subpixel accuracy for the center of the recognized checkerboard
'''
r = rospy.Rate(10) # Hz
while rospy.Time.now(
) - self.latest_image.header.stamp > rospy.Duration(
1) and not rospy.is_shutdown():
print 'no image received'
print rospy.Time.now(), self.latest_image.header.stamp
r.sleep()
cvImage = self.bridge.imgmsg_to_cv(self.latest_image, "mono8")
image_raw = cv2util.cvmat2np(cvImage)
#image_processed = cv2.undistort(image_raw, self.cm, self.dc)
#points=self.detector.detect_image_points(image_processed,True)
(rmat,
tvec) = self.detector.calculate_object_pose(image_raw, self.cm,
self.dc, True)
#print tvec[1]
return (rmat, tvec), self.latest_image.header.frame_id
def _checkHandle(self, req):
'''
Service handle for "Capture" Service
Grabs images, converts them and saves them to files
@param req: service request
@type req: CaptureRequest() message
@return: CaptureResponse() message
'''
self.image_received = [False] * self.numCams
visible = []
while not all(self.image_received):
print "waiting for images"
rospy.sleep(1)
if all(self.image_received):
print "=== ALL IMAGES RECEIVED ==="
self.images_received = [False] * self.numCams
for id in range(self.numCams):
image = self.image[id]
cvImage = self.bridge.imgmsg_to_cv(image, 'rgb8')
img_raw = cv2util.cvmat2np(cvImage)
visible.append(self.detector.detect_image_points(
img_raw, False, True) is not None)
# grab image messages
#print '%s checkerboards found --> return %s'%(sum(visible),all(visible))
response = VisibleResponse()
response.every = False
response.master = False
if all(visible):
response.every = True
response.master = True
elif visible[0]:
response.master = True
return response
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received or not self._right_received or not self._kinect_rgb_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received: print "--> still waiting for sample from left"
if not self._right_received: print "--> still waiting for sample from right"
if not self._kinect_rgb_received: print "--> still waiting for sample from kinect"
start_time = rospy.Time.now()
latest_left = self._left
latest_right = self._right
latest_kinect_rgb = self._kinect_rgb
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
start_time = rospy.Time.now()
while (not self._torso_joint_msg_received or not self._arm_joint_msg_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._torso_joint_msg_received: print "--> still waiting for torso joint states"
if not self._arm_joint_msg_received: print "--> still waiting for srm joint states"
start_time = rospy.Time.now()
latest_torso = self._torso_joint_msg
latest_arm = self._arm_joint_msg
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp
cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# detect cb right
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: right"
img_points_right = []
for (x, y) in corners.reshape(-1, 2):
img_points_right.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg right
# -----------------------
cam_msg_right = CameraMeasurement()
cam_msg_right.camera_id = "right"
cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp
cam_msg_right.cam_info = latest_right["camera_info"]
cam_msg_right.image_points = img_points_right
cam_msg_right.verbose = False
#cam_msg_right.image = latest_right["image_color"]
#cam_msg_right.image_rect = latest_right["image_rect"]
#cam_msg_right.features = # Not implemented here
# detect cb kinect_rgb
# --------------------
cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image, is_grayscale=True)
if corners != None:
print "cb found: kinect_rgb"
img_points_kinect_rgb = []
for (x, y) in corners.reshape(-1, 2):
img_points_kinect_rgb.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg kinect_rgb
# ----------------------------
cam_msg_kinect_rgb = CameraMeasurement()
cam_msg_kinect_rgb.camera_id = "kinect_rgb"
cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb["camera_info"].header.stamp
cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"]
cam_msg_kinect_rgb.image_points = img_points_kinect_rgb
cam_msg_kinect_rgb.verbose = False
#cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"]
#cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"]
#cam_msg_kinect_rgb.features = # Not implemented here
# create torso_chain msg
# ----------------------
torso_chain_msg = ChainMeasurement()
torso_chain_msg.header = latest_torso.header
torso_chain_msg.chain_id = "torso_chain"
torso_chain_msg.chain_state = latest_torso
# create arm_chain msg
# --------------------
arm_chain_msg = ChainMeasurement()
arm_chain_msg.header = latest_arm.header
arm_chain_msg.chain_id = "arm_chain"
arm_chain_msg.chain_state = latest_arm
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image_color"])
self._image_pub_right.publish(latest_right["image_color"])
self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb]
robot_msg.M_chain = [torso_chain_msg, arm_chain_msg]
self._robot_measurement_pub.publish(robot_msg)
return True
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size, square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received:
print "--> still waiting for sample from left"
start_time = rospy.Time.now()
print "got sample"
latest_left = self._left
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(
image, is_grayscale=True)
if corners is not None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["image"].header.stamp
#cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# create chain msgs
# ----------------------
transformations = []
print self.transformations
for (key, links) in self.transformations.iteritems():
chain_msg = ChainMeasurement()
chain_msg.chain_id = key
while not rospy.is_shutdown():
try:
(
trans, rot) = self.listener.lookupTransform(links[0], links[1],
rospy.Time(0))
break
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "error looking up transformation from ", links[0], " to ", links[1]
rospy.sleep(0.5)
chain_msg.translation = trans
chain_msg.rotation = rot
transformations.append(chain_msg)
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left]
robot_msg.M_chain = transformations
self._robot_measurement_pub.publish(robot_msg)
return True
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size,
square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
# --------------------
# receive images
# -----------------
for v in self._images_received:
self._images_received[v] = False
for v in self.transformations_received:
self.transformations_received[v] = False
print self._images_received
start_time = rospy.Time.now()
while (not all(self._images_received.values())
or not all(self.transformations.values())):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still
# missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
for name, v in self._images_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
for name, v in self._transformations_received.iteritems():
if not v:
print "--> still waiting for sample from %s" % name
start_time = rospy.Time.now()
print "got sample"
#latest_left = self._left
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb
# --------------
for name, image in self._images.iteritems():
image = image["image"]
#print image.header
cvImage = self.bridge.imgmsg_to_cv(image, "mono8")
imagecv = cv2util.cvmat2np(cvImage)
try:
corners = checkerboard_detector.detect_image_points(
imagecv, is_grayscale=True)
except:
# cb not found
rospy.logwarn("No calibration pattern found for: '%s'" % name)
return False
else:
print "cb found: %s" % name
img_points = []
for (x, y) in corners.reshape(-1, 2):
img_points.append(ImagePoint(x, y))
# create camera msg left
# ----------------------
cam_msg = CameraMeasurement()
cam_msg.camera_id = name
#print "crash before"
cam_msg.header.stamp = image.header.stamp
#print "crash after"
#cam_msg.cam_info = image["camera_info"]
cam_msg.image_points = img_points
cam_msg.verbose = False
robot_msg.M_cam.append(cam_msg)
cam_msg.image = image
# print cam_msg.camera_id
#print cam_msg.header
# cam_msg.image_rect = latest_left["image_rect"]
# cam_msg.features = # Not implemented here
#----------------------
#DEBUG publish pic
#-----------------
#self._image_pub_left.publish(latest_left["image"])
#----------------------
# Fill robot_msg
#----------------------
robot_msg.M_chain = self.transformations.values()
self._robot_measurement_pub.publish(robot_msg)
return True
def _capture_and_pub(self, sample_id, target_id, chain_id, pattern_size,
square_size):
'''
Main capturing function. Gets a set of recent messages for all needed topics.
Processes messages and creates RobotMeasuerment message which is published.
@param sample_id: Sample identifier (e.g. sample01)
@type sample_id: string
@param target_id: Name of checkerboard (e.g. cb_9x6)
@type target_id: string
@param chain_id: Name of dh chain to which checkerboard is attached (e.g. arm_chain)
@type chain_id: string
@param pattern_size: Size of checkerboard pattern as defined by opencv (e.g. (9, 6))
@type pattern_size: tuple(x, y)
@param square_size: Size of checkerboard sqaures (im m)
@type square_size: float
'''
# capture measurements
# --------------------
self._left_received = False
self._right_received = False
self._kinect_rgb_received = False
start_time = rospy.Time.now()
while (not self._left_received or not self._right_received
or not self._kinect_rgb_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._left_received:
print "--> still waiting for sample from left"
if not self._right_received:
print "--> still waiting for sample from right"
if not self._kinect_rgb_received:
print "--> still waiting for sample from kinect"
start_time = rospy.Time.now()
latest_left = self._left
latest_right = self._right
latest_kinect_rgb = self._kinect_rgb
self._torso_joint_msg_received = False
self._arm_joint_msg_received = False
start_time = rospy.Time.now()
while (not self._torso_joint_msg_received
or not self._arm_joint_msg_received):
rospy.sleep(0.005)
# print warning every 2 seconds if one of the messages is still missing...
if start_time + rospy.Duration(2.0) < rospy.Time.now():
if not self._torso_joint_msg_received:
print "--> still waiting for torso joint states"
if not self._arm_joint_msg_received:
print "--> still waiting for srm joint states"
start_time = rospy.Time.now()
latest_torso = self._torso_joint_msg
latest_arm = self._arm_joint_msg
# set up checkerboard and checkerboard detector
# ---------------------------------------------
checkerboard = Checkerboard(pattern_size, square_size)
checkerboard_detector = CheckerboardDetector(checkerboard)
# detect cb left
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_left["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: left"
img_points_left = []
for (x, y) in corners.reshape(-1, 2):
img_points_left.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg left
# ----------------------
cam_msg_left = CameraMeasurement()
cam_msg_left.camera_id = "left"
cam_msg_left.header.stamp = latest_left["camera_info"].header.stamp
cam_msg_left.cam_info = latest_left["camera_info"]
cam_msg_left.image_points = img_points_left
cam_msg_left.verbose = False
#cam_ms_leftg.image = latest_left["image_color"]
#cam_msg_left.image_rect = latest_left["image_rect"]
#cam_msg_left.features = # Not implemented here
# detect cb right
# --------------
cvImage = self.bridge.imgmsg_to_cv(latest_right["image_rect"], "mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: right"
img_points_right = []
for (x, y) in corners.reshape(-1, 2):
img_points_right.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg right
# -----------------------
cam_msg_right = CameraMeasurement()
cam_msg_right.camera_id = "right"
cam_msg_right.header.stamp = latest_right["camera_info"].header.stamp
cam_msg_right.cam_info = latest_right["camera_info"]
cam_msg_right.image_points = img_points_right
cam_msg_right.verbose = False
#cam_msg_right.image = latest_right["image_color"]
#cam_msg_right.image_rect = latest_right["image_rect"]
#cam_msg_right.features = # Not implemented here
# detect cb kinect_rgb
# --------------------
cvImage = self.bridge.imgmsg_to_cv(latest_kinect_rgb["image_color"],
"mono8")
image = cv2util.cvmat2np(cvImage)
corners = checkerboard_detector.detect_image_points(image,
is_grayscale=True)
if corners != None:
print "cb found: kinect_rgb"
img_points_kinect_rgb = []
for (x, y) in corners.reshape(-1, 2):
img_points_kinect_rgb.append(ImagePoint(x, y))
else:
# cb not found
return False
# create camera msg kinect_rgb
# ----------------------------
cam_msg_kinect_rgb = CameraMeasurement()
cam_msg_kinect_rgb.camera_id = "kinect_rgb"
cam_msg_kinect_rgb.header.stamp = latest_kinect_rgb[
"camera_info"].header.stamp
cam_msg_kinect_rgb.cam_info = latest_kinect_rgb["camera_info"]
cam_msg_kinect_rgb.image_points = img_points_kinect_rgb
cam_msg_kinect_rgb.verbose = False
#cam_ms_kinect_rgbg.image = latest_kinect_rgb["image_color"]
#cam_msg_kinect_rgb.image_rect = latest_kinect_rgb["image_rect"]
#cam_msg_kinect_rgb.features = # Not implemented here
# create torso_chain msg
# ----------------------
torso_chain_msg = ChainMeasurement()
torso_chain_msg.header = latest_torso.header
torso_chain_msg.chain_id = "torso_chain"
torso_chain_msg.chain_state = latest_torso
# create arm_chain msg
# --------------------
arm_chain_msg = ChainMeasurement()
arm_chain_msg.header = latest_arm.header
arm_chain_msg.chain_id = "arm_chain"
arm_chain_msg.chain_state = latest_arm
# DEBUG publish pic
# -----------------
self._image_pub_left.publish(latest_left["image_color"])
self._image_pub_right.publish(latest_right["image_color"])
self._image_pub_kinect_rgb.publish(latest_kinect_rgb["image_color"])
# create robot measurement msg and publish
# -----------------
robot_msg = RobotMeasurement()
robot_msg.sample_id = sample_id
robot_msg.target_id = target_id
robot_msg.chain_id = chain_id
robot_msg.M_cam = [cam_msg_left, cam_msg_right, cam_msg_kinect_rgb]
robot_msg.M_chain = [torso_chain_msg, arm_chain_msg]
self._robot_measurement_pub.publish(robot_msg)
return True