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