How to publish odometry from 3 wheeled omnidirectional robot

edit

Hello,

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 confused in the 'Publishing Odometry Information over ROS' step. The robot ouputs odometry data in geometry_msgs/Pose2D 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;
}

//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;
}

//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);
    }
}

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?

How can I publish in nav_msgs/Odometry? Do I have all the data I need correctly?

UPDATE

I already converted the format to nav_msgs/Odometry but when I send the 2D Nav Goal the robot just spinning in place doing recovery behaviour and the amcl localization always failed

The code can be found here: https://github.com/Vahzahhun/Robot/blob/master/navigation/mobile_robot_odom/src/mobile_odom.cpp

This is the converted odometry msgs: https://github.com/Vahzahhun/Robot/blob/master/navigation/odom.jpg

This is the rqt graph: https://github.com/Vahzahhun/Robot/blob/master/navigation/rqt_graph.jpg

This is the tf frames: https://github.com/Vahzahhun/Robot/blob/master/navigation/frames.jpg

The launch files can be found here: https://github.com/Vahzahhun/Robot/tree/master/navigation

The local_costmap/obstacles and local_costmap/inflated_obstacles topics don't show up in nav_msgs/GridCells https://github.com/Vahzahhun/Robot/blob/master/navigation/local_costmap_obstacles.jpg and then I use OccupancyGrid instead of GridCells https://github.com/Vahzahhun/Robot/blob/master/navigation/obstaclerviz.jpg

After I send the 2D Nav Goal this is the warnings & errors I get: https://github.com/Vahzahhun/Robot/blob/master/navigation/planner_plan_error.jpg

Please help me troubleshoot this problem

//mobile_robot_configuration.launch

[ WARN] [1560753719.492768443]: [Kinect2Bridge::initCalibration] using sensor defaults for color intrinsic parameters.
[ WARN] [1560753719.492823712]: [Kinect2Bridge::initCalibration] using sensor defaults for ir intrinsic parameters.
[ WARN] [1560753719.492860307]: [Kinect2Bridge::initCalibration] using defaults for rotation and translation.
[ WARN] [1560753719.492903831]: [Kinect2Bridge::initCalibration] using defaults for depth shift.

//move_base.launch

[ WARN] [1560754114.709451440]: Trajectory Rollout planner ...