예제 #1
0
def numpy_to_pose(arr):

    shape, rest = arr.shape[:-2], arr.shape[-2:]
    assert rest == (4, 4)

    if len(shape) == 0:
        trans = transformations.translation_from_matrix(arr)
        quat = transformations.quaternion_from_matrix(arr)

        return Pose(position=Vector3(*trans), orientation=Quaternion(*quat))
    else:
        res = np.empty(shape, dtype=np.object_)
        for idx in np.ndindex(shape):
            res[idx] = Pose(
                position=Vector3(
                    *transformations.translation_from_matrix(arr[idx])),
                orientation=Quaternion(
                    *transformations.quaternion_from_matrix(arr[idx])))
예제 #2
0
def get_tform(R, T, parent_frame, child_frame):
    cam_rot = np.eye(4)
    cam_rot[:3, :3] = R
    cam_q = tx.quaternion_from_matrix(cam_rot)

    # construct the static transform
    tform = TransformStamped()
    tform.header.stamp = rospy.Time.now()
    tform.header.frame_id = parent_frame
    tform.child_frame_id = child_frame
    tform.transform.translation.x = float(T[0])
    tform.transform.translation.y = float(T[1])
    tform.transform.translation.z = float(T[2])
    tform.transform.rotation.x = cam_q[0]
    tform.transform.rotation.y = cam_q[1]
    tform.transform.rotation.z = cam_q[2]
    tform.transform.rotation.w = cam_q[3]
    return tform
예제 #3
0
def msg_from_matrix(transform_matrix, child_frame_id="child", parent_frame_id="world"):
    t = geometry_msgs.msg.TransformStamped()
    t.header.frame_id = parent_frame_id
    t.header.stamp = rospy.Time.now()
    t.child_frame_id = child_frame_id

    translation = transformations.translation_from_matrix(transform_matrix)

    t.transform.translation.x = translation[0]
    t.transform.translation.y = translation[1]
    t.transform.translation.z = translation[2]

    quaternion = transformations.quaternion_from_matrix(transform_matrix)

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

    return t
예제 #4
0
if __name__ == '__main__':
  rospy.init_node('thermal_tf_broadcaster')
  rospack = rospkg.RosPack()

  # read the TF info for cameras
  with open(osp.join(rospack.get_path(package_name),
    'calibrations/stereo.pkl'), 'r') as f:
    extrinsics = pickle.load(f)

  cam_trans = extrinsics['T']
  cam_rot = np.eye(4)
  cam_rot[0, :3] = extrinsics['R'][0, :]
  cam_rot[1, :3] = extrinsics['R'][1, :]
  cam_rot[2, :3] = extrinsics['R'][2, :]
  cam_q = tx.quaternion_from_matrix(cam_rot)
  cam_parent = 'kinect2_rgb_optical_frame'
  cam_child = 'boson_frame'

  # construct the static transform
  tform = TransformStamped()
  tform.header.stamp = rospy.Time.now()
  tform.header.frame_id = cam_parent
  tform.child_frame_id = cam_child
  tform.transform.translation.x = float(cam_trans[0])
  tform.transform.translation.y = float(cam_trans[1])
  tform.transform.translation.z = float(cam_trans[2])
  tform.transform.rotation.x = cam_q[0]
  tform.transform.rotation.y = cam_q[1]
  tform.transform.rotation.z = cam_q[2]
  tform.transform.rotation.w = cam_q[3]
def pubTransforms():
    br = tf2_ros.TransformBroadcaster()

    m_bot = tf.concatenate_matrices( tf.rotation_matrix( 1.5, [ 0, 0, 1 ] ), tf.translation_matrix( [ 0.0, -1.0, 0.0 ] ) )
    trans_bot = tf.translation_from_matrix(m_bot)
    q_bot = tf.quaternion_from_matrix(m_bot)

    t_robot = geometry_msgs.msg.TransformStamped()
    t_robot.header.stamp = rospy.Time.now()
    t_robot.header.frame_id = "base"
    t_robot.child_frame_id = "robot"
    t_robot.transform.translation.x = trans_bot[0]
    t_robot.transform.translation.y = trans_bot[1]
    t_robot.transform.translation.z = trans_bot[2]
    t_robot.transform.rotation.x = q_bot[0]
    t_robot.transform.rotation.y = q_bot[1]
    t_robot.transform.rotation.z = q_bot[2]
    t_robot.transform.rotation.w = q_bot[3]

    m_obj = tf.concatenate_matrices( tf.euler_matrix( 0.79, 0.0, 0.79 ), tf.translation_matrix( [ 0.0, 1.0, 1.0 ] ) )
    trans_obj = tf.translation_from_matrix(m_obj)
    q_object = tf.quaternion_from_matrix(m_obj)

    t_object = geometry_msgs.msg.TransformStamped()
    t_object.header.stamp = rospy.Time.now()
    t_object.header.frame_id = "base"
    t_object.child_frame_id = "object"
    t_object.transform.translation.x = trans_obj[0]
    t_object.transform.translation.y = trans_obj[1]
    t_object.transform.translation.z = trans_obj[2]
    t_object.transform.rotation.x = q_object[0]
    t_object.transform.rotation.y = q_object[1]
    t_object.transform.rotation.z = q_object[2]
    t_object.transform.rotation.w = q_object[3]
    # Calculate the vector from the camera's frame to the object's frame.
    m_cam = tf.concatenate_matrices( m_bot, tf.translation_matrix( [ 0.0, 0.1, 0.1 ] ) )
    trans_cam = tf.translation_from_matrix( m_cam )
    cam_obj_vec = []
    cam_obj_vec.append( trans_obj[0] - trans_cam[0] )
    cam_obj_vec.append( trans_obj[1] - trans_cam[1] )
    cam_obj_vec.append( trans_obj[2] - trans_cam[2] )
    # Convert the vector to unit vector
    cam_obj_vec = tf.unit_vector( cam_obj_vec )
    # Calculate angle between the two vectors
    cam_x_comp_vec = [ trans_cam[0], 0.0, 0.0 ]
    theta = angleBetween( cam_x_comp_vec, cam_obj_vec )
    # Cross product between the previous vector and the x component of the camera's frame.
    res_vec = np.cross( cam_x_comp_vec, cam_obj_vec )
    # Rotate the camera's frame around the resulting vector of the cross product.
    q = tf.quaternion_about_axis( theta, res_vec );

    t_camera = geometry_msgs.msg.TransformStamped()
    t_camera.header.stamp = rospy.Time.now()
    t_camera.header.frame_id = "base"
    t_camera.child_frame_id = "camera"
    t_camera.transform.translation.x = trans_cam[0]
    t_camera.transform.translation.y = trans_cam[1]
    t_camera.transform.translation.z = trans_cam[2]
    t_camera.transform.rotation.x = q[0]
    t_camera.transform.rotation.y = q[1]
    t_camera.transform.rotation.z = q[2]
    t_camera.transform.rotation.w = q[3]

    br.sendTransform(t_robot)
    br.sendTransform(t_camera)
    br.sendTransform(t_object)