Ask Your Question

How to publish odometry from 3 wheeled omnidirectional robot

asked 2019-06-07 21:33:28 -0600

Vahzahhun gravatar image

updated 2019-06-17 03:16:00 -0600


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;

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;


        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


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


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:

This is the converted odometry msgs:

This is the rqt graph:

This is the tf frames:

The launch files can be found here:

The local_costmap/obstacles and local_costmap/inflated_obstacles topics don't show up in nav_msgs/GridCells and then I use OccupancyGrid instead of GridCells

After I send the 2D Nav Goal this is the warnings & errors I get:

Please help me troubleshoot this problem


[ 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.


[ WARN] [1560754114.709451440]: Trajectory Rollout planner ...
edit retag flag offensive close merge delete


Excuse me, can you explain about the odometry equation for me? I am a student I am working on a project about this. But I still can't find the answer. thank you.

authida gravatar image authida  ( 2020-05-04 03:26:32 -0600 )edit

1 Answer

Sort by » oldest newest most voted

answered 2019-06-14 16:54:38 -0600

dasanche gravatar image

As you say, in that code you have all the variables you need. Only one strange thing, you have a transformation to cm, usually ROS works with meters (m).

Then, you only need to publish that information in 2 ways: a tf transform and a nav_msgs/Odometry message.

Just follow the tutorial you have linked, fro line 40 to 75.

edit flag offensive delete link more


Thank you for the answer, actually 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. How can I update my question, should I edit or add a comment?

Vahzahhun gravatar image Vahzahhun  ( 2019-06-14 17:07:50 -0600 )edit

Normally you can edit the original message to update.

Are you also publishing the tf transform? Do you have all your data in meters?

dasanche gravatar image dasanche  ( 2019-06-14 17:13:54 -0600 )edit

Thank you, I'm sorry for the late reply my rviz is error (segmentation fault core dumped) after my friend installed something on the pc

Yes the robot is publishing the tf transform

You mean for example the x position will increment by one after the robot move forward for 1 meter?

Vahzahhun gravatar image Vahzahhun  ( 2019-06-16 23:41:24 -0600 )edit

Yes, by default ROS works in meters. I can see you have a transformation to cm (odometry_to_cm) that makes me doubt.

Besides, when you launch you have 2 warnings that are suspicious:

[ WARN] [1560755726.440543596]: The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?

[ WARN] [1560755726.738540579]: The origin for the sensor at (1.64, -3.30) is out of map bounds. So, the costmap cannot raytrace for it

Are you sure your starting location is fine?

dasanche gravatar image dasanche  ( 2019-06-17 07:43:03 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower


Asked: 2019-06-07 21:33:28 -0600

Seen: 449 times

Last updated: Jun 17 '19