How to publish odometry from 3 wheeled omnidirectional robot?

Question

I'm new to ROS and robotics and I'm trying to implement the ROS navigation stack in a 3 wheeled omnidirectional robot. I already made the map using RTABMap but now I'm stuck in the 'Publishing Odometry Information over ROS' step. Currently the robot ouputs odometry data in geometry_msgs/Pose2D (http://docs.ros.org/kinetic/api/geometry_msgs/html/msg/Pose2D.html) format.

Here are some of the codes compiled in STM32:

//Calculate Odometry
void calculate_odometry(void)
{
    short int odometry0_speed = odometry0;
    odometry0 = 0;
    short int odometry1_speed = odometry1;
    odometry1 = 0;

    float buffer_x[2];
    float buffer_y[2];

    buffer_x[0] = odometry0_speed * cosf(gyro_radian + 0.785398);
    buffer_x[1] = odometry1_speed * cosf(gyro_radian + 2.356190);

    buffer_y[0] = odometry0_speed * sinf(gyro_radian + 0.785398);
    buffer_y[1] = odometry1_speed * sinf(gyro_radian + 2.356190);

    x_buffer_position += (buffer_x[0] + buffer_x[1]) * odometry_to_cm;
    y_buffer_position -= (buffer_y[0] + buffer_y[1]) * odometry_to_cm;

    x_position = x_buffer_position - x_offset_position;
    y_position = y_buffer_position - y_offset_position;
}

//RECEIVE data FROM gyro
void DMA1_Stream1_IRQHandler(void)
{
    if (DMA_GetITStatus(DMA1_Stream1, DMA_IT_TCIF1))
    {
        if (gyro_status == RESET)
        {
            gyro_status = SET;
            buzzer(22, 22);
        }

        memcpy(&gyro_buffer, gyro_receive + 3, 4);

        gyro_angle = (gyro_offset - gyro_buffer);
        gyro_radian = (gyro_offset - gyro_buffer) * 0.01745329252;

        while (gyro_angle > 180)
            gyro_angle -= 360;
        while (gyro_angle < -180)
            gyro_angle += 360;

        while (gyro_radian > 3.14159265359)
            gyro_radian -= 6.28318530718;
        while (gyro_radian < -3.14159265359)
            gyro_radian += 6.28318530718;

        USART_DMACmd(USART3, USART_DMAReq_Rx, DISABLE);
        USART_ITConfig(USART3, USART_IT_RXNE, ENABLE);

        DMA_ClearITPendingBit(DMA1_Stream1, DMA_IT_TCIF1);
    }
}

//ROS SUBSCRIBER CALLBACK
void cllbck_sub_velocity(const geometry_msgs::Twist &msg)
{
    x_velocity = msg.linear.x;
    y_velocity = msg.linear.y;
    angular_velocity = msg.angular.z;

    motor_timer = 0;
}

The gyro data is from GY-521 + Arduino.

From these code I should be able to get

x -> x_position          dx -> x_velocity
y -> y_position          dy -> y_velocity
A -> gyro_angle          ω -> angular_velocity

right?

And then how can I publish the data in nav_msgs/Odometry (http://docs.ros.org/kinetic/api/nav_msgs/html/msg/Odometry.html) format? Do I have all the data I need correctly?

Should I use the code in http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom, what need to be changed or included in the code?

Or can I do autonomous navigation by only using visual odometry from RTABMap?

Answer 1

It looks like you have most of what you need to output the Odometry message. The tutorial at http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom is probably the best practical resource.

The odometry message is a full 3D message. So you can simply fill in the other dimensions as zero (height, roll, and pitch) assuming that's accurate for your vehicle.

The other element of the Odometry message is the velocity expressed as a Twist. This is just a name for a 6dof velocity vector (x, y, z, roll, pitch, yaw). So I see again that you have the (vx, vy, v$w$) So you will again fill in the other fields as zero velocity for (z, pitch, yaw).

The last part is that both pose and velocity terms optionally have a covariance matrix. To start off you can not fill it in. For the next step level you can manually estimate the covariance and just plug the same ones in every time that you publish the message. How to estimate the covariance matrices is a whole additional question that I'd suggest that you look for tutorials on. Depending on what application you're planning to use it for the accuracy of the covariance might be important or less important.