If you are able to the visualization in Rviz, you are done with the interfacing. The next step is to convert IMU orientation into command velocity as ROS twist messages. For this, we have to create a ROS package and a Python script. You can get this package from chapter_5_codes/gesture_teleop; look for a script called gesture_teleop.py from the gesture_teleop/scripts folder.

If you want to create the package from scratch, here is the command:

    $ catkin_create_pkg gesture_teleop rospy roscpp std_msgs sensor_msgs geometry_msgs

Now let's look at the explanation of gesture_teleop.py, which is performing the conversion from IMU orientation values to twist commands.

In this code, what we basically do is subscribe to the /imu_data topic and extract only the yaw and pitch values. When these values change in the positive or negative direction, a step value is added or subtracted from the linear and angular velocity variable. The resultant velocity is sent using ROS twist messages with a topic name defined by the user.

We need the following modules to perform this conversion. As you know, rospy is a mandatory header for a ROS Python node.

    import rospy 
    from geometry_msgs.msg import Twist 
    from geometry_msgs.msg import Vector3 

After importing the modules, you will see the initialization of some parameters; these parameters are keeping in a file called gesture_teleop/config/teleop_config.yaml. If the node can't retrieve parameters from the file, it will load the default values mentioned in the code.

Here is the subscriber for the /imu_data topic, in which the topic name is defined as a variable. The callback function is called Get_RPY and the message type is Vector3.

    rospy.Subscriber(imu_topic,Vector3,Get_RPY) 

The Get_RPY simply computes the delta value of the yaw and pitch values of the IMU data and sends those values along with the yaw value to another function called Send_Twist():

    def Get_RPY(rpy_data): 
        global prev_yaw 
        global prev_pitch 
        global dy,dp 
        dy = rpy_data.x - prev_yaw 
        dp = rpy_data.y - prev_pitch 
        Send_Twist(dy,dp,rpy_data.y) 
        prev_yaw = rpy_data.x 
        prev_pitch = rpy_data.y 

The following code is the definition of Send_Twist(). This is the function generating the twist message from the orientation values. Here, the linear velocity variable is control_speed and angular velocity variable is control_turn. When the pitch value is very less and change in yaw value is zero, the position is called home position. In home position, the IMU will be horizontal to the ground. In this position, the robot should stop its movement. We are assigning both the speeds as zero at this condition. In other cases, the control speed and control turn is computed up to the maximum or minimum speed limits. If the speed is beyond the limit, it will switch to the limiting speed itself. The computed velocities are assigned to a twist message header and published to the ROS environment.

     def Send_Twist(dy,dp,pitch): 
        global pub 
        global control_speed 
        global control_turn 
        dy = int(dy) 
        dp = int(dp) 
        check_pitch = int(pitch) 
        if (check_pitch < 2 and check_pitch > -2 and dy == 0): 
          control_speed = 0 
          control_turn = 0 
        else: 
          control_speed = round(control_speed + (step_size * dp),2) 
          control_turn = round(control_turn + ( step_size * dy),2) 
          if (control_speed > high_speed): 
            control_speed = high_speed 
            elif (control_turn > high_turn): 
            control_turn = high_turn 
          if (control_speed < low_speed): 
             control_speed = low_speed 
            elif (control_turn < low_turn): 
             control_turn = low_turn 
            twist = Twist() 
            twist.linear.x = control_speed; twist.linear.y = 0; 
            twist.linear.z = 0 
            twist.angular.x = 0; twist.angular.y = 0; twist.angular.z          
    = control_turn 
            pub.publish(twist) 

That is all about the converter node, which converts orientation data to twist commands. Next is the configuration file of the gesture_teleop.py node. This node stores the essential parameters of the converter node. The file is named teleop_config.yaml and placed in the gesture_teleop/config/ folder. The file consists of the IMU data topic, limits of linear and angular velocity, and step size.

    imu_topic: "/imu_data" 
    low_speed: -4 
    high_speed: 4 
    low_turn: -2 
    high_turn: 2 
    step_size: 0.02