Python ROS (rospy) dealing with rotation matrix and transformations

main.py

from tf.transformations import quaternion_matrix #Return homogeneous rotation matrix from quaternion.
from tf.transformations import quaternion_from_matrix  #Return quaternion from rotation matrix.
from tf.transformations import quaternion_multiply

"""
>>> dir( tf.transformations )
['Arcball', '_AXES2TUPLE', '_EPS', '_NEXT_AXIS', '_TUPLE2AXES', '__builtins__', '__doc__', '__docformat__', '__file__', '__name__', '__package__', '_import_module', 'arcball_constrain_to_axis', 'arcball_map_to_sphere', 'arcball_nearest_axis', 'clip_matrix', 'compose_matrix', 'concatenate_matrices', 'decompose_matrix', 'division', 'euler_from_matrix', 'euler_from_quaternion', 'euler_matrix', 'identity_matrix', 'inverse_matrix', 'is_same_transform', 'math', 'numpy', 'orthogonalization_matrix', 'projection_from_matrix', 'projection_matrix', 'quaternion_about_axis', 'quaternion_conjugate', 'quaternion_from_euler', 'quaternion_from_matrix', 'quaternion_inverse', 'quaternion_matrix', 'quaternion_multiply', 'quaternion_slerp', 'random_quaternion', 'random_rotation_matrix', 'random_vector', 'reflection_from_matrix', 'reflection_matrix', 'rotation_from_matrix', 'rotation_matrix', 'scale_from_matrix', 'scale_matrix', 'shear_from_matrix', 'shear_matrix', 'superimposition_matrix', 'translation_from_matrix', 'translation_matrix', 'unit_vector', 'vector_norm', 'warnings']
"""


# eg: 
R_0 = np.eye(4,4)
R_0[ 0:3,0:3 ] = np.matmul( self.create_rotY( tilt_angle_deg / 180. * np.pi ) ,  self.create_rotZ( pan_angle_deg / 180. * np.pi  ) )
quaternion_of_R = quaternion_from_matrix( R_0 )


quaternion_of_wp = [ nav_msg.pose.orientation.x, nav_msg.pose.orientation.y, nav_msg.pose.orientation.z, nav_msg.pose.orientation.w ]

# -2- Add the above rotation matrix to cam_marker.pose.orientation
rslt_quaternion = quaternion_multiply( quaternion_of_wp, quaternion_of_R )


# set the resulting orientation to marker ( original_orientation (+) pan_of_ptz )
cam_marker.pose.orientation.x = rslt_quaternion[0]
cam_marker.pose.orientation.y = rslt_quaternion[1]
cam_marker.pose.orientation.z = rslt_quaternion[2]
cam_marker.pose.orientation.w = rslt_quaternion[3]

rotation_helpers.py

    # Helpers for rotation....
    def create_rotZ( self, angle_rads  ):
        c, s = np.cos(angle_rads), np.sin(angle_rads)
        R = np.array( ( (c, -s, 0), (s, c, 0) , (0,0,1) ) )
        return R

    def create_rotY( self, angle_rads  ):
        c, s = np.cos(angle_rads), np.sin(angle_rads)
        R = np.array( ( (c, 0, s), (0, 1, 0) , (-s, 0, c) ) )
        return R

    def create_rotX( self, angle_rads  ):
        c, s = np.cos(angle_rads), np.sin(angle_rads)
        R = np.array( ( (1, 0, 0), (0, c, -s) , (0, s, c) ) )
        return R
    # Helpers for rotation END