1import numpy as np
 2import rospy
 3import math
 4from sensor_msgs.msg import Imu
 5from tf.transformations import euler_from_quaternion
 6
 7data = []
 8
 9def callback(msg):
10    '''
11    The callback function is required by the Subscriber to the ROSTopic. This callback function reads the orientation list from the IMU
12    It then converts the quaternion angles to euler ones. This gives us the roll, pitch and yaw of the body.
13    We convert these radian values to degrees to get the orientation of the body.
14
15    Aside from these values the IMU also gives us the angular velocity of the body.\n
16    All of these values are stored in the global `data` variable of the block.
17    '''
18    global data
19    # Get the orientation list from the IMU sensor
20    orientation_list = [msg.orientation.x,msg.orientation.y,msg.orientation.z,msg.orientation.w]
21    # Convert orientation obtainted into values of roll, pitch and yaw
22    (roll,pitch,yaw) = euler_from_quaternion(orientation_list)
23    # Convert val;ues in Radians to Degrees
24    roll = math.degrees(roll)
25    pitch = math.degrees(pitch)
26    yaw = math.degrees(yaw)
27    # Obtain Angular Velocities from IMU sensor
28    angVel_x = msg.angular_velocity.x
29    angVel_y = msg.angular_velocity.y
30    angVel_z = msg.angular_velocity.z
31    # Store all this data in 'data' variable for use in main function
32    data = [roll, pitch, yaw, angVel_x, angVel_y, angVel_z]
33
34def main(inputs, outputs, parameters, synchronise):
35    '''
36    ## Reads IMU sensor data
37    This is a specialized block used to read IMU sensor data.
38
39    It reads the ROSTopic name from the `ROSTopic` parameter. Default is `mavros/imu/data`.\n
40    This data is sent to the callback function which converts the orientation list obtained into roll, pitch and yaw for the
41    robot that the IMU is present on. Alongwith orientation, it also gives the angular velocity of the robot.
42    This data is shared in the form of an array using the `share_array()` function.
43    
44    **Inputs**: None
45
46    **Outputs**: Array [Roll, Pitch , Yaw, Angular Velocity in X, Angular Velocity in Y, Angular Velocity in Z]
47
48    **Parameters**: ROSTopic
49    '''
50    global data 
51    auto_enable = False
52    try:
53        enable = inputs.read_number('Enable')
54    except Exception:
55        auto_enable = True
56
57    rospy.init_node('imu_vc')
58    rostopic_name = parameters.read_string("ROSTopic")
59    rospy.Subscriber(rostopic_name, Imu, callback)
60
61    while ((auto_enable or inputs.read_number('Enable')) and not rospy.is_shutdown()):
62        if not data:
63            continue
64
65        outputs.share_array("Out", data)
66        synchronise()