Ask Your Question
0

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

asked 2015-02-01 22:41:59 -0500

Diego gravatar image

updated 2015-02-02 02:31:54 -0500

gvdhoorn gravatar image

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2015-02-02 02:23:16 -0500

ahendrix gravatar image

I haven't done an in-depth analysis of all of your program logic, but the obvious bug seems to be that you never set the angular.z component of the motion command to a non-zero value, which means that you never send a turning command to the robot.

(linear.x is forward/backward velocity. angular.z is left/right)

edit flag offensive delete link more

Comments

Ahh I've got it... It was always in front of me :( . Thanks for your help.

Diego gravatar imageDiego ( 2015-02-02 16:32:32 -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

Stats

Asked: 2015-02-01 22:41:59 -0500

Seen: 37 times

Last updated: Feb 02 '15