No rotation seen in going from one place to another program!

edit

Hello everyone, I am not very good at C++, however I am learning so much with ROS that I want to improve it (I don't know if I can ask this here but I do it cause I don't know what else to try). Right now I am making a robot to go from an original to an aim position. I am getting the estimate coordinates and yaw in every iteration as you can see in my program

#define pi 3.14159265359

//Gets the current estimated position from the robot

void Guess_what::poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{

    y_estimated = msg->pose.pose.position.y;    

    x_estimated = msg->pose.pose.position.x;

    yaw_estimated = tf::getYaw(msg->pose.pose.orientation);
}

// robot shall stop, in case anything is closer than ... 



void Guess_what::calculateCommand() {

    aim_x = 33.5;
    aim_y = 14.5;

    dist = sqrt(pow(aim_y - y_estimated,2) + pow(aim_x - x_estimated,2));

    double delta_angle = asin((aim_x - x_estimated)/dist);      

    double aim_angle = yaw_estimated - delta_angle;

    /*ROS_INFO("estimate_yaw %f, delta_angle %f, aim_angle %f", yaw_estimated, delta_angle, aim_angle);*/

    if(yaw_estimated > (aim_angle + 2.5/pi) || yaw_estimated < (aim_angle - 2.5/pi))
    {
        if(yaw_estimated > aim_angle)
        {
            m_roombaCommand.linear.x  = 0.2 ;
            m_roombaCommand.angular.z = 0.0 ;
        }
        else
        {
            m_roombaCommand.linear.x  = -0.2 ;
            m_roombaCommand.angular.z = 0.0 ;
        }
    }
    else
    {
        m_roombaCommand.linear.x  = 0.2 ;
        m_roombaCommand.angular.z = 0.0 ;
        /*ROS_INFO("estimate_yaw %f, delta_angle %f, aim_angle %f", yaw_estimated, delta_angle, aim_angle);*/
    }

    if(dist <= 0.15)
    {
        m_roombaCommand.linear.x  = 0.0 ;
        m_roombaCommand.angular.z = 0.0 ;
    }       
} 

//
void Guess_what::mainLoop() {
    // determines the number of loops per second
    ros::Rate loop_rate(20); // it was 20

    while (m_nodeHandle.ok())
    {       
        calculateCommand();
        emergencyStop();

        ROS_INFO("POS estimated_x = %f, aim_X= %f, estimated_y =%f, aim_Y= %f, dist = %f, estimated_Yaw %f",x_estimated,aim_x,y_estimated,aim_y,dist, yaw_estimated);

        // send the command to the roomrider for execution
        m_commandPublisher.publish(m_roombaCommand);

        ros::spinOnce();
        loop_rate.sleep();
    }



int main(int argc, char** argv) {

    ros::init(argc, argv, "Guess_what");
    ros::Time::init();
    ros::Duration(5).sleep(); //*/
    Guess_what dude  ;
    dude.mainLoop();

    return 1;
}

However when I run it, it turns out that the robot just makes a rotation in the beginning of the program and later it just keeps going forward.

The initial position is at (33.0, 0.0). I also used the parameter /use_sim_time (in case it can give more information)

I tried several combinations, however either the robot stays turning around with linnear speed 0 or it just ignores the place to go. I was going to copy the whole file, however I guess that these are the most important parts.

Could you please tell me if there is something missing in my function calculateCommand (I think it's fine, it should not be that complicated to go from one place to another one) or if there is a problem with the arcsin function that I don't know?

Thank you.