1
$\begingroup$

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?

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service, privacy policy and cookie policy

Browse other questions tagged or ask your own question.