Ask Your Question
0

How to publish odometry from 3 wheeled omnidirectional robot

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

Vahzahhun gravatar image

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

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 ...
(more)
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

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 imagedasanche ( 2019-06-17 07:43:03 -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: 235 times

Last updated: Jun 17