Ask Your Question
0

why the robot position coordinate change a lot when doing rotation?

asked 2019-05-09 07:08:37 -0500

macleonsh gravatar image

updated 2019-05-09 20:56:37 -0500

Dear all, Sorry forget my silly question, here I follow some tutorial code to write a test code to control TB3 burger to move a triangle shape.

Below are part of code that to obtain the current position and then using "close-loop" way to move the burger to specified distance (say 0.3m) and then rotate specified angle (say 120deg).

But I found issue so print-out the detail data to check--I find the problem is as soon as the burger starting rotate. the position.x obtained from curr_pos_.x = msg->pose.pose.position.x will jumped a lot (from ~0.3 to ~0.5!!).. so I wonder if such way to get the "current" position of robot are correct or not?? I search around and find suggestion that odometry is not precise, however it should be have such big error? if it indeed naturally is, should I consider to use some other way like IMU?

Thanks a lot for your kindly suggestion..

BR LM

void velControl::odomCallback(const nav_msgs::Odometry::ConstPtr& msg)  
{  
    geometry_msgs::Quaternion orientation = msg->pose.pose.orientation;  //obtain Quaternion info.
    tf::Matrix3x3 mat(tf::Quaternion(orientation.x, orientation.y, orientation.z, orientation.w));  

    double yaw, pitch, roll;  
    mat.getEulerYPR (yaw, pitch, roll);
    //Get the matrix represented as euler angles around YXZ, roundtrip with setEulerYPR. 
    //Yaw around Z axis; Pitch around Y axis; Roll around X axis
    curr_pos_.x = msg->pose.pose.position.x;  
    curr_pos_.y = msg->pose.pose.position.y;  
    curr_pos_.theta = yaw;  

       velocityControl();

    if(Is_Fininsh_)    //Is_Fininsh_ =true= robot complete one round line+rotaion;
       {   start_pos_.x = curr_pos_.x;  
        start_pos_.y = curr_pos_.y;  
        start_pos_.theta = curr_pos_.theta; 
       }

}

Below are part of the move forward and rotate. I did not paste all as too much code.. concept are similar.

   if ( line_travel < triagle_line1 )  
        {  
            #ifdef VERBOSE
             cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
              #endif
            controlVel_.angular.z = 0.0;
            controlVel_.linear.x  = vel_line_;
            controlVel_.linear.y  = 0;
            controlVel_.linear.z  = 0;  
            Is_Fininsh_ = false;  
        }  
        else  
        {   
            controlVel_.angular.z = 0.0;
            controlVel_.linear.x  = 0.0;
            controlVel_.linear.y  = 0;  
            controlVel_.linear.z  = 0;  
            state_ = 1;  
            Is_Fininsh_ = true;  

            cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
             }  
        break;  

    case 1: 
        ang_travel = abs(angleWrap(curr_pos_.theta - start_pos_.theta));
        if ( ang_travel < TRIANGLE_Ang1 )  
        {   
            #ifdef VERBOSE
            cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
            #endif
            controlVel_.angular.z = vel_angle_;  
            controlVel_.linear.x  = 0;  
            controlVel_.linear.y  = 0;  
            controlVel_.linear.z  = 0;  
            Is_Fininsh_ = false;  
        }  
        else  
        {  
            controlVel_.angular.z = 0.0;  
            controlVel_.linear.x  = 0.0;  
            controlVel_.linear.y  = 0;  
            controlVel_.linear.z  = 0;  
            state_ = 2;  
            Is_Fininsh_ = true;  
            cout<<"[State#1]: Angle1 rotation completed"<<endl;
            cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
            cout<<endl;

        }  
        break;
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2019-05-10 01:35:56 -0500

billy gravatar image

updated 2019-05-10 01:39:32 -0500

I see one possibility. Note you do not tell the robot to go 0.3m. You tell it go some velocity and then tell it to stop at some point in time after it has passed 0.3m. It's likely the code works as you expect. It passes 0.3m and then comes to a stop some distance later.

The question is how long after it has passed 0.3m does the condition

if ( line_travel < triagle_line1 )

get checked? It if gets checked when robot is at 0.4m and takes a 0.1m to stop, then it's working great.

If you provide the data that you print to the screen, but include the commanded velocity at each 'cout' statement, we'd have more to go on. You should change code so you can watch the robot come to stop prior to turning

More like this:

   if ( line_travel < triagle_line1 )  
        { 
            counter = 0; // init the counter
            controlVel_.angular.z = 0.0;
            controlVel_.linear.x  = vel_line_;
            controlVel_.linear.y  = 0;
            controlVel_.linear.z  = 0;  
            Is_Fininsh_ = false;  
            #ifdef VERBOSE
             cout<<"In IF statement: "<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<"||  ||"<< line_travel <<"||  ||"<< controlVel_.linear.x <<endl;
            #endif
        }  
        else  
        {   
            controlVel_.angular.z = 0.0;
            controlVel_.linear.x  = 0.0;
            controlVel_.linear.y  = 0;  
            controlVel_.linear.z  = 0;  
            state_ = 1;
            cout<<"In ELSE statement: "<< counter << "||"<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<"||  ||"<< line_travel <<"||  ||" << controlVel_.linear.x <<endl;


           // Go through this else statement 20 times 
           // so you can watch the robot come to stop before turning
           if (counter++ > 20)  
           {  
            Is_Fininsh_ = true;  
           }
        }
edit flag offensive delete link more
0

answered 2019-05-10 21:48:03 -0500

macleonsh gravatar image

updated 2019-05-10 21:49:35 -0500

My current controlling code:

controlVel_.angular.z = 0.0; line_travel=sqrt(pow((curr_pos_.x - start_pos_.x),2)+pow((curr_pos_.y - start_pos_.y),2)); line_error=LineOneTarget - line_travel;

if (line_error > 0.15)                   //still far from target
{   controlVel_.linear.x  = vel_line_;    //general speed
    vel_pub_.publish(controlVel_);      //publish control data to robot
    #ifdef VERBOSE
    cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
    #endif
    Is_Fininsh_ = false;
 }
else if (line_error < 0.15 && line_error>0)   //near target
{
    controlVel_.linear.x  = vel_line_/2;        //slow-down speed a bit, shall have better algorithm TODO.
    vel_pub_.publish(controlVel_);      //publish control data to robot
    cout<< "Near target, slow down line speed..." <<endl;
    Is_Fininsh_ = false;
}
else if ( line_error <0 )     //already cross the target line
{
    controlVel_.linear.x  = 0;
    vel_pub_.publish(controlVel_);      //publish control data to robot
    Is_Fininsh_ = true;
    state_ = 1;
    cout<<"[State#0]: Target arrived, Line Stop!"<<endl;
    cout<<"[State#0]: Planed travel distance: "<< TRIANGLE_Line1 << "m" <<endl;
    cout<<"[State#0]: Actual travel distance: "<< line_travel    << "m" <<endl;
    cout<<"[State#0]: Error                 : "<< line_error     << "m" <<endl;
    //cout<< curr_pos_.x <<"||"<< start_pos_.x<<"||  ||"<<curr_pos_.y<<"||"<<start_pos_.y<<"||  ||"<< line_travel <<endl;
}
edit flag offensive delete link more

Comments

Good to hear you figured it out. About the comment in your code to have better algo, you could control distance with PID to get better response, but really if you have it working to spec, then it's working.

The moderators will be along shortly to tell you to not use an answer to your own question to make comments. OK to make a comment or to add additional info to your original question.

billy gravatar imagebilly ( 2019-05-11 00:26:00 -0500 )edit

Is this an answer or an update to your question?

jayess gravatar imagejayess ( 2019-05-11 00:43:40 -0500 )edit
0

answered 2019-05-10 21:44:28 -0500

macleonsh gravatar image

updated 2019-05-10 21:45:00 -0500

Thanks so much, I more or less find the reason, it looks I shall publish the linear/angular volecity control message immediately after set these value so the rbot can act in time, otherwise there will be big error exist.

But your suggestion is also vey valueable--which is just what I am doingfor a little fine tuning!! i.e. how to make the controlling precisely when robot move to near the "target"--should be predict and slow down the speed rather than brake...

I will put my current code in next answer.

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2019-05-09 07:08:37 -0500

Seen: 88 times

Last updated: May 10