Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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?

What need to be changed or included in

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

tutorial's code?

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?

What need to

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 changed or included in

http://wiki.ros.org/navigation/Tutorials/RobotSetup/Odom

tutorial's code? 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 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

//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

The launch files can be found here: https://github.com/Vahzahhun/Robot/tree/master/navigation

Please help me troubleshoot this problem

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/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 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

The launch files can be found here: https://github.com/Vahzahhun/Robot/tree/master/navigation

Please help me troubleshoot this problem