class LocalizationNode(DTROS):
def __init__(self):
super(LocalizationNode, self).__init__(
node_name='localization_node',
node_type=NodeType.LOCALIZATION
)
# get static parameters
self.robot_configuration = rospy.get_param('~robot_configuration', 'NOT_SET')
self.map_frame = rospy.get_param('~map_frame', '/world')
# create publisher
self._group = DTCommunicationGroup("/localization/tags", TransformStamped)
self._tags_pub = self._group.Publisher()
# create subscribers
self._img_sub = rospy.Subscriber(
'~detections',
AprilTagDetectionArray,
self._cb_detections,
queue_size=1
)
def on_shutdown(self):
self._group.shutdown()
def _cb_detections(self, msg):
for detection in msg.detections:
out_msg = TransformStamped(
child_frame_id='/tag/{:s}'.format(str(detection.tag_id)),
transform=detection.transform
)
out_msg.header.stamp = msg.header.stamp
out_msg.header.frame_id = self.map_frame
self._tags_pub.publish(out_msg)
def __init__(self):
super(DistributedTFNode,
self).__init__(node_name='distributed_tf_node',
node_type=NodeType.SWARM)
# get static parameter - `~veh`
try:
self.robot_hostname = rospy.get_param('~veh')
except KeyError:
self.logerr('The parameter ~veh was not set, the node will abort.')
exit(1)
# get static parameter - `~map`
try:
self.map_name = _sanitize_hostname(rospy.get_param('~map'))
except KeyError:
self.logerr('The parameter ~map was not set, the node will abort.')
exit(2)
# load atags DB
# TODO: this is temporary, we should actually subscribe to the atags_with_info topic
# we can't do that now because the atag-post-processor node is a mess
self._tags = self._load_atag_db()
self._tag_type_to_rf_type = {
'Localization': AutolabReferenceFrame.TYPE_GROUND_TAG,
'Vehicle': AutolabReferenceFrame.TYPE_DUCKIEBOT_TAG
}
self._tag_type_to_is_static = {'Localization': True, 'Vehicle': False}
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create publishers
self._tf_pub = self._group.Publisher()
# create local subscribers
self._atag_sub = rospy.Subscriber('~detections_in',
AprilTagDetectionArray,
self._cb_atag,
queue_size=20)
class ExperimentsManager(ExperimentsManagerAbs):
def __init__(self):
super().__init__()
# placeholders
self._group = None
self._sub = None
@property
def communication_group(self):
return self._group
def start(self, topic: str, msg_type: Any):
if self._group is not None:
raise ValueError(
"You cannot launch an ExperimentsManager more than once.")
# create communication group
self._group = DTCommunicationGroup(topic,
msg_type,
loglevel=logging.DEBUG)
# create subscribers
self._sub = self._group.Subscriber(self._cb)
def stop(self):
if self._group is not None:
self._group.shutdown()
def start(self, topic: str, msg_type: Any):
if self._group is not None:
raise ValueError(
"You cannot launch an ExperimentsManager more than once.")
# create communication group
self._group = DTCommunicationGroup(topic,
msg_type,
loglevel=logging.DEBUG)
# create subscribers
self._sub = self._group.Subscriber(self._cb)
def __init__(self):
super(LocalizationNode, self).__init__(node_name='localization_node',
node_type=NodeType.LOCALIZATION)
# get static parameters
self.robot_configuration = rospy.get_param('~robot_configuration',
'NOT_SET')
self.map_frame = rospy.get_param('~map_frame', '/world')
# create subscribers
self._group = DTCommunicationGroup("/localization/tags",
TransformStamped)
self._tags_sub = self._group.Subscriber(self._cb_detections)
class LocalizationNode(DTROS):
def __init__(self):
super(LocalizationNode, self).__init__(node_name='localization_node',
node_type=NodeType.LOCALIZATION)
# get static parameters
self.robot_configuration = rospy.get_param('~robot_configuration',
'NOT_SET')
self.map_frame = rospy.get_param('~map_frame', '/world')
# create subscribers
self._group = DTCommunicationGroup("/localization/tags",
TransformStamped)
self._tags_sub = self._group.Subscriber(self._cb_detections)
def on_shutdown(self):
self._group.shutdown()
def _cb_detections(self, msg, header):
if msg.child_frame_id == '/tag/{:s}'.format(str(DUCKIEBOT_TAG_ID)):
print(msg.child_frame_id)
print(header)
def __init__(self):
super(DistributedTFNode,
self).__init__(node_name='distributed_tf_node',
node_type=NodeType.SWARM)
# get static parameters
self.robot_hostname = rospy.get_param('~veh')
self.map_name = _sanitize_hostname(rospy.get_param('~map'))
self.tag_id = rospy.get_param('~tag_id')
self.min_distance_odom = rospy.get_param('~min_distance_odom')
self.max_time_between_poses = rospy.get_param(
'~max_time_between_poses')
# define local reference frames' names
self._tag_mount = f'{self.robot_hostname}/tag_mount'
self._footprint_frame = f'{self.robot_hostname}/footprint'
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create static tfs holder and access semaphore
self._static_tfs = {}
self._static_tfs_sem = threading.Semaphore(1)
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# create publishers
self._tf_pub = self._group.Publisher()
# fetch/publish right away and then set timers
if self.tag_id != '__NOTSET__':
threading.Thread(target=self._fetch_tag_tf).start()
self._pub_timer = rospy.Timer(
rospy.Duration(self.PUBLISH_TF_STATIC_EVERY_SECS),
self._publish_static_tfs)
# setup subscribers for odometry
self._odo_sub = rospy.Subscriber("~odometry_in",
Odometry,
self._cb_odometry,
queue_size=1)
self._pose_last = None
self._reminder = DTReminder(frequency=10)
class DistributedTFNode(DTROS):
def __init__(self):
super(DistributedTFNode,
self).__init__(node_name='distributed_tf_node',
node_type=NodeType.SWARM)
# get static parameter - `~veh`
try:
self.robot_hostname = rospy.get_param('~veh')
except KeyError:
self.logerr('The parameter ~veh was not set, the node will abort.')
exit(1)
# get static parameter - `~map`
try:
self.map_name = _sanitize_hostname(rospy.get_param('~map'))
except KeyError:
self.logerr('The parameter ~map was not set, the node will abort.')
exit(2)
# load atags DB
# TODO: this is temporary, we should actually subscribe to the atags_with_info topic
# we can't do that now because the atag-post-processor node is a mess
self._tags = self._load_atag_db()
self._tag_type_to_rf_type = {
'Localization': AutolabReferenceFrame.TYPE_GROUND_TAG,
'Vehicle': AutolabReferenceFrame.TYPE_DUCKIEBOT_TAG
}
self._tag_type_to_is_static = {'Localization': True, 'Vehicle': False}
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create publishers
self._tf_pub = self._group.Publisher()
# create local subscribers
self._atag_sub = rospy.Subscriber('~detections_in',
AprilTagDetectionArray,
self._cb_atag,
queue_size=20)
def on_shutdown(self):
if hasattr(self, '_group') and self._group is not None:
self._group.shutdown()
def _cb_atag(self, msg):
for detection in msg.detections:
if detection.tag_id not in self._tags:
continue
tag_type = self._tags[detection.tag_id]["tag_type"]
if tag_type not in self._tag_type_to_rf_type:
continue
rf_type = self._tag_type_to_rf_type[tag_type]
is_static = self._tag_type_to_is_static[tag_type]
# ---
tf = AutolabTransform(origin=AutolabReferenceFrame(
time=msg.header.stamp,
type=AutolabReferenceFrame.TYPE_WATCHTOWER_CAMERA,
name=msg.header.frame_id,
robot=self.robot_hostname),
target=AutolabReferenceFrame(
time=msg.header.stamp,
type=rf_type,
name=f"tag/{detection.tag_id}",
robot=self.map_name),
is_fixed=False,
is_static=is_static,
transform=detection.transform)
# publish
self._tf_pub.publish(tf, destination=self.map_name)
@staticmethod
def _load_atag_db():
# TODO: this is temporary, we should actually subscribe to the atags_with_info topic
# we can't do that now because the atag-post-processor node is a mess
rospack = rospkg.RosPack()
distributed_tf_pkg_path = rospack.get_path('distributed_tf')
apriltags_db_filepath = os.path.join(distributed_tf_pkg_path, '..',
'..', 'assets',
'apriltagsDB.yaml')
tags = {}
with open(apriltags_db_filepath, 'r') as fin:
db = yaml.safe_load(fin)
for tag in db:
tags[tag["tag_id"]] = tag
return tags
#!/usr/bin/env python3
import rospy
import tf2_ros
from geometry_msgs.msg import TransformStamped
from dt_communication_utils import DTCommunicationGroup
from autolab_msgs.msg import \
AutolabTransform
# rospy.init_node('autolab_tf_listener')
# br = tf2_ros.TransformBroadcaster()
group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
shelf = set()
printed = set()
def cb(msg, _):
shelf.add((msg.origin.name, msg.target.name))
# t = TransformStamped()
#
# t.header.stamp = rospy.Time.now()
# t.header.frame_id = msg.origin.name
# t.child_frame_id = msg.target.name
# t.transform = msg.transform
#
# br.sendTransform(t)
to_print = shelf.difference(printed)
def __init__(self):
super(DistributedTFNode, self).__init__(
node_name='distributed_tf_node',
node_type=NodeType.SWARM
)
# get static parameter - `~veh`
try:
self.robot_hostname = rospy.get_param('~veh')
except KeyError:
self.logerr('The parameter ~veh was not set, the node will abort.')
exit(1)
# get static parameter - `~map`
try:
self.map_name = rospy.get_param('~map')
except KeyError:
self.logerr('The parameter ~map was not set, the node will abort.')
exit(2)
# make sure the map exists
maps = dw.list_maps()
if self.map_name not in maps:
self.logerr(f"Map `{self.map_name}` not found in "
f"duckietown-world=={dw.__version__}. "
f"The node will abort.")
exit(2)
self._map = dw.load_map(self.map_name)
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create TF between the /world frame and the origin of this map
self._world_to_map_origin_tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_WORLD,
name="world",
robot="*"
),
target=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_MAP_ORIGIN,
name="map",
robot=self.robot_hostname
),
is_fixed=True,
is_static=False,
transform=Transform(
translation=Vector3(x=0, y=0, z=0),
rotation=Quaternion(x=0, y=0, z=0, w=1)
)
)
# create static tfs holder and access semaphore
self._static_tfs = [
self._world_to_map_origin_tf
]
# add TFs from ground tags
self._static_tfs.extend(self._get_tags_tfs())
# create publisher
self._tf_pub = self._group.Publisher()
# publish right away and then set a timer
self._publish_tfs()
self._pub_timer = rospy.Timer(
rospy.Duration(self.PUBLISH_TF_STATIC_EVERY_SECS),
self._publish_tfs
)
class DistributedTFNode(DTROS):
PUBLISH_TF_STATIC_EVERY_SECS = 2
def __init__(self):
super(DistributedTFNode, self).__init__(
node_name='distributed_tf_node',
node_type=NodeType.SWARM
)
# get static parameter - `~veh`
try:
self.robot_hostname = rospy.get_param('~veh')
except KeyError:
self.logerr('The parameter ~veh was not set, the node will abort.')
exit(1)
# get static parameter - `~map`
try:
self.map_name = rospy.get_param('~map')
except KeyError:
self.logerr('The parameter ~map was not set, the node will abort.')
exit(2)
# make sure the map exists
maps = dw.list_maps()
if self.map_name not in maps:
self.logerr(f"Map `{self.map_name}` not found in "
f"duckietown-world=={dw.__version__}. "
f"The node will abort.")
exit(2)
self._map = dw.load_map(self.map_name)
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create TF between the /world frame and the origin of this map
self._world_to_map_origin_tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_WORLD,
name="world",
robot="*"
),
target=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_MAP_ORIGIN,
name="map",
robot=self.robot_hostname
),
is_fixed=True,
is_static=False,
transform=Transform(
translation=Vector3(x=0, y=0, z=0),
rotation=Quaternion(x=0, y=0, z=0, w=1)
)
)
# create static tfs holder and access semaphore
self._static_tfs = [
self._world_to_map_origin_tf
]
# add TFs from ground tags
self._static_tfs.extend(self._get_tags_tfs())
# create publisher
self._tf_pub = self._group.Publisher()
# publish right away and then set a timer
self._publish_tfs()
self._pub_timer = rospy.Timer(
rospy.Duration(self.PUBLISH_TF_STATIC_EVERY_SECS),
self._publish_tfs
)
def on_shutdown(self):
if hasattr(self, '_group') and self._group is not None:
self._group.shutdown()
def _publish_tfs(self, *_):
# publish
for tf in self._static_tfs:
self._tf_pub.publish(tf, destination=self.robot_hostname)
def _get_tags_tfs(self):
tfs = []
# iterate over the floor tags
for srel in self._map.spatial_relations.values():
try:
origin, (target,) = srel.a, srel.b
if origin != ():
continue
if not target.startswith('tag'):
continue
position = srel.transform.p
theta = srel.transform.theta
# get 3D rotation
Rz = tr.rotation_matrix(theta, np.array([0, 0, 1]))
Rx = tr.rotation_matrix(np.deg2rad(180), np.array([1, 0, 0]))
R = np.matmul(Rz, Rx)
q = tr.quaternion_from_matrix(R)
# compile pose
tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_MAP_ORIGIN,
name="map",
robot=self.robot_hostname
),
target=AutolabReferenceFrame(
time=rospy.Time.now(),
type=AutolabReferenceFrame.TYPE_GROUND_TAG,
name=f"tag/{target[3:]}",
robot=self.robot_hostname
),
is_fixed=True,
is_static=False,
transform=Transform(
translation=Vector3(x=position[0], y=position[1], z=0),
rotation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
)
)
# add tf
tfs.append(tf)
except (KeyError, AttributeError, ValueError):
pass
# ---
return tfs
class DistributedTFNode(DTROS):
PUBLISH_TF_STATIC_EVERY_SECS = 2
def __init__(self):
super(DistributedTFNode,
self).__init__(node_name='distributed_tf_node',
node_type=NodeType.SWARM)
# get static parameters
self.robot_hostname = rospy.get_param('~veh')
self.map_name = _sanitize_hostname(rospy.get_param('~map'))
self.tag_id = rospy.get_param('~tag_id')
self.min_distance_odom = rospy.get_param('~min_distance_odom')
self.max_time_between_poses = rospy.get_param(
'~max_time_between_poses')
# define local reference frames' names
self._tag_mount = f'{self.robot_hostname}/tag_mount'
self._footprint_frame = f'{self.robot_hostname}/footprint'
# create communication group
self._group = DTCommunicationGroup("/autolab/tf", AutolabTransform)
# create static tfs holder and access semaphore
self._static_tfs = {}
self._static_tfs_sem = threading.Semaphore(1)
self._tf_buffer = tf2_ros.Buffer()
self._tf_listener = tf2_ros.TransformListener(self._tf_buffer)
# create publishers
self._tf_pub = self._group.Publisher()
# fetch/publish right away and then set timers
if self.tag_id != '__NOTSET__':
threading.Thread(target=self._fetch_tag_tf).start()
self._pub_timer = rospy.Timer(
rospy.Duration(self.PUBLISH_TF_STATIC_EVERY_SECS),
self._publish_static_tfs)
# setup subscribers for odometry
self._odo_sub = rospy.Subscriber("~odometry_in",
Odometry,
self._cb_odometry,
queue_size=1)
self._pose_last = None
self._reminder = DTReminder(frequency=10)
def on_shutdown(self):
if hasattr(self, '_group') and self._group is not None:
self._group.shutdown()
def _fetch_tag_tf(self, *_):
origin = self._tag_mount
target = self._footprint_frame
# try to fetch the TF
while not self.is_shutdown:
try:
self.loginfo(f"Looking for TF[{origin}] -> [{target}]...")
transform = self._tf_buffer.lookup_transform(
origin, target, rospy.Time())
tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=transform.header.stamp,
type=AutolabReferenceFrame.TYPE_DUCKIEBOT_TAG,
# NOTE: fetch TF /tag_frame -> /footprint but publish tag/XYZ -> /footprint
name=f'tag/{self.tag_id}',
robot=self.robot_hostname),
target=AutolabReferenceFrame(
time=transform.header.stamp,
type=AutolabReferenceFrame.TYPE_DUCKIEBOT_FOOTPRINT,
name=target,
robot=self.robot_hostname),
is_fixed=True,
is_static=False,
transform=transform.transform)
# add TF to the list of TFs to publish
self._static_tfs_sem.acquire()
try:
self._static_tfs[(origin, target)] = tf
finally:
self._static_tfs_sem.release()
self.loginfo(
f"Found TF[{origin}] -> [{target}]. Stopping TF listener thread."
)
return
except (tf2_ros.LookupException, tf2_ros.ExtrapolationException):
self.logwarn(
f"Could not find a static TF [{origin}] -> [{target}]")
except tf2_ros.ConnectivityException:
pass
time.sleep(2)
def _publish_static_tfs(self, *_):
tfs = []
self._static_tfs_sem.acquire()
try:
for transform in self._static_tfs.values():
tfs.append(transform)
finally:
self._static_tfs_sem.release()
# publish
for tf in tfs:
self._tf_pub.publish(tf, destination=self.map_name)
def _cb_odometry(self, pose_now):
if self._pose_last is None:
self._pose_last = pose_now
return
if not self._reminder.is_time():
return
# Only add the transform if the new pose is sufficiently different
t_now_to_world = np.array([
pose_now.pose.pose.position.x, pose_now.pose.pose.position.y,
pose_now.pose.pose.position.z
])
t_last_to_world = np.array([
self._pose_last.pose.pose.position.x,
self._pose_last.pose.pose.position.y,
self._pose_last.pose.pose.position.z
])
o = pose_now.pose.pose.orientation
q_now_to_world = np.array([o.x, o.y, o.z, o.w])
o = self._pose_last.pose.pose.orientation
q_last_to_world = np.array([o.x, o.y, o.z, o.w])
q_world_to_last = q_last_to_world
q_world_to_last[3] *= -1
q_now_to_last = tr.quaternion_multiply(q_world_to_last, q_now_to_world)
world_to_last = np.matrix(tr.quaternion_matrix(q_world_to_last))
#now_to_last = np.matrix(tr.quaternion_matrix(q_now_to_last))
# print(now_to_last)
#now_to_last = now_to_last[0:3][:, 0:3]
R_world_to_last = world_to_last[0:3][:, 0:3]
# print(now_to_last)
#t_now_to_last = np.array(np.dot(now_to_last, t_now_to_world - t_last_to_world))
t_now_to_last = np.array(
np.dot(R_world_to_last, t_now_to_world - t_last_to_world))
t_now_to_last = t_now_to_last.flatten()
# compute TF between `pose_now` and `pose_last`
transform = Transform(translation=Vector3(x=t_now_to_last[0],
y=t_now_to_last[1],
z=t_now_to_last[2]),
rotation=Quaternion(x=q_now_to_last[0],
y=q_now_to_last[1],
z=q_now_to_last[2],
w=q_now_to_last[3]))
# pack TF into an AutolabTransform message
tf = AutolabTransform(
origin=AutolabReferenceFrame(
time=self._pose_last.header.stamp,
type=AutolabReferenceFrame.TYPE_DUCKIEBOT_FOOTPRINT,
name=self._footprint_frame,
robot=self.robot_hostname),
target=AutolabReferenceFrame(
time=pose_now.header.stamp,
type=AutolabReferenceFrame.TYPE_DUCKIEBOT_FOOTPRINT,
name=self._footprint_frame,
robot=self.robot_hostname),
is_fixed=False,
is_static=False,
transform=transform)
# publish
self._tf_pub.publish(tf, destination=self.map_name)
# update last pose
self._pose_last = pose_now