class ATPoseNode(DTROS):
"""
Computes an estimate of the Duckiebot pose using the wheel encoders.
Args:
node_name (:obj:`str`): a unique, descriptive name for the ROS node
Configuration:
Publisher:
~/rectified_image (:obj:`Image`): The rectified image
~at_localization (:obj:`PoseStamped`): The computed position broadcasted in TFs
Subscribers:
~/camera_node/image/compressed (:obj:`CompressedImage`): Observation from robot
"""
def __init__(self, node_name):
# Initialize the DTROS parent class
super(ATPoseNode, self).__init__(node_name=node_name,
node_type=NodeType.LOCALIZATION)
# get the name of the robot
self.veh = rospy.get_namespace().strip("/")
self.bridge = CvBridge()
self.camera_info = None
self.Rectify = None
self.at_detector = Detector(families='tag36h11',
nthreads=4,
quad_decimate=4.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
# Construct publishers
self.at_estimated_pose = rospy.Publisher(
f'/{self.veh}/at_localization',
Odometry,
queue_size=1,
dt_topic_type=TopicType.LOCALIZATION)
# Camera subscribers:
camera_topic = f'/{self.veh}/camera_node/image/compressed'
self.camera_feed_sub = rospy.Subscriber(camera_topic,
CompressedImage,
self.detectATPose,
queue_size=1,
buff_size=2**24)
camera_info_topic = f'/{self.veh}/camera_node/camera_info'
self.camera_info_sub = rospy.Subscriber(camera_info_topic,
CameraInfo,
self.getCameraInfo,
queue_size=1)
self.image_pub = rospy.Publisher(f'/{self.veh}/rectified_image',
Image,
queue_size=10)
self.tag_pub = rospy.Publisher(f'/{self.veh}/detected_tags',
Int32MultiArray,
queue_size=5)
self.log("Initialized!")
def getCameraInfo(self, cam_msg):
if (self.camera_info == None):
self.camera_info = cam_msg
self.Rectify = Rectify(self.camera_info)
return
def _broadcast_tf(self,
parent_frame_id: str,
child_frame_id: str,
stamp: Optional[float] = None,
translations: Optional[Tuple[float, float,
float]] = None,
euler_angles: Optional[Tuple[float, float,
float]] = None,
quarternion: Optional[List[float]] = None):
br = tf2_ros.StaticTransformBroadcaster()
ts = TransformStamped()
ts.header.frame_id = parent_frame_id
ts.child_frame_id = child_frame_id
if stamp is None:
stamp = rospy.Time.now()
ts.header.stamp = stamp
if translations is None:
translations = 0, 0, 0
ts.transform.translation.x = translations[0]
ts.transform.translation.y = translations[1]
ts.transform.translation.z = translations[2]
if euler_angles is not None:
q = tf_conversions.transformations.quaternion_from_euler(
*euler_angles)
elif quarternion is not None:
q = quarternion
else:
q = 0, 0, 0, 1
ts.transform.rotation.x = q[0]
ts.transform.rotation.y = q[1]
ts.transform.rotation.z = q[2]
ts.transform.rotation.w = q[3]
br.sendTransform(ts)
def _broadcast_detected_tag(self, image_msg, tag_id, tag):
# inverse the rotation and tranformation comming out of the AT detector.
# The AP detector's output is the camera frame relative to the TAG
# According to: https://duckietown.slack.com/archives/C6ZHPV212/p1619876209086300?thread_ts=1619751267.084600&cid=C6ZHPV212
# While the transformation below needs TAG relative to camera
# Therefore we need to reverse it first.
pose_R = np.identity(4)
pose_R[:3, :3] = tag.pose_R
inv_pose_R = np.transpose(pose_R)
pose_t = np.ones((4, 1))
pose_t[:3, :] = tag.pose_t
inv_pose_t = -np.matmul(inv_pose_R, pose_t)
out_q = quaternion_from_matrix(inv_pose_R)
out_t = inv_pose_t[:3, :]
tag_frame_id = 'april_tag_{}'.format(tag_id)
tag_cam_frame_id = 'april_tag_cam_{}'.format(tag_id)
camera_rgb_link_frame_id = 'at_{}_camera_rgb_link'.format(tag_id)
camera_link_frame_id = 'at_{}_camera_link'.format(tag_id)
base_link_frame_id = 'at_{}_base_link'.format(tag_id)
# ATag to ATag cam (this is because AprilTAG pose is different from physical)
self._broadcast_tf(parent_frame_id=tag_frame_id,
child_frame_id=tag_cam_frame_id,
euler_angles=(-90 * math.pi / 180, 0,
-90 * math.pi / 180))
# ATag cam to camera_rgb_link (again this is internal cam frame)
self._broadcast_tf(parent_frame_id=tag_cam_frame_id,
child_frame_id=camera_rgb_link_frame_id,
stamp=image_msg.header.stamp,
translations=out_t,
quarternion=out_q)
# camera_rgb_link to camera_link (internal cam frame to physical cam frame)
self._broadcast_tf(parent_frame_id=camera_rgb_link_frame_id,
child_frame_id=camera_link_frame_id,
stamp=image_msg.header.stamp,
euler_angles=(0, -90 * math.pi / 180,
90 * math.pi / 180))
# camera_link to base_link of robot
self._broadcast_tf(parent_frame_id=camera_link_frame_id,
child_frame_id=base_link_frame_id,
stamp=image_msg.header.stamp,
translations=(-0.066, 0, -0.106),
euler_angles=(0, -15 * math.pi / 180, 0))
def detectATPose(self, image_msg):
"""
Image callback.
Args:
msg_encoder (:obj:`Compressed`) encoder ROS message.
"""
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(image_msg)
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
new_cam, rectified_img = self.Rectify.rectify_full(cv_image)
try:
self.image_pub.publish(
self.bridge.cv2_to_imgmsg(rectified_img, "bgr8"))
except ValueError as e:
self.logerr('Could not decode image: %s' % e)
return
gray_img = cv2.cvtColor(rectified_img, cv2.COLOR_RGB2GRAY)
camera_params = (new_cam[0, 0], new_cam[1, 1], new_cam[0,
2], new_cam[1,
2])
detected_tags = self.at_detector.detect(gray_img,
estimate_tag_pose=True,
camera_params=camera_params,
tag_size=0.065)
detected_tag_ids = list(map(lambda x: fetch_tag_id(x), detected_tags))
array_for_pub = Int32MultiArray(data=detected_tag_ids)
for tag_id, tag in zip(detected_tag_ids, detected_tags):
print('detected {}: ({}, {})'.format(
tag_id, image_msg.header.stamp.to_time(),
rospy.Time.now().to_time()))
self._broadcast_detected_tag(image_msg, tag_id, tag)
self.tag_pub.publish(array_for_pub)
def go(self):
robot_name = dtu.get_current_robot_name()
output = self.options.output
if output is None:
output = 'out-calibrate-extrinsics' # + dtu.get_md5(self.options.image)[:6]
self.info('No --output given, using %s' % output)
if self.options.input is None:
print("{}\nCalibrating using the ROS image stream...\n".format(
"*" * 20))
import rospy
from sensor_msgs.msg import CompressedImage
topic_name = os.path.join('/', robot_name,
'camera_node/image/compressed')
print('Topic to listen to is: %s' % topic_name)
print('Let\'s wait for an image. Say cheese!')
# Dummy for getting a ROS message
rospy.init_node('calibrate_extrinsics')
img_msg = None
try:
img_msg = rospy.wait_for_message(topic_name,
CompressedImage,
timeout=10)
print('Image captured!')
except rospy.ROSException as e:
print(
'\n\n\n'
'Didn\'t get any message!: %s\n '
'MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER!'
'\n\n\n' % (e, ))
bgr = dtu.bgr_from_rgb(dtu.rgb_from_ros(img_msg))
print('Picture taken: %s ' % str(bgr.shape))
else:
print('Loading input image %s' % self.options.input)
bgr = dtu.bgr_from_jpg_fn(self.options.input)
if bgr.shape[1] != 640:
interpolation = cv2.INTER_CUBIC
bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation)
print('Resized to: %s ' % str(bgr.shape))
# Disable the old calibration file
print("Disableing old homography")
disable_old_homography(robot_name)
print("Obtaining camera info")
try:
camera_info = get_camera_info_for_robot(robot_name)
except Exception as E:
print("Error on obtaining camera info!")
print(E)
print("Get default homography")
try:
homography_dummy = get_homography_default()
except Exception as E:
print("Error on getting homography")
print(E)
print("Rectify image")
try:
rect = Rectify(camera_info)
except Exception as E:
print("Error rectifying image!")
print(E)
print("Calculate GPG")
try:
gpg = GroundProjectionGeometry(camera_info.width,
camera_info.height,
homography_dummy.reshape((3, 3)))
except Exception as E:
print("Error calculating GPG!")
print(E)
print("Ordered Dict")
res = OrderedDict()
try:
bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC)
res['bgr'] = bgr
res['bgr_rectified'] = bgr_rectified
_new_matrix, res['rectified_full_ratio_auto'] = rect.rectify_full(
bgr, ratio=1.65)
(result_gpg, status) = gpg.estimate_homography(bgr_rectified)
if status is not None:
raise Exception(status)
save_homography(result_gpg.H, robot_name)
msg = '''
To check that this worked well, place the robot on the road, and run:
rosrun complete_image_pipeline single_image
Look at the produced jpgs.
'''
print(msg)
except Exception as E:
print(E)
finally:
dtu.write_bgr_images_as_jpgs(res, output)
def go(self):
robot_name = dtu.get_current_robot_name()
# noinspection PyUnresolvedReferences
output = self.options.output
# noinspection PyUnresolvedReferences
the_input = self.options.input
if output is None:
output = "out/calibrate-extrinsics" # + dtu.get_md5(self.options.image)[:6]
self.info(f"No --output given, using {output}")
if the_input is None:
self.info(
("{}\nCalibrating using the ROS image stream...\n".format("*" *
20)))
import rospy
from sensor_msgs.msg import CompressedImage
topic_name = os.path.join("/", robot_name,
"camera_node/image/compressed")
logger.info(f"Topic to listen to is: {topic_name}")
self.info("Let's wait for an image. Say cheese!")
# Dummy for getting a ROS message
rospy.init_node("calibrate_extrinsics")
try:
img_msg = rospy.wait_for_message(topic_name,
CompressedImage,
timeout=10)
self.info("Image captured")
except rospy.ROSException as e:
print((
"\n\n\n"
f"Didn't get any message: {e}\n "
"MAKE SURE YOU USE DT SHELL COMMANDS OF VERSION 4.1.9 OR HIGHER."
))
raise
img_msg = cast(CompressedImage, img_msg)
bgr = dtu.bgr_from_rgb(dru.rgb_from_ros(img_msg))
self.info(f"Picture taken: {str(bgr.shape)} ")
else:
self.info(f"Loading input image {the_input}")
bgr = dtu.bgr_from_jpg_fn(the_input)
if bgr.shape[1] != 640:
interpolation = cv2.INTER_CUBIC
bgr = dtu.d8_image_resize_fit(bgr, 640, interpolation)
self.info(f"Resized to: {str(bgr.shape)} ")
# Disable the old calibration file
self.info("Disableing old homography")
disable_old_homography(robot_name)
self.info("Obtaining camera info")
try:
camera_info = get_camera_info_for_robot(robot_name)
except Exception as e:
msg = "Error on obtaining camera info."
raise Exception(msg) from e
self.info("Get default homography")
try:
homography_dummy = get_homography_default()
except Exception as e:
msg = "Error on getting homography."
raise Exception(msg) from e
self.info("Rectify image")
try:
rect = Rectify(camera_info)
except Exception as e:
msg = "Error rectifying image."
raise Exception(msg) from e
self.info("Calculate GPG")
try:
gpg = GroundProjectionGeometry(camera_info.width,
camera_info.height,
homography_dummy.reshape((3, 3)))
except Exception as e:
msg = "Error calculating GroundProjectionGeometry."
raise Exception(msg) from e
self.info("Ordered Dict")
res = {}
try:
bgr_rectified = rect.rectify(bgr, interpolation=cv2.INTER_CUBIC)
res["bgr"] = bgr
res["bgr_rectified"] = bgr_rectified
_new_matrix, res["rectified_full_ratio_auto"] = rect.rectify_full(
bgr, ratio=1.65)
(result_gpg, status) = gpg.estimate_homography(bgr_rectified)
if status is not None:
raise Exception(status)
save_homography(result_gpg.H, robot_name)
msg = """
To check that this worked well, place the robot on the road, and run:
rosrun complete_image_pipeline single_image
Look at the produced jpgs.
"""
self.info(msg)
except Exception as E:
self.error(E)
finally:
dtu.write_bgr_images_as_jpgs(res, output)