Python TransformStamped.transform方法代码示例

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
    def _compute_transform(self, sensor_data, cur_time):
        parent_frame_id = "base_link"
        child_frame_id = self.name

        t = TransformStamped()
        t.header.stamp = cur_time
        t.header.frame_id = parent_frame_id
        t.child_frame_id = child_frame_id

        # for camera we reorient it to look at the same axis as the opencv projection
        # in order to get easy depth cloud for RGBD camera
        t.transform = carla_transform_to_ros_transform(
            self.carla_object.get_transform())

        rotation = t.transform.rotation
        quat = [rotation.x, rotation.y, rotation.z, rotation.w]
        quat_swap = tf.transformations.quaternion_from_matrix(
            [[0, 0, 1, 0],
            [-1, 0, 0, 0],
            [0, -1, 0, 0],
            [0, 0, 0, 1]])
        quat = tf.transformations.quaternion_multiply(quat, quat_swap)

        t.transform.rotation.x = quat[0]
        t.transform.rotation.y = quat[1]
        t.transform.rotation.z = quat[2]
        t.transform.rotation.w = quat[3]

        self.process_msg_fun('tf', t)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
    def tag_callback(self, msg_tag):
        # Listen for the transform of the tag in the world
        avg = PoseAverage.PoseAverage()
        for tag in msg_tag.detections:
            try:
                Tt_w = self.tfbuf.lookup_transform(self.world_frame, "tag_{id}".format(id=tag.id), rospy.Time(), rospy.Duration(1))
                Mtbase_w=self.transform_to_matrix(Tt_w.transform)
                Mt_tbase = tr.concatenate_matrices(tr.translation_matrix((0,0,0.17)), tr.euler_matrix(0,0,np.pi))
                Mt_w = tr.concatenate_matrices(Mtbase_w,Mt_tbase)
                Mt_r=self.pose_to_matrix(tag.pose)
                Mr_t=np.linalg.inv(Mt_r)
                Mr_w=np.dot(Mt_w,Mr_t)
                Tr_w = self.matrix_to_transform(Mr_w)
                avg.add_pose(Tr_w)
                self.publish_sign_highlight(tag.id)
            except(tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as ex:
                rospy.logwarn("Error looking up transform for tag_%s", tag.id)
                rospy.logwarn(ex.message)

        Tr_w =  avg.get_average() # Average of the opinions

        # Broadcast the robot transform
        if Tr_w is not None:
            # Set the z translation, and x and y rotations to 0
            Tr_w.translation.z = 0
            rot = Tr_w.rotation
            rotz=tr.euler_from_quaternion((rot.x, rot.y, rot.z, rot.w))[2]
            (rot.x, rot.y, rot.z, rot.w) = tr.quaternion_from_euler(0, 0, rotz)
            T = TransformStamped()
            T.transform = Tr_w
            T.header.frame_id = self.world_frame
            T.header.stamp = rospy.Time.now()
            T.child_frame_id = self.duckiebot_frame
            self.pub_tf.publish(TFMessage([T]))
            self.lifetimer = rospy.Time.now()

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def transform_helper(vec3, frame):
    pos = TransformStamped()
    pos.child_frame_id = frame
    pos.header = Header()
    pos.transform = Transform()
    pos.transform.translation = vec3

    return pos

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
    def run(self):
        marker_map = rospy.get_param('~marker_map', {})
        args = {
            'device': rospy.get_param('~device'),
            'marker_map': marker_map,
            'callback_global': self.callback_global,
            'callback_local': self.callback_local,
        }
        parameters = self.get_options()

        self.fixed_frame_id = rospy.get_param('~fixed_frame_id', 'map')
        self.robot_frame_id = rospy.get_param('~robot_frame_id', 'base_link')
        self.stargazer_frame_id = rospy.get_param('~stargazer_frame_id', 'stargazer')
        self.map_frame_prefix = rospy.get_param('~map_frame_prefix', 'stargazer/map_')
        self.marker_frame_prefix = rospy.get_param('~marker_frame_prefix', 'stargazer/marker_')

        self.covariance = rospy.get_param('~covariance', None)
        if self.covariance is None:
            raise Exception('The "covariance" parameter is required.')
        elif len(self.covariance) != 36:
            raise Exception('The "covariance" parameter must be a 36 element vector.')

        # Publish static TF frames for the Stargazer map.
        stamp_now = rospy.Time.now()
        map_tf_msg = TFMessage()

        for marker_id, Tmap_marker in marker_map.iteritems():
            marker_tf_msg = TransformStamped()
            marker_tf_msg.header.stamp = stamp_now
            marker_tf_msg.header.frame_id = self.fixed_frame_id
            marker_tf_msg.child_frame_id = self.map_frame_prefix + marker_id
            marker_tf_msg.transform = matrix_to_transform(Tmap_marker)
            map_tf_msg.transforms.append(marker_tf_msg)

        self.tf_static_pub = rospy.Publisher('tf_static', TFMessage, latch=True)
        self.tf_static_pub.publish(map_tf_msg)
        
        # Start publishing Stargazer data.
        with StarGazer(**args) as stargazer:
            # The StarGazer might be streaming data. Turn off streaming mode.
            stargazer.stop_streaming()

            # Set all parameters, possibly to their default values. This is the
            # safest option because the parameters can be corrupted when the
            # StarGazer is powered off.
            for name, value in parameters.iteritems():
                stargazer.set_parameter(name, value)

            # Start streaming. ROS messages will be published in callbacks.
            stargazer.start_streaming()
            rospy.spin()

            # Stop streaming. Try to clean up after ourselves.
            stargazer.stop_streaming()

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
 def make_transform_stamped(self, stamp, parent_frame, child_frame):
     try:
         (trans,rot) = self.listener.lookupTransform(parent_frame, child_frame, rospy.Time(0)) # current map->base transform
     except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
         rospy.logwarn("[%s]" + " Failed to solve tf: %s to %s", rospy.get_name(), parent_frame, child_frame)
         return None
     tf_stamped = TransformStamped()
     tf_stamped.header.stamp = stamp
     tf_stamped.header.frame_id = parent_frame
     tf_stamped.child_frame_id = child_frame
     tf_stamped.transform = Transform(Vector3(*trans), Quaternion(*rot))
     return tf_stamped

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
 def to_tf_stamped_msg(*args):
     header, homo_mat, quat_rot, _ = PoseConverter._make_generic(args)
     if homo_mat is None:
         rospy.logwarn("[pose_converter] Unknown pose type.")
         return None, None, None, None
     tf_stamped = TransformStamped()
     if header is None:
         tf_stamped.header.stamp = rospy.Time.now()
     else:
         tf_stamped.header.seq = header[0]
         tf_stamped.header.stamp = header[1]
         tf_stamped.header.frame_id = header[2]
     tf_stamped.transform = Transform(Vector3(*homo_mat[:3,3].T.A[0]), Quaternion(*quat_rot))
     return tf_stamped

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
 def process_msg(self, data, cur_time):
     t = TransformStamped()
     t.header.stamp = cur_time
     t.header.frame_id = self.world_link
     t.child_frame_id = "base_link"
     t.transform = carla_transform_to_ros_transform(
         carla_Transform(data.transform))
     header = Header()
     header.stamp = cur_time
     header.frame_id = self.world_link
     marker = get_vehicle_marker(
         data, header=header, marker_id=0, is_player=True)
     self.process_msg_fun(self.name, marker)
     self.process_msg_fun('tf', t)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
    def recalculate_transform(self, currentTime):
        """
        Creates updated transform from /odom to /map given recent odometry and
        laser data.
        
        :Args:
            | currentTime (rospy.Time()): Time stamp for this update
         """
        
        transform = Transform()

        T_est = transformations.quaternion_matrix([self.estimatedpose.pose.pose.orientation.x,
                                                   self.estimatedpose.pose.pose.orientation.y,
                                                   self.estimatedpose.pose.pose.orientation.z,
                                                   self.estimatedpose.pose.pose.orientation.w])
        T_est[0, 3] = self.estimatedpose.pose.pose.position.x
        T_est[1, 3] = self.estimatedpose.pose.pose.position.y
        T_est[2, 3] = self.estimatedpose.pose.pose.position.z
        
        T_odom = transformations.quaternion_matrix([self.last_odom_pose.pose.pose.orientation.x,
                                                   self.last_odom_pose.pose.pose.orientation.y,
                                                   self.last_odom_pose.pose.pose.orientation.z,
                                                   self.last_odom_pose.pose.pose.orientation.w])
        T_odom[0, 3] = self.last_odom_pose.pose.pose.position.x
        T_odom[1, 3] = self.last_odom_pose.pose.pose.position.y
        T_odom[2, 3] = self.last_odom_pose.pose.pose.position.z
        T = np.dot(T_est, np.linalg.inv(T_odom))
        q = transformations.quaternion_from_matrix(T) #[:3, :3])

        transform.translation.x = T[0, 3] 
        transform.translation.y = T[1, 3] 
        transform.translation.z = T[2, 3] 
        transform.rotation.x = q[0]
        transform.rotation.y = q[1]
        transform.rotation.z = q[2]
        transform.rotation.w = q[3]
        

        # Insert new Transform into a TransformStamped object and add to the
        # tf tree
        new_tfstamped = TransformStamped()
        new_tfstamped.child_frame_id = "/odom"
        new_tfstamped.header.frame_id = "/map"
        new_tfstamped.header.stamp = currentTime
        new_tfstamped.transform = transform

        # Add the transform to the list of all transforms
        self.tf_message = tfMessage(transforms=[new_tfstamped])

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def pose_to_transform(pose, name, time):
    tr = Transform()
    tr.translation.x = pose.position.x
    tr.translation.y = pose.position.y
    tr.translation.z = pose.position.z
    tr.rotation.x = pose.orientation.x
    tr.rotation.y = pose.orientation.y
    tr.rotation.z = pose.orientation.z
    tr.rotation.w = pose.orientation.w

    trs = TransformStamped()
    trs.transform = tr
    trs.header.stamp = time
    trs.header.frame_id = 'world'
    trs.child_frame_id = name
    return trs

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
  def run(self):
    rospy.init_node('calibrate_bundle')
    next_pub = rospy.Publisher('next_poses', String, queue_size = 1)
    tf_b = tf.TransformBroadcaster()
    tf_l = tf.TransformListener()
    
    rate = rospy.Rate(10)
    
    rospy.Service('commit_next_poses', CommitNextPoses, self.commit_next_poses )
    rospy.Service('save_known_poses', SaveKnownPoses, self.save_known_poses )

    main_frame_avg = self.main_marker + '_avg'
    main_frame_bundle = self.main_marker + '_bundle'
    
    while not rospy.is_shutdown():
      self.next_poses = {}
      for bundle_marker in self.bundle_markers:
        marker_avg_frame = bundle_marker + '_avg'
        marker_bundle_frame = bundle_marker + '_bundle'
        if marker_bundle_frame not in self.known_poses.keys():
          try:
            now = rospy.Time.now()
            if (now - tf_l.getLatestCommonTime(main_frame_avg, marker_avg_frame)) < TIME_THRESH:
              T,R = tf_l.lookupTransform(main_frame_avg, marker_avg_frame, rospy.Time(0))
              tfs = TransformStamped()
              tfs.header.stamp = now
              tfs.header.frame_id = main_frame_bundle
              tfs.child_frame_id = marker_bundle_frame
              tfs.transform = numpy_to_tf_msg(T,R)
              self.next_poses[tfs.child_frame_id] = tfs

          except (
            tf.Exception, tf.LookupException, tf.ConnectivityException,
            tf.ExtrapolationException) as e :
            continue

      tf_b.sendTransform([0,0,0],[0,0,0,1],now, main_frame_bundle, main_frame_avg)
      
      for kp in self.known_poses.values():
        base_frame = kp.header.frame_id
        child_frame = kp.child_frame_id
        T,R = tf_msg_to_numpy(kp)
        tf_b.sendTransform(T,R,now,child_frame,base_frame)
      
      next_pub.publish(str(self.next_poses.keys()))
      
      rate.sleep()

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def tf_from_tfmat(mat):
    tf_stamped = TransformStamped()
    _tf = Transform()

    t_vec = tf.transformations.translation_from_matrix(mat)
    _tf.translation.x = t_vec[0]
    _tf.translation.y = t_vec[1]
    _tf.translation.z = t_vec[2]

    q_vec = tf.transformations.quaternion_from_matrix(mat)
    _tf.rotation.x = q_vec[0]
    _tf.rotation.y = q_vec[1]
    _tf.rotation.z = q_vec[2]
    _tf.rotation.w = q_vec[3]
    tf_stamped.transform = _tf
    # header and child_frame_id needs to be defined
    return tf_stamped

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def main():
    rospy.init_node('helmet')
    pub = rospy.Publisher('helmet', TransformStamped, queue_size=10)

    tx, ty, tz = 0, 0, 0
    w0, w1, w2 = 0, 1, 0
    theta = 0 #-5 * pi / 180
    r = rospy.Rate(100)
    while True:
        tx, ty, tz = tx+delt(), ty+delt(), tz+delt()
        w0, w1, w2, = w0+delt(), w1+delt(), w2+delt()
        theta = (theta+delt()) % (2*pi)
        
        quat = ks.rot_to_quaternion(ks.rotation_3d(np.array([w0, w1, w2]), theta))
        translation = Vector3(tx, ty, tz)
        rotation = Quaternion(*quat)
        transform = Transform(translation, rotation)
        ts = TransformStamped()
        ts.transform = transform

        pub.publish(ts)
        r.sleep()

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def avg_transforms(tf_groups):
  avgs = []
  for tf_group in tf_groups:
    trans = numpy.empty((len(tf_group),3))
    rots = numpy.empty((len(tf_group),4))
    for i in range(len(tf_group)):
      trans[i,:], rots[i,:] = tf_msg_to_numpy(tf_group[i])
  
    avg_trans = trans.mean(axis=0)
    avg_rot = rots.mean(axis=0)
    # Assuming quaternions are close, averaging and normalizing should be close
    # to the desired average quaternion
    avg_rot[3] = (1.0-(avg_rot[:3]**2).sum())**0.5

    avg_tf = TransformStamped()
    avg_tf.header.frame_id = tf_group[0].header.frame_id
    avg_tf.child_frame_id = tf_group[0].child_frame_id
    avg_tf.transform = numpy_to_tf_msg(avg_trans, avg_rot)

    avgs.append(avg_tf)
  
  return avgs

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import transform [as 别名]
def main():

    parser = argparse.ArgumentParser(description="Create the tf messages from "
                                                 "the Tango logs.")
    parser.add_argument('-d', '--directory',
                        help="The folder must have images.txt, "
                             "depth_to_imu_transformation.txt, "
                             "fisheye_to_imu_transformation.txt, "
                             "narrow_to_imu_transformation.txt",
                        required=True)
    parser.add_argument('-o', '--output',
                        help='output bag file (with location)',
                        metavar='bag_filename',
                        type=str,
                        required=True
                        )
    parser.add_argument('-y',
                        help='if images_adjusted.txt is found in the same folder'
                             'as the supplied filename, then use it without'
                             'asking',
                        metavar='True/False',
                        type=bool,
                        default=False,
                        choices=[True, False]
                        )

    arguments = parser.parse_args()
    folder_name = arguments.directory
    standard_file = os.path.join(folder_name, 'images.txt')
    alt_file = os.path.join(folder_name, 'images_adjusted.txt')
    if os.path.exists(alt_file):
        if arguments.y:
            images_file = open(alt_file)
        else:
            reply = None
            while reply not in ['y', 'n']:
                print("The images_adjusted.txt file is in the folder {}, do you"
                      " want to use that instead? [y/n]".format(folder_name))
                reply = raw_input()
            if reply == 'y':
                images_file = open(alt_file)
            else:
                images_file = open(standard_file)
    else:
        images_file = open(standard_file)
    print("Processing data from {}...".format(images_file.name))

    #depth to imu
    depth_np = np.loadtxt(os.path.join(folder_name,
                                       "depth_to_imu_transformation.txt",
                                       ),
                          delimiter=',').reshape(3, 4)
    rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
    quaternion = transformations.quaternion_from_matrix(rotation_matrix)
    depth_transform_msg = Transform()
    depth_transform_msg.translation.x = rotation_matrix[0, 3]
    depth_transform_msg.translation.y = rotation_matrix[1, 3]
    depth_transform_msg.translation.z = rotation_matrix[2, 3]
    depth_transform_msg.rotation.x = quaternion[0]
    depth_transform_msg.rotation.y = quaternion[1]
    depth_transform_msg.rotation.z = quaternion[2]
    depth_transform_msg.rotation.w = quaternion[3]

    #fisheye to imu
    depth_np = np.loadtxt(os.path.join(folder_name,
                                       "fisheye_to_imu_transformation.txt"),
                          delimiter=',').reshape(3, 4)
    rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
    quaternion = transformations.quaternion_from_matrix(rotation_matrix)
    fisheye_transform_msg = Transform()
    fisheye_transform_msg.translation.x = rotation_matrix[0, 3]
    fisheye_transform_msg.translation.y = rotation_matrix[1, 3]
    fisheye_transform_msg.translation.z = rotation_matrix[2, 3]
    fisheye_transform_msg.rotation.x = quaternion[0]
    fisheye_transform_msg.rotation.y = quaternion[1]
    fisheye_transform_msg.rotation.z = quaternion[2]
    fisheye_transform_msg.rotation.w = quaternion[3]

    #narrow to imu
    depth_np = np.loadtxt(os.path.join(folder_name,
                                       "narrow_to_imu_transformation.txt"),
                          delimiter=',').reshape(3, 4)
    rotation_matrix = np.vstack((depth_np, [0, 0, 0, 1]))
    quaternion = transformations.quaternion_from_matrix(rotation_matrix)
    narrow_transform_msg = Transform()
    narrow_transform_msg.translation.x = rotation_matrix[0, 3]
    narrow_transform_msg.translation.y = rotation_matrix[1, 3]
    narrow_transform_msg.translation.z = rotation_matrix[2, 3]
    narrow_transform_msg.rotation.x = quaternion[0]
    narrow_transform_msg.rotation.y = quaternion[1]
    narrow_transform_msg.rotation.z = quaternion[2]
    narrow_transform_msg.rotation.w = quaternion[3]

    #for each entry in images.txt, add a transformation with the values above
    #TODO skip some values? we don't need high frame rate!
    with rosbag.Bag(arguments.output, 'w') as outputbag:
        for lineno, line in enumerate(images_file):
            #lines must be of the form:
            #image_basename,t_android,t_movidius,r11,r12,r12,t1,r21,r22,r23,t2,r31,r32,r33,t3
            tokens = line.strip('\n').split(',')
#.........这里部分代码省略.........