Erratic movements with move_base, hector_mapping, mecanum wheels robot (bag file, source code)

edit

I have a 4 wheels mecanum robot with a LMS111 laser and beacons that gives me the x,y position of the robot when it moves inside a 4x5 meters room. I'm using Melodic with move_base and hector_mapping since I would like to make the robot move towards a (x, y) goal inside a defined map.

I'm successfully able to build a map by using hector_mapping and my laser (this is a screen recording), the problem is that when I try to set a goal in RVIZ, then the robot starts to move but it doesn't move accordingly with the point, but starts to move erratically with no sense. This is a BAG file that contains all the topics.

This is the map:

The beacon mounted on the robot gives me x,y,z coordinates and the quaternion of its orientation in the space and the fixed beacons are placed at the four corners of the room. Could the problem be related to the planner which is not able to correctly computer the cmd_vel commands for the mecanum wheel robot? I can't understand if the problem is related to the odom topic related to the beacon which publishes the tf from odom->base_link because may be I did a mistake when I calculated the yaw angle or the linear velocity:

  ....
void hedgePosAngCallback(const beginner_tutorials::hedge_pos_ang& hedge_pos_msg)
{
  ROS_INFO("Hedgehog data: Address= %d, timestamp= %d, X=%.3f  Y= %.3f  Z=%.3f  Angle: %.1f  flags=%d",     
                (int) hedge_pos_msg.address, 
                (int) hedge_pos_msg.timestamp_ms, 
                (float) hedge_pos_msg.x_m, (float) hedge_pos_msg.y_m, (float) hedge_pos_msg.z_m,
                (float) hedge_pos_msg.angle,  
                (int) hedge_pos_msg.flags);

 x = hedge_pos_msg.x_m;
 y = hedge_pos_msg.y_m;
 z = hedge_pos_msg.z_m;

}

void IMUFusionCallback(const beginner_tutorials::hedge_imu_fusion& hedge_imu_fusion_msg)
{
    ROS_INFO("IMU fusion: Timestamp: %08d, X=%.3f  Y= %.3f  Z=%.3f  q=%.3f,%.3f,%.3f,%.3f v=%.3f,%.3f,%.3f  a=%.3f,%.3f,%.3f",  
                (int) hedge_imu_fusion_msg.timestamp_ms,
                (float) hedge_imu_fusion_msg.x_m, (float) hedge_imu_fusion_msg.y_m, (float) hedge_imu_fusion_msg.z_m,
                (float) hedge_imu_fusion_msg.qw, (float) hedge_imu_fusion_msg.qx, (float) hedge_imu_fusion_msg.qy, (float) hedge_imu_fusion_msg.qz,
                (float) hedge_imu_fusion_msg.vx, (float) hedge_imu_fusion_msg.vy, (float) hedge_imu_fusion_msg.vz,
                (float) hedge_imu_fusion_msg.ax, (float) hedge_imu_fusion_msg.ay, (float) hedge_imu_fusion_msg.az);

   orientation_qx= hedge_imu_fusion_msg.qx;
   orientation_qy= hedge_imu_fusion_msg.qy;
   orientation_qz= hedge_imu_fusion_msg.qz;
   orientation_qw= hedge_imu_fusion_msg.qw;
}
....

     while(n.ok()){    
        ros::spinOnce();               // check for incoming messages
        current_time = ros::Time::now();

        //compute odometry in a typical way given the velocities of the robot
        double dt = (current_time - last_time).toSec();


        geometry_msgs::Quaternion odom_quat;
        odom_quat.x = orientation_qx;
        odom_quat.y = orientation_qy;
        odom_quat.z = orientation_qz;
        odom_quat.w = orientation_qw;

       // double roll, pitch, yaw;
       // tf::Matrix3x3(odom_quat).getRPY(roll, pitch, yaw);
        double siny_cosp = 2 * (orientation_qw * orientation_qz + orientation_qx * orientation_qy);
        double cosy_cosp = 1 - 2 * (orientation_qy * orientation_qy + orientation_qz * orientation_qz);
        double yaw = std::atan2(siny_cosp, cosy_cosp);

        //first, we'll publish the transform over tf
        geometry_msgs::TransformStamped odom_trans;
        odom_trans.header.stamp = current_time;
        odom_trans.header.frame_id = "odom";
        odom_trans.child_frame_id = "base_link";

        odom_trans.transform.translation.x = x;
        odom_trans.transform.translation.y = y;
        odom_trans.transform.translation.z = z;
        odom_trans.transform.rotation = odom_quat;

        vx = (x-prev_x)/dt;
        prev_x = x;

        vy = (y-prev_y)/dt;
        prev_y = y;

        //send the transform
        odom_broadcaster.sendTransform(odom_trans);

        //next, we'll publish the odometry ...