Kinematics node for iRobot Create 2: https://github.com/RoboticaUtnFrba/create_autonomy

kinematics.launch

<launch>
  <!-- Spawn Gazebo -->
  <include file="$(find ca_gazebo)/launch/create_empty_world.launch"/>
  <!-- Change real-time factor of Gazebo to 2x-->
  <node pkg="ca_gazebo" type="set_properties" name="gazebo_physics_properties" output="screen">
    <param name="real_time_factor" value="2"/>
  </node>
  <!-- Run the node to move the robot -->
  <node pkg="ca_tools" type="kinematics.py" name="kinematics" output="screen"/>
</launch>

kinematics.py

#! /usr/bin/env python

import math
import rospy
import PyKDL

from geometry_msgs.msg import Twist, Pose, Pose2D, Quaternion
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion, quaternion_from_euler

CMD_VEL_TOPIC = "/create1/cmd_vel"
GT_TOPIC = "/create1/gts"

class Kinematics(object):
    """
    Class Kinematics to move the robot.
    """
    def __init__(self):
        """
        Constructor
        """
        # Declare variables
        self.pose = Pose2D()
        self.last_pose = None
        # Initialize ROS node and its connections
        rospy.init_node("kinematics_node")
        self.vel_pub = rospy.Publisher(CMD_VEL_TOPIC, Twist, queue_size=1)
        self.gt_sub = rospy.Subscriber(GT_TOPIC, Odometry, self._gt_cb)
        # This sleep avoids issues with the first case
        rospy.sleep(rospy.Duration(secs=2))
        msg = rospy.wait_for_message(GT_TOPIC, Odometry, timeout=None)
        # Get initial robot pose
        x, y, _, yaw = self._odom_to_pose2d(msg)
        self.last_pose = Pose2D(x, y, yaw)

    def move(self, lin=0., ang=0., duration_sec=0.):
        """
        Move the unicycle robot during the specified time.

        Args:
            lin: linear velocity.
            ang: angular velocity.
            duration_sec: time at which the velocity is applied to the robot.
        """
        # Generate Twist message
        twist_msg = self._twist(lin, ang)
        # Record initial pose
        self.pose = Pose2D()
        # Move robot at the specified velocity during 'duration_sec'
        now = rospy.Time.now().secs
        while (rospy.Time.now().secs - now) <= duration_sec:
            self.vel_pub.publish(twist_msg)
        # Stop robot
        self.vel_pub.publish(self._twist(0, 0))
        rospy.sleep(rospy.Duration(nsecs=2000))
        # Get final measurements
        rospy.loginfo("[{0:.3f}, {1:.3f}, {2:.3f}]".format(
            self.pose.x,
            self.pose.y,
            math.degrees(self.pose.theta)
        ))
        rospy.sleep(rospy.Duration(secs=5))

    def _gt_cb(self, msg):
        """
        Ground Truth callback

        Args:
            msg: nav_msgs/Odometry message.
                std_msgs/Header header
                string child_frame_id
                geometry_msgs/PoseWithCovariance pose
                geometry_msgs/TwistWithCovariance twist
        """
        # Wait until we have the first reading
        if self.last_pose is None: return
        # Get current pose based on Odometry message
        x, y, q, yaw = self._odom_to_pose2d(msg)
        # Get robot displacements
        self.pose.x += (x - self.last_pose.x)
        self.pose.y += (y - self.last_pose.y)
        # Calculate rotation displacement
        [qx, qy, qz, qw] = quaternion_from_euler(0, 0, self.last_pose.theta)
        angle_diff = self._get_quat_diff(q, Quaternion(qx, qy, qz, qw))
        self.pose.theta = self._wrap_angle(self.pose.theta + angle_diff)
        # Update last pose
        self.last_pose = Pose2D(x, y, yaw)

    def _odom_to_pose2d(self, msg):
        """
        Convert nav_msgs/Odometry to geometry_msgs/Pose2D

        Args:
            msg: navs_msgs/Odometry message.
                std_msgs/Header header
                string child_frame_id
                geometry_msgs/PoseWithCovariance pose
                geometry_msgs/TwistWithCovariance twist

        Returns:
            geometry_msgs/Pose2D message.
                float64 x
                float64 y
                float64 theta
        """
        x = msg.pose.pose.position.x
        y = msg.pose.pose.position.y
        q = msg.pose.pose.orientation
        yaw = self._get_yaw_from_quat(q)
        return x, y, q, yaw

    def _get_quat_diff(self, q1, q0):
        """
        Get the difference between two quaternions.

        Args:
            q1: final angle.
            q0: initial angle.

        Returns:
            q1-q0 expressed as quaternion.
        """
        angle1 = PyKDL.Rotation.Quaternion(q1.x, q1.y, q1.z, q1.w)
        angle0 = PyKDL.Rotation.Quaternion(q0.x, q0.y, q0.z, q0.w)
        rot = angle1 * angle0.Inverse()
        # get the RPY (fixed axis) from the rotation
        [_, _, yaw] = rot.GetRPY()
        return yaw

    def _get_yaw_from_quat(self, q):
        """
        Get yaw angle from quaternion.

        Args:
            q: quaternion angle.

        Returns:
            Converted yaw angle.
        """
        [_, _, yaw] = euler_from_quaternion([q.x, q.y, q.z, q.w])
        return yaw

    def _twist(self, lin, ang):
        """
        Create a geometry_msgs/Twist message from Unicycle velocities.

        Args:
            lin: linear velocity.
            ang: angular velocity.

        Returns:
            geometry_msgs/Twist message to command the robot.
                geometry_msgs/Vector3 linear
                geometry_msgs/Vector3 angular
        """
        msg = Twist()
        msg.linear.x = lin
        msg.angular.z = ang
        return msg

    def _wrap_angle(self, angle):
        """
        Wrap angle between [-PI, PI).

        Args:
            angle: angle to wrap.

        Returns:
            Wrapped angle.
        """
        angle %= (2 * math.pi)
        if(angle > math.pi):
            angle -= (2 * math.pi)
        elif(angle <= -math.pi):
            angle += (2 * math.pi)
        return angle

def kinematics():
    kine = Kinematics()
    # Move forward
    kine.move(0.2, 0, 5)
    # Move backward
    kine.move(-0.2, 0, 5)
    # Rotate left
    # dT * dAngle = PI
    kine.move(0, 0.5236, 6)
    # Rotate right
    kine.move(0, -0.5236, 6)

if __name__ == '__main__':
    kinematics()