ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

First note here : is it intended for your robot to be only turning left ? That could be problematic if the robot deviates too much at the left of the goal, it would be turning for almost 2*pi instead of a little bit to the right to correct its orientation.

Anyway, you should be able to achieve what you want with some little changes :

  1. I would directly set the angular speed equal to the angle difference. That would allow the robot to always correct its orientation when reaching the goal. So your angular speed at any time could be :

    speed.angular.z = angle_to_goal - theta
    

    That being said you might want to have a function calculating the shortest angle (because your calculation might return 3*pi/2 instead of -pi/2) not to be always turning too much and also to deal with the case when your robot is aligned to the goal but the calculation returns 2*pi. If it's not what you want I would still recommend to get the sign of the angular difference to allow your robot to turn left or right when needed.

  2. I would change the condition. Instead of choosing between moving forward and turning, you could have a flag turning to true the first time your robot gets aligned to the goal and then allow it to move forward :

    move_forward = False        #define this outside of the while loop
    ...
    if abs(angle_to_goal - theta) > 0.1: 
            move_forward = True
    
    speed.angular.z = angle_to_goal - theta   #the angular speed is always the angualr difference
    if move_forward is True:
            speed.linear = 0.3                #the linear speed is only set once the flag is True
    
  3. If the robot has almost reached a goal but you are not exactly aligned with it and you are still moving forward at a constant speed, you might miss the goal (eventhough I don't know which tolerance you have) but to avoid that it is possible to also set the linear speed as the angular speed, but instead of the angular difference it would be the distance. Like that your robot will slow down when getting closer to the goal, letting you the time to adjust your orientation before missing the goal.

First note here : is it intended for your robot to be only turning left ? That could be problematic if the robot deviates too much at the left of the goal, it would be turning for almost 2*pi instead of a little bit to the right to correct its orientation.

Anyway, you should be able to achieve what you want with some little changes :

  1. I would directly set the angular speed equal to the angle difference. That would allow the robot to always correct its orientation when reaching the goal. So your angular speed at any time could be :

    speed.angular.z = angle_to_goal - theta
    

    That being said you might want to have a function calculating the shortest angle (because your calculation might return 3*pi/2 instead of -pi/2) not to be always turning too much and also to deal with the case when your robot is aligned to the goal but the calculation returns 2*pi. If it's not what you want I would still recommend to get the sign of the angular difference to allow your robot to turn left or right when needed.

  2. I would change the condition. Instead of choosing between moving forward and turning, you could have a flag turning to true the first time your robot gets aligned to the goal and then allow it to move forward :

    move_forward = False        #define this outside of the while loop
    ...
    if abs(angle_to_goal - theta) > 0.1: 
            move_forward = True
    
    speed.angular.z = angle_to_goal - theta   #the angular speed is always the angualr difference
    if move_forward is True:
            speed.linear = 0.3                #the linear speed is only set once the flag is True
    
  3. If the robot has almost reached a goal but you are not exactly aligned with it and you are still moving forward at a constant speed, you might miss the goal (eventhough I don't know which tolerance you have) but to avoid that it is possible to also set the linear speed as the angular speed, but instead of the angular difference it would be the distance. Like that your robot will slow down when getting closer to the goal, letting you the time to adjust your orientation before missing the goal.


EDIT :

So if I understand the values right, if the coordinates are on the robots right side, I get a value between 0.0 and -pi returned and if its on the left its between 0.0 and pi? How do I effectively get which angle is smaller?

If you get the correct angle then yes but you won't get it simply with angle_to_goal - theta because angle_to_goal and theta will return a value between [-pi; pi] : If you have x > 0 a little angle, if you get angle_to_goal = pi - x and theta = -pi + x then angle_to_goal - theta = 2pi + 2x instead of 2x. So you have to modify the result of this angular difference, here's an exemple of a function (in C++) doing that :

float shortestAngleDifference(float th1, float th2)
{
    float anglediff = fmod( (th1 - th2) , 2*M_PI);

    if( anglediff < 0.0 )
    {
        if( fabs(anglediff) > (2*M_PI + anglediff) )
        {
            anglediff = 2*M_PI + anglediff;
        }
    }
    else
    {
        if( anglediff > fabs(anglediff - 2*M_PI) )
        {
            anglediff = anglediff - 2*M_PI;
        }
    }
    return anglediff;
}

Where angle_to_goal = th1 and theta = th2 to direclty have anglediff > 0 if the robot has to turn to the left and anglediff < 0 to turn to the right.

Note : In your solution you have multiple conditions to keep your speed between [0.3;0.7] but you could simply do :

if move_forward == True:
    speed.linear.x = dist
    #keep speed between 0.3 and 0.7
    if dist > 0.7:
        dist = 0.7
    elif dist < 0.3:
        dist = 0.3

    speed.linear.x = dist

First note here : is it intended for your robot to be only turning left ? That could be problematic if the robot deviates too much at the left of the goal, it would be turning for almost 2*pi instead of a little bit to the right to correct its orientation.

Anyway, you should be able to achieve what you want with some little changes :

  1. I would directly set the angular speed equal to the angle difference. That would allow the robot to always correct its orientation when reaching the goal. So your angular speed at any time could be :

    speed.angular.z = angle_to_goal - theta
    

    That being said you might want to have a function calculating the shortest angle (because your calculation might return 3*pi/2 instead of -pi/2) not to be always turning too much and also to deal with the case when your robot is aligned to the goal but the calculation returns 2*pi. If it's not what you want I would still recommend to get the sign of the angular difference to allow your robot to turn left or right when needed.

  2. I would change the condition. Instead of choosing between moving forward and turning, you could have a flag turning to true the first time your robot gets aligned to the goal and then allow it to move forward :

    move_forward = False        #define this outside of the while loop
    ...
    if abs(angle_to_goal - theta) > 0.1: 
            move_forward = True
    
    speed.angular.z = angle_to_goal - theta   #the angular speed is always the angualr difference
    if move_forward is True:
            speed.linear = 0.3                #the linear speed is only set once the flag is True
    
  3. If the robot has almost reached a goal but you are not exactly aligned with it and you are still moving forward at a constant speed, you might miss the goal (eventhough I don't know which tolerance you have) but to avoid that it is possible to also set the linear speed as the angular speed, but instead of the angular difference it would be the distance. Like that your robot will slow down when getting closer to the goal, letting you the time to adjust your orientation before missing the goal.


EDIT :

So if I understand the values right, if the coordinates are on the robots right side, I get a value between 0.0 and -pi returned and if its on the left its between 0.0 and pi? How do I effectively get which angle is smaller?

If you get the correct angle then yes but you won't get it simply with angle_to_goal - theta because angle_to_goal and theta will return a value between [-pi; pi] : If you have x > 0 a little angle, if you get angle_to_goal = pi - x and theta = -pi + x then angle_to_goal - theta = 2pi + 2x instead of 2x. So you have to modify the result of this angular difference, here's an exemple of a function (in C++) doing that :

float shortestAngleDifference(float th1, float th2)
{
    float anglediff = fmod( (th1 - th2) , 2*M_PI);

    if( anglediff < 0.0 )
    {
        if( fabs(anglediff) > (2*M_PI + anglediff) )
        {
            anglediff = 2*M_PI + anglediff;
        }
    }
    else
    {
        if( anglediff > fabs(anglediff - 2*M_PI) )
        {
            anglediff = anglediff - 2*M_PI;
        }
    }
    return anglediff;
}

Where angle_to_goal = th1 and theta = th2 to direclty have anglediff > 0 if the robot has to turn to the left and anglediff < 0 to turn to the right.

Note : In your solution you have multiple conditions to keep your speed between [0.3;0.7] but you could simply do :

if move_forward == True:
    speed.linear.x = dist
    #keep speed between 0.3 and 0.7
    if dist > 0.7:
        dist = 0.7
    elif dist < 0.3:
        dist = 0.3

    speed.linear.x = dist