Python TransformStamped.child_frame_id方法代码示例

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def frame_tf(msg):
    br = tf2_ros.TransformBroadcaster()
    t = TransformStamped()
    t.header.stamp = rospy.Time.now()
    t.header.frame_id = "ned"
    t.child_frame_id = "vicon"
    t.transform.translation.x=0
    t.transform.translation.y=0
    t.transform.translation.z=0
    q = tf.transformations.quaternion_from_euler(math.pi, 0, -53.0*math.pi/180)
    t.transform.rotation.x = q[0]
    t.transform.rotation.y = q[1]
    t.transform.rotation.z = q[2]
    t.transform.rotation.w = q[3]
    br.sendTransform(t)

    t2 = TransformStamped()
    t2.header.stamp = rospy.Time.now()
    t2.header.frame_id = "vicon"
    t2.child_frame_id = "uav"
    t2.transform.translation.x = msg.pose.position.x
    t2.transform.translation.y = msg.pose.position.y
    t2.transform.translation.z = msg.pose.position.z
    t2.transform.rotation.x = msg.pose.orientation.x
    t2.transform.rotation.y = msg.pose.orientation.y
    t2.transform.rotation.z = msg.pose.orientation.z
    t2.transform.rotation.w = msg.pose.orientation.w
    br.sendTransform(t2)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
    def _publish_tf(self, stamp):
        # check that we know which frames we need to publish from
        if self.map is None or self.base_frame is None:
            rospy.loginfo('Not publishing until map and odometry frames found')
            return

        # calculate the mean position
        x = np.mean([p.x for p in self.particles])
        y = np.mean([p.y for p in self.particles])
        theta = np.mean([p.theta for p in self.particles]) #TODO - wraparound

        # map to base_link
        map2base_frame = PyKDL.Frame(
            PyKDL.Rotation.Quaternion(*transformations.quaternion_from_euler(0, 0, theta)),
            PyKDL.Vector(x,y,0)
        )

        odom2base_frame = tf2_kdl.transform_to_kdl(self.tf_buffer.lookup_transform(
            target_frame=self.odom_frame,
            source_frame=self.base_frame,
            time=stamp,
            timeout=rospy.Duration(4.0)
        ))

        # derive frame according to REP105
        map2odom = map2base_frame * odom2base_frame.Inverse() 

        br = tf2_ros.TransformBroadcaster()
        t = TransformStamped()

        t.header.stamp = stamp
        t.header.frame_id = self.map.frame
        t.child_frame_id = self.odom_frame
        t.transform.translation = Vector3(*map2odom.p)
        t.transform.rotation = Quaternion(*map2odom.M.GetQuaternion())
        br.sendTransform(t)


        # for Debugging
        if False:
            t.header.stamp = stamp
            t.header.frame_id = self.map.frame
            t.child_frame_id = self.base_frame+"_old"
            t.transform.translation = Vector3(*map2base_frame.p)
            t.transform.rotation = Quaternion(*map2base_frame.M.GetQuaternion())
            br.sendTransform(t)

        if self.publish_cloud:
            msg = PoseArray(
                header=Header(stamp=stamp, frame_id=self.map.frame),
                poses=[
                    Pose(
                        position=Point(p.x, p.y, 0),
                        orientation=Quaternion(*transformations.quaternion_from_euler(0, 0, p.theta))
                    )
                    for p in self.particles
                ]
            )
            self.pose_array.publish(msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def read_nav_csv(fname, origin):
    ''' Reads the nav.csv , converts into a local coordinate frame and returns tf msgs '''
    msgs = []
    DEG2RAD = m.pi/180.0  
    with open(fname, 'r') as f:
        # example data 
        # 1354679075.295000000,6248548.85357,332949.839026,-41.4911136152,-0.00740605127066,-0.0505019649863,1.95254564285
        # a Nx7 matrix containing pose [time N E D R P Y] UTM cartesian coordinate system  
        for i, line in enumerate(f):
            words = line.split(',')
            time = words[0]
            [secs, nsecs]  = map(int, time.split('.'))
            x = float(words[1])
            y = float(words[2])
            z = float(words[3])
            R = float(words[4])
            P = float(words[5])
            Y = float(words[6])
            # convert to local coordinate frame, located at the origin 
            x = x - origin[0]
            y = y - origin[1]
            z = z - origin[2]
            # create TF msg 
            tf_msg = tfMessage()
            geo_msg = TransformStamped()
            geo_msg.header.stamp = Time(secs,nsecs) 
            geo_msg.header.seq = i
            geo_msg.header.frame_id = "map"
            geo_msg.child_frame_id = "body"
            geo_msg.transform.translation.x = x
            geo_msg.transform.translation.y = y
            geo_msg.transform.translation.z = z
            angles = tf.transformations.quaternion_from_euler(R,P,Y) 
            geo_msg.transform.rotation.x = angles[0]
            geo_msg.transform.rotation.y = angles[1]
            geo_msg.transform.rotation.z = angles[2]
            geo_msg.transform.rotation.w = angles[3]
            # rviz frame 
            geo_msg2 = TransformStamped()
            geo_msg2.header.stamp = Time(secs,nsecs) 
            geo_msg2.header.seq = i
            geo_msg2.header.frame_id = "map_rviz"
            geo_msg2.child_frame_id = "map"
            geo_msg2.transform.translation.x = 0
            geo_msg2.transform.translation.y = 0
            geo_msg2.transform.translation.z = 0
            angles = tf.transformations.quaternion_from_euler(DEG2RAD*180, 0, 0) # ax, ay, az 
            geo_msg2.transform.rotation.x = angles[0]
            geo_msg2.transform.rotation.y = angles[1]
            geo_msg2.transform.rotation.z = angles[2]
            geo_msg2.transform.rotation.w = angles[3]
            # push all transformations 
            tf_msg.transforms.append(geo_msg)
            tf_msg.transforms.append(geo_msg2)
            msgs.append(tf_msg)

    return msgs

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
    def model_state_cb(self, msg):
        self.names = set( rospy.get_param('/nav_experiments/people', []) )
        objects = rospy.get_param('/nav_experiments/scenario/objects', {})

        people_list = PositionMeasurementArray()
        people_list.header.stamp = rospy.Time.now()
        people_list.header.frame_id = '/map'
        for name, pose, twist in zip(msg.name, msg.pose, msg.twist):
            if len(self.names)==len(people_list.people):
                break
            if name not in self.names:
                continue

            p = PositionMeasurement()
            oname = name[:-10]
            p.name = oname
            p.object_id = oname
            p.reliability = 1.0
            

            properties = objects[oname]
            if 'movement' not in properties:
                p.pos.x = properties['xyz'][0]
                p.pos.y = properties['xyz'][1]
                p.pos.z = properties['xyz'][2]
            else:
                trans = TransformStamped()
                trans.header.frame_id = '/map'
                trans.child_frame_id = '/start'
                trans.transform.translation.x = properties['xyz'][0]
                trans.transform.translation.y = properties['xyz'][1]
                trans.transform.translation.z = properties['xyz'][2]
                trans.transform.rotation.x = 0
                trans.transform.rotation.y = 0
                trans.transform.rotation.z = 0
                trans.transform.rotation.w = 1
                self.tf.setTransform(trans)
                trans.header.frame_id = '/start'
                trans.child_frame_id = '/pos'
                trans.transform.translation = pose.position
                self.tf.setTransform(trans)
                nt = self.tf.lookupTransform('/map', '/pos', rospy.Time(0))
                
                p.pos.x = nt[0][0]
                p.pos.y = nt[0][1]
                p.pos.z = nt[0][2]
            p.header.stamp = people_list.header.stamp
            p.header.frame_id = '/map'
            people_list.people.append(p)
        self.pub.publish(people_list)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def post_tf(self, component_instance):
    component_name = component_instance.blender_obj.name
    frame_id = self._properties[component_name]['frame_id']
    child_frame_id = self._properties[component_name]['child_frame_id']

    euler = mathutils.Euler((component_instance.local_data['roll'],
                             component_instance.local_data['pitch'],
                             component_instance.local_data['yaw']))
    quaternion = euler.to_quaternion()

    t = TransformStamped()
    t.header.frame_id = frame_id
    t.header.stamp = rospy.Time.now()
    t.child_frame_id = child_frame_id
    t.transform.translation.x = component_instance.local_data['x']
    t.transform.translation.y = component_instance.local_data['y']
    t.transform.translation.z = component_instance.local_data['z']

    t.transform.rotation = quaternion

    tfm = tfMessage([t])

    for topic in self._topics:
        # publish the message on the correct topic    
        if str(topic.name) == str("/tf"):
            topic.publish(tfm)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def pose_to_tf_msg(parent, child, position, orientation):
  ts = TransformStamped()
  ts.header.frame_id = parent
  ts.child_frame_id = child
  ts.transform.rotation = Quaternion(*orientation)
  ts.transform.translation = Vector3(*position)
  return ts

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
    def _toTransform(self, my_x, my_y):
        transform = TransformStamped()
        transform.header.stamp = rospy.Time.now()
        transform.header.frame_id = self.camera_frame
        transform.child_frame_id = self.blob.name

        (x, y, z) = self._projectTo3d(my_x, my_y)
        transform.transform.translation.x = x
        transform.transform.translation.y = y
        transform.transform.translation.z = z

        transform.transform.rotation.w = 1.0

        # If our parent frame is not the camera frame then an additional transformation is required.
        if self.parent_frame != self.camera_frame:
            point = PointStamped()
            point.header.frame_id = self.camera_frame
            point.header.stamp = rospy.Time(0)
            point.point.x = transform.transform.translation.x
            point.point.y = transform.transform.translation.y
            point.point.z = transform.transform.translation.z

            # Now we've gone from the regular camera frame to the correct parent_frame.
            point_transformed = self.listener.transformPoint(self.parent_frame, point)

            transform.header.frame_id = self.parent_frame
            transform.transform.translation.x = point_transformed.point.x
            transform.transform.translation.y = point_transformed.point.y
            transform.transform.translation.z = point_transformed.point.z

        return transform

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
    def transformer(data):
        for marker in data.markers:
            if marker.subject_name:
                if marker.subject_name not in crazyflies:
                    crazyflies[marker.subject_name] = {}
                crazyflie = crazyflies[marker.subject_name]
                crazyflie[marker.marker_name] = marker.translation
                crazyflie["frame_id"] = str(data.frame_number)

        transforms = []
        for crazyflie_name in crazyflies:
            crazyflie = crazyflies[crazyflie_name]
            trans = TransformStamped()
            # trans.child_frame_id = crazyflie["frame_id"]
            trans.child_frame_id = "1"
            trans.header.frame_id = crazyflie["frame_id"]

            top = marker_to_vector(crazyflie["top"])
            bottom = marker_to_vector(crazyflie["bot"])
            left = marker_to_vector(crazyflie["left"])
            front = marker_to_vector(crazyflie["front"])

            center = get_center(top, left, bottom)
            rotation = get_rotation(center, front, top, left, bottom)

            mTrans = trans.transform.translation
            mTrans.x, mTrans.y, mTrans.z = center
            mRot = trans.transform.rotation
            mRot.x, mRot.y, mRot.z, mRot.w = rotation
            transforms.append(trans)

        msg.transforms = transforms
        pubtf.publish(msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [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 child_frame_id [as 别名]
    def publishLatchTransform(self, arm):
        if arm == 'left':
            self.pub_tf_left = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True)
        else:
            self.pub_tf_right = rospy.Publisher("/tf", tfMessage, queue_size=1, latch=True)
            
        f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/transform_" + arm + \
            ".txt", "r")
        transform = f.readline().split()
        f.close()
        print(transform)
        # Send static link transforms
        msg = TransformStamped()

        msg.header.stamp = rospy.Time.now()
        msg.transform.rotation.x =  float(transform[3])
        msg.transform.rotation.y =  float(transform[4])
        msg.transform.rotation.z =  float(transform[5])
        msg.transform.rotation.w =  float(transform[6])
        msg.child_frame_id = "left_BC"   
        msg.transform.translation.x = float(transform[0])
        msg.transform.translation.y = float(transform[1])
        msg.transform.translation.z = float(transform[2])
        if arm == 'left':
            # msg.header.frame_id = "two_psm_base_link"
            msg.header.frame_id = "two_remote_center_link"
        else:
            msg.header.frame_id = "one_remote_center_link"
        while not rospy.is_shutdown():
                msg.header.stamp = rospy.Time.now()
                self.pub_tf_right.publish([msg])
                rospy.sleep(0.5)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def pack_transform(src_frame,dest_frame,trans,rot):
    transf = TransformStamped()    
    transf.header.frame_id = dest_frame
    transf.child_frame_id = src_frame
    transf.transform.translation = trans
    transf.transform.rotation = rot
    return transf

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def sendTransform(self, translation, rotation, time, child, parent):
        """
        :param translation: the translation of the transformtion as a tuple (x, y, z)
        :param rotation: the rotation of the transformation as a tuple (x, y, z, w)
        :param time: the time of the transformation, as a rospy.Time()
        :param child: child frame in tf, string
        :param parent: parent frame in tf, string

        Broadcast the transformation from tf frame child to parent on ROS topic ``"/tf"``.
        """

        t = TransformStamped()
        t.header.frame_id = parent
        t.header.stamp = time
        t.child_frame_id = child
        t.transform.translation.x = translation[0]
        t.transform.translation.y = translation[1]
        t.transform.translation.z = translation[2]

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

        tfm = tfMessage([t])
        self.pub_tf.publish(tfm)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
    def publish_state(self, t):
        state_msg = TransformStamped()
        t_ned_imu = tft.translation_matrix(self.model.get_position())
        R_ned_imu = tft.quaternion_matrix(self.model.get_orientation())
        T_ned_imu = tft.concatenate_matrices(t_ned_imu, R_ned_imu)
        #rospy.loginfo("T_ned_imu = \n" + str(T_ned_imu))
        if self.T_imu_vicon is None:
            # grab the static transform from imu to vicon frame from param server:
            frames = rospy.get_param("frames", None)
            ypr = frames['vicon_to_imu']['rotation']
            q_vicon_imu = tft.quaternion_from_euler(*[radians(x) for x in ypr], axes='rzyx') # xyzw
            R_vicon_imu = tft.quaternion_matrix(q_vicon_imu)
            t_vicon_imu = tft.translation_matrix(frames['vicon_to_imu']['translation'])
#            rospy.loginfo(str(R_vicon_imu))
#            rospy.loginfo(str(t_vicon_imu))
            self.T_vicon_imu = tft.concatenate_matrices(t_vicon_imu, R_vicon_imu)
            self.T_imu_vicon = tft.inverse_matrix(self.T_vicon_imu)
            self.T_enu_ned = tft.euler_matrix(radians(90), 0, radians(180), 'rzyx')
            rospy.loginfo(str(self.T_enu_ned))
            rospy.loginfo(str(self.T_imu_vicon))
            #rospy.loginfo(str(T_vicon_imu))
        # we have /ned -> /imu, need to output /enu -> /vicon:
        T_enu_vicon = tft.concatenate_matrices(self.T_enu_ned, T_ned_imu, self.T_imu_vicon )
        state_msg.header.stamp  = t
        state_msg.header.frame_id = "/enu"
        state_msg.child_frame_id = "/simflyer1/flyer_vicon"
        stt = state_msg.transform.translation
        stt.x, stt.y, stt.z = T_enu_vicon[:3,3]
        stro = state_msg.transform.rotation
        stro.x, stro.y, stro.z, stro.w = tft.quaternion_from_matrix(T_enu_vicon)
        
        self.pub_state.publish(state_msg)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
 def broadcast(self):
     f = open("/home/davinci2/catkin_ws/src/davinci_vision/launch/BC_registration/camera_frame.p", "rb")
     self.camera_transform = pickle.load(f)
     f.close()
     #SOMETIMES NEED TO INVERT X & Y AXES; just change from 1 to -1 and vice versa
     offset = tfx.transform([SKETCH_OFFSET, 0, CAP_OFFSET], [[1, 0, 0], [0, 1, 0], [0, 0, 1]])
     self.camera_transform = tfx.transform(self.camera_transform).as_transform() * offset
     transform = tfx.inverse_tf(self.camera_transform)
     pt = transform.translation
     rot = transform.rotation
     msg = TransformStamped()
     msg.header.stamp = rospy.Time.now()
     msg.transform.rotation.x = rot.x
     msg.transform.rotation.y = rot.y
     msg.transform.rotation.z = rot.z
     msg.transform.rotation.w = rot.w
     msg.child_frame_id = "left_BC"   
     msg.transform.translation.x = pt.x
     msg.transform.translation.y = pt.y
     msg.transform.translation.z = pt.z
     msg.header.frame_id = "registration_brick_right"
     msg.header.stamp = rospy.Time.now()
     print('boo')
     while not rospy.is_shutdown():
         msg.header.stamp = rospy.Time.now()
         self.pub.publish([msg])
         rospy.sleep(0.1)

# 需要导入模块: from geometry_msgs.msg import TransformStamped [as 别名]
# 或者: from geometry_msgs.msg.TransformStamped import child_frame_id [as 别名]
def pack_pose(time, child, parent, matrix=None, trans=None, quat=None):

    if matrix is not None and (trans is None and quat is None):
        trans = tfs.translation_from_matrix(matrix)
        quat = tfs.quaternion_from_matrix(matrix)
    elif matrix is None and trans is not None and quat is not None:
        matrix = None  # nothing
    else:
        print 'invalid use'

    t = TransformStamped()
    t.header.frame_id = parent
    t.header.stamp = time
    t.child_frame_id = child
    t.transform.translation.x = trans[0]
    t.transform.translation.y = trans[1]
    t.transform.translation.z = trans[2]

    quat = numpy.array(quat)
    quat = quat / numpy.linalg.norm(quat, ord=2)
    t.transform.rotation.x = quat[0]
    t.transform.rotation.y = quat[1]
    t.transform.rotation.z = quat[2]
    t.transform.rotation.w = quat[3]

    return t