def tf_from_pose(self, pose_msg):
# Update message timestamp
self.tf_msg.header.stamp = rospy.Time.now()
# Update translation
self.tf_msg.transform.translation.x = pose_msg.position.x
self.tf_msg.transform.translation.y = pose_msg.position.y
self.tf_msg.transform.translation.z = pose_msg.position.z
# Update rotation
self.tf_msg.transform.rotation.x = pose_msg.orientation.x
self.tf_msg.transform.rotation.y = pose_msg.orientation.y
self.tf_msg.transform.rotation.z = pose_msg.orientation.z
self.tf_msg.transform.rotation.w = pose_msg.orientation.w
if sum ([self.tf_msg.transform.rotation.x, self.tf_msg.transform.rotation.y,
self.tf_msg.transform.rotation.z, self.tf_msg.transform.rotation.w]) != 0.0:
if self.invert == "True":
# Create transformerROS and add our transform
transformer = TransformerROS()
transformer.setTransform(self.tf_msg)
# Lookup the transform
(trans, rot) = transformer.lookupTransform(self.tf_msg.header.frame_id, self.tf_msg.child_frame_id, rospy.Time(0))
# Create transform object
transform = tft.concatenate_matrices(tft.translation_matrix(trans), tft.quaternion_matrix(rot))
# Invert
inverse_tf = tft.inverse_matrix(transform)
# Get translation, rotation vectors back out
translation = tft.translation_from_matrix(inverse_tf)
quaternion = tft.quaternion_from_matrix(inverse_tf)
# Get RPY
rpy = tft.euler_from_quaternion(quaternion)
rpy_new = [rpy[0], rpy[1], -rpy[2]]
# Back to quaternion
quaternion = tft.quaternion_from_euler(rpy_new[0], rpy_new[1], rpy_new[2])
# Update translation
self.tf_msg.transform.translation.x = translation[0]
self.tf_msg.transform.translation.y = -translation[1]
self.tf_msg.transform.translation.z = translation[2]
# Update rotation
self.tf_msg.transform.rotation.x = quaternion[0]
self.tf_msg.transform.rotation.y = quaternion[1]
self.tf_msg.transform.rotation.z = quaternion[2]
self.tf_msg.transform.rotation.w = quaternion[3]
# Publish transform
self.broadcaster.sendTransform(self.tf_msg)
def invert_pose(pose):
transformer = TransformerROS()
transform = transform_from_pose('frame1', 'frame2', pose)
transformer.setTransform(transform)
frame1_origin_frame1 = pose_stamped('frame1', [0.0, 0.0, 0.0],
[0.0, 0.0, 0.0, 1.0])
frame1_origin_frame2 = transformer.transformPose('frame2',
frame1_origin_frame1)
return frame1_origin_frame2.pose
def transform_ar_tag(message):
"""
Input: message of type AlvarMarkers
Transforms the encoded PoseStamped to the base frame
and stores it in transformed_message
"""
global transformed_message
if message.markers == []:
return
global counter
if counter > 1:
return
# Create a TransformerROS to transform the AR tag poses we get
t = TransformerROS(True, rospy.Duration(10.0))
# Wait for our transform and get it
tf_listener.waitForTransform('/head_camera', '/base', rospy.Time(),
rospy.Duration(5.0))
(trans, rot) = tf_listener.lookupTransform('/head_camera', '/base',
rospy.Time(0))
# Create our TransformStamped object
transform = TransformStamped()
transform.child_frame_id = 'base'
transform.header.frame_id = 'head_camera'
transform.header.stamp = rospy.Time(0)
transform.transform.translation.x = trans[0]
transform.transform.translation.y = trans[1]
transform.transform.translation.z = trans[2]
transform.transform.rotation.x = rot[0]
transform.transform.rotation.y = rot[1]
transform.transform.rotation.z = rot[2]
transform.transform.rotation.w = rot[3]
# Set the transform for t
t.setTransform(transform)
pose = message.markers[0].pose # Assume one marker for now
pose.header.frame_id = '/head_camera'
transformed_message = t.transformPose('/base', pose)
global board_x, board_y, board_z
board_x = transformed_message.pose.position.x
board_y = transformed_message.pose.position.y - 0.177 / 2
board_z = transformed_message.pose.position.z - 0.177 / 2
counter += 1
class Intersection4wayModel():
def __init__(self):
# Origin of ther intersection coordinate system is the center of stopline 0.
tile_side_length = 0.61
lane_width = 0.205
stopline_thickness = 0.048
center_markings_thickness = 0.025
q = lane_width
p = stopline_thickness
s = center_markings_thickness
self.stopline_poses = []
self.tf1 = TransformerROS()
self.tf2 = TransformerROS()
def add_stopline_transform(idx, position, angle):
orientation = unit_vector(
quaternion_from_euler(0, 0, angle, axes='sxyz'))
pose = utils.pose(position, orientation)
# Add transform for reference stopline poses
child_frame = self.stopline_frame(idx)
parent_frame = 'intersection'
transform = utils.transform_from_pose('intersection', child_frame,
pose)
self.tf1.setTransform(transform)
# Add transform for measured stopline poses
child_frame = self.intersection_frame(idx)
parent_frame = self.stopline_frame(idx)
inverted_pose = utils.invert_pose(utils.pose(
position, orientation))
transform = utils.transform_from_pose(parent_frame, child_frame,
inverted_pose)
self.tf2.setTransform(transform)
add_stopline_transform(0, [0.0, 0.0, 0.0], 0)
add_stopline_transform(
1, [-(q / 2.0 + s + q + p / 2.0), p / 2.0 + q / 2.0, 0.0],
-np.pi / 2.0)
add_stopline_transform(
2, [-(q / 2.0 + s + q / 2.0), p / 2.0 + 2 * q + s + p / 2.0, 0.0],
-np.pi)
add_stopline_transform(
3, [q / 2.0 + p / 2.0, p / 2.0 + q + s + q / 2.0, 0.0],
-3.0 * np.pi / 2.0)
def stopline_frame(self, idx):
return 'stopline_' + str(idx)
def intersection_frame(self, idx):
return 'intersection_' + str(idx)
# axle_pose needs to be in intersection coordinates
def get_stopline_poses_reference(self, axle_pose):
transform = utils.transform_from_pose('intersection', 'axle',
axle_pose)
self.tf1.setTransform(transform)
stopline_poses = [
self.tf1.transformPose('axle',
utils.origin_pose(
self.stopline_frame(i))).pose
for i in range(4)
]
return stopline_poses
def get_axle_pose_from_stopline_pose(self, stopline_pose, idx):
transform = utils.transform_from_pose('axle', self.stopline_frame(idx),
stopline_pose)
self.tf2.setTransform(transform)
axle_origin = utils.origin_pose('axle')
axle_pose = self.tf2.transformPose(self.intersection_frame(idx),
axle_origin)
return axle_pose
class LocalTransformer:
"""This class allows to use the TF class TransformerROS without using the ROS
network system or the topic /tf, where transforms are usually published to.
Instead a local transformer is saved and allows to publish local transforms,
as well the use of TFs convenient lookup functions (see functions below).
This class uses the robots (currently only one! supported) URDF file to
initialize the tfs for the robot. Moreover, the function update_local_transformer_from_btr
updates these tfs by copying the tfs state from the pybullet world."""
def __init__(self, map_frame, odom_frame, projection_namespace,
kitchen_namespace):
# Frame names of the map and odom frame
self.map_frame = map_frame
self.odom_frame = odom_frame
# Namespaces
self.projection_namespace = projection_namespace
self.kitchen_namespace = kitchen_namespace
# TF tf_stampeds and static_tf_stampeds of the Robot in the BulletWorld:
# These are initialized with the function init_transforms_from_urdf and are
# used to update the local transformer with the function update_local_transformer_from_btr
self.tf_stampeds = []
self.static_tf_stampeds = []
self.local_transformer = TransformerROS(interpolate=True,
cache_time=Duration(10.0))
self.init_local_tf_with_tfs_from_urdf()
def init_transforms_from_urdf(self):
"""
This static function gets with the robot name in robot_description.py, the
robots URDF file from the "resources" folder in the ROS "pycram" folder.
All joints of the URDF are extracted and saved in a list of static and
normal TFStamped.
:returns: list of static_tf_stampeds, list of tf_stampeds
"""
# Save joint translations and orientations in tf_stampeds
# static_tf_stampeds saves only the static joint translations and orientations
self.tf_stampeds = []
self.static_tf_stampeds = []
# Get URDF file path
robot_name = robot_description.i.name
rospack = rospkg.RosPack()
filename = rospack.get_path(
'pycram') + '/resources/' + robot_name + '.urdf'
# ... and open it
with open(filename) as f:
s = f.read()
robot = URDF.from_xml_string(s)
# Save for every joint in the robot the source_frame, target_frame,
# aswell the joints origin, where the translation xyz and rotation rpy
# are saved
for joint in robot.joints:
source_frame = self.projection_namespace + '/' + joint.parent
target_frame = self.projection_namespace + '/' + joint.child
if joint.origin:
if joint.origin.xyz:
translation = joint.origin.xyz
else:
translation = [0, 0, 0]
if joint.origin.rpy:
roll = joint.origin.rpy[0]
pitch = joint.origin.rpy[1]
yaw = joint.origin.rpy[2]
rotation = transformations.quaternion_from_euler(
roll, pitch, yaw)
else:
rotation = [0, 0, 0, 1]
if joint.name in robot_description.i.odom_joints:
# since pybullet wont update this joints, these are declared as static
translation = [0, 0, 0]
rotation = [0, 0, 0, 1]
# Wrap the joint attributes in a TFStamped and append it to static_tf_stamped if the joint was fixed
tf_stamped = helper.list2tfstamped(source_frame, target_frame,
[translation, rotation])
if (joint.type and joint.type == 'fixed'
) or joint.name in robot_description.i.odom_joints:
self.static_tf_stampeds.append(tf_stamped)
else:
self.tf_stampeds.append(tf_stamped)
def init_local_tf_with_tfs_from_urdf(self):
"This function initializes the local transformer with TFs from the robots URDF."
self.init_transforms_from_urdf()
for static_tfstamped in self.static_tf_stampeds:
self.local_transformer._buffer.set_transform_static(
static_tfstamped, "default_authority")
for tfstamped in self.tf_stampeds:
self.local_transformer.setTransform(tfstamped)
def update_robot_state(self):
robot = BulletWorld.robot
if robot:
# First publish (static) joint states to tf
# Update tf stampeds which might have changed
for tf_stamped in self.tf_stampeds:
source_frame = tf_stamped.header.frame_id
target_frame = tf_stamped.child_frame_id
# Push calculated transformation to the local transformer
p, q = robot.get_link_relative_to_other_link(
source_frame.replace(self.projection_namespace + "/", ""),
target_frame.replace(self.projection_namespace + "/", ""))
self.local_transformer.setTransform(
helper.list2tfstamped(source_frame, target_frame, [p, q]))
# Update the static transforms
for static_tf_stamped in self.static_tf_stampeds:
self.local_transformer._buffer.set_transform_static(
static_tf_stamped, "default_authority")
def update_robot_pose(self):
robot = BulletWorld.robot
if robot:
# Then publish pose of robot
try:
robot_pose = BulletWorld.robot.get_link_position_and_orientation_tf(
robot_description.i.base_frame)
except KeyError:
robot_pose = BulletWorld.robot.get_position_and_orientation()
self.publish_robot_pose(helper.ensure_pose(robot_pose))
def update_robot(self):
self.update_robot_state()
self.update_robot_pose()
def update_objects(self):
if BulletWorld.current_bullet_world:
for obj in list(BulletWorld.current_bullet_world.objects):
if obj.name == robot_description.i.name or obj.type == "environment":
continue
else:
published = local_transformer.publish_object_pose(
obj.name, obj.get_position_and_orientation())
if not published:
logerr("(publisher) Could not publish given pose of %s.",
obj.name)
def update_static_odom(self):
self.publish_pose(self.map_frame,
self.projection_namespace + '/' + self.odom_frame,
[[0, 0, 0], [0, 0, 0, 1]],
static=True)
def update_from_btr(self):
# Update static odom
self.update_static_odom()
# Update pose and state of robot
self.update_robot()
# Update pose of objects which are possibly attached on the robot
self.update_objects()
############################################################################################################
######################################### Query Local Transformer ##########################################
############################################################################################################
def tf_pose_transform(self, source_frame, target_frame, pose, time=None):
tf_pose = helper.ensure_pose(pose)
tf_time = time if time else self.local_transformer.getLatestCommonTime(
source_frame, target_frame)
tf_pose_stamped = PoseStamped(Header(0, tf_time, source_frame),
tf_pose)
return self.tf_pose_stamped_transform(target_frame, tf_pose_stamped)
def tf_pose_stamped_transform(self, target_frame, pose_stamped):
return helper.pose_stamped2tuple(
self.local_transformer.transformPose(target_frame, pose_stamped))
def tf_transform(self, source_frame, target_frame, time=None):
tf_time = time if time else self.local_transformer.getLatestCommonTime(
source_frame, target_frame)
return self.local_transformer.lookupTransform(source_frame,
target_frame, tf_time)
############################################################################################################
####################################### Publish to Local Transformer ###########'###########################
############################################################################################################
def publish_pose(self, frame_id, child_frame_id, pose, static=None):
if frame_id != child_frame_id:
pose = helper.ensure_pose(pose)
tf_stamped = helper.pose2tfstamped(frame_id, child_frame_id, pose)
if tf_stamped:
if static:
self.local_transformer._buffer.set_transform_static(
tf_stamped, "default_authority")
else:
self.local_transformer.setTransform(tf_stamped)
return True
def publish_object_pose(self, name, pose):
"""
Publishes the given pose of the object of given name in reference to the map frame to tf.
:type name: str
:type pose: list or Pose
"""
if name in robot_description.i.name:
return None
if name not in robot_description.i.name:
pose = helper.ensure_pose(pose)
if pose:
tf_name = self.projection_namespace + '/' + name if self.projection_namespace else name
return self.publish_pose(self.map_frame, tf_name, pose)
else:
logerr(
"(publisher) Could not publish given pose of %s since"
" it could not be converted to a Pose object.", name)
return None
def publish_robot_pose(self, pose):
"Publishes the base_frame of the robot in reference to the map frame to tf."
robot_pose = helper.ensure_pose(pose)
if robot_pose:
tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \
if self.projection_namespace \
else robot_description.i.base_frame
return self.publish_pose(self.map_frame, tf_base_frame, robot_pose)
else:
logerr("(publisher) Could not publish given pose of robot since"
" it could not be converted to a Pose object.")
return None
def publish_robots_link_pose(self, link, pose):
"Publishes the frame link of the robot in reference to the base_frame of the robot to tf."
robot_pose = helper.ensure_pose(pose)
if robot_pose:
tf_base_frame = self.projection_namespace + '/' + robot_description.i.base_frame \
if self.projection_namespace \
else robot_description.i.base_frame
tf_link_frame = self.projection_namespace + '/' + link \
if self.projection_namespace \
else link
if not tf_base_frame in tf_link_frame:
return self.publish_pose(tf_base_frame, tf_link_frame,
robot_pose)
else:
logerr("(publisher) Could not publish given pose of robot since"
" it could not be converted to a Pose object.")
return None
