Robotics StackExchange | Archived questions

How to publish odometry from 3 wheeled omnidirectional robot

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/mobilerobotodom/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 localcostmap/obstacles and localcostmap/inflatedobstacles topics don't show up in navmsgs/GridCells https://github.com/Vahzahhun/Robot/blob/master/navigation/localcostmapobstacles.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/plannerplanerror.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 initialized with param meter_scoring not set. Set it to true to make your settins robust against changes of costmap resolution.

[ WARN] [1560755725.460280445]: Control loop missed its desired rate of 20.0000Hz... the loop actually took 0.0698 seconds
[ 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.
[ WARN] [1560755737.651517047]: Costmap2DROS transform timeout. Current time: 1560755737.6514, global_pose stamp: 1560755737.3415, tolerance: 0.3000
[ WARN] [1560755737.651762338]: Could not get robot pose, cancelling reconfiguration
[ WARN] [1560755737.690421012]: Unable to get starting pose of robot, unable to create global plan
[ WARN] [1560755742.140895832]: Clearing costmap to unstuck robot (3.000000m).
[ WARN] [1560755747.190440729]: Rotate recovery behavior started.
[ERROR] [1560755747.490729175]: Rotate recovery can't rotate in place because there is a potential collision. Cost: -1.00
[ WARN] [1560755752.541120780]: Clearing costmap to unstuck robot (1.840000m).
[ERROR] [1560755752.541211849]: Cannot clear map because pose cannot be retrieved
[ERROR] [1560755762.793111077]: Aborting because a valid plan could not be found. Even after executing all recovery behaviors

//rviz

[ WARN] [1560754541.374491506]: MessageFilter [target=map ]: Discarding message from [/move_base] due to empty frame_id.  This message will only print once.
[ WARN] [1560754541.374699624]: Invalid argument passed to canTransform argument source_frame in tf2 frame_ids cannot be empty

Asked by Vahzahhun on 2019-06-07 21:33:28 UTC

Comments

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.

Asked by authida on 2020-05-04 03:26:32 UTC

Answers

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.

Asked by dasanche on 2019-06-14 16:54:38 UTC

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?

Asked by Vahzahhun on 2019-06-14 17:07:50 UTC

Normally you can edit the original message to update.

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

Asked by dasanche on 2019-06-14 17:13:54 UTC

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.jpg https://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?

Asked by Vahzahhun on 2019-06-16 23:41:24 UTC

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?

Asked by dasanche on 2019-06-17 07:43:03 UTC