Ask Your Question
0

How to publish odometry from 3 wheeled omnidirectional robot

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

Vahzahhun gravatar image

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?

What need to be changed or included in

http://wiki.ros.org/navigation/Tutori...

tutorial's code?

edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

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

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

Comments

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 imageVahzahhun ( 2019-06-14 17:07:50 -0500 )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 imagedasanche ( 2019-06-14 17:13:54 -0500 )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 https://github.com/Vahzahhun/Robot/blob/master/navigation/frames.jpghttps://github.com/Vahzahhun/Robot/blob/master/navigation/tf_rviz.jpg

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

Vahzahhun gravatar imageVahzahhun ( 2019-06-16 23:41:24 -0500 )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

Stats

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

Seen: 176 times

Last updated: 2 days ago