0

I am trying to implement a basic path planning algorithm using python with a 4 wheels robot created for ROS and Gazebo, RViz.

The only thing required for my algorithm is to make my robot oriented towards a given (using mouse) point in the x,y plane.

The problem I am facing is that I am converting from quaternion to Euler angles and I always (no mater how I manipulate my code) come to a situation like the following:

Theta to target:257.106918658
Theta_deg:-179.85
dTheta:-436.96
Theta to target:257.106918658
Theta_deg:-179.85
dTheta:-436.96
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99
dTheta:-77.13
Theta to target:257.118158708
Theta_deg:179.99

So:

Theta to target: Euler [-180, 180] Angle in [deg] of the line connecting the center of mass of robot to the given point.

Theta_deg: Euler [-180, 180] Angle in [deg] of the orientation (vertical vector on the front face of the robot) to X axis.

As you can see from the data above, Theta_deg experiences a non-continuous step from:

Theta_deg:-179.99

to

Theta_deg:179.99

I know that this is an issue with using Euler angles. How can I overcome this issue?

Relative code:

while not rospy.is_shutdown():
    theta_deg = (round(180*(theta/math.pi),2))
    old_distance = round(distance_to_target,2)

    theta_to_target =180*(math.pi/2+math.atan2((y-goal.y),(x-goal.x)))/math.pi        
    dTheta =(round(((theta_deg)-(theta_to_target)),2))

    dtt = round((math.sqrt((goal.x - x)**2 + (goal.y - y)**2)),2)

    if dtt < 0.1:
        print "<<< Destination Reached >>>"
    else:
        theta_to_target =180*(math.pi/2+math.atan2((y-goal.y),(x-goal.x)))/math.pi
        dTheta =(round(((theta_deg)-(theta_to_target)),2))
        print "Theta to target:" + str(theta_to_target)
        print "Theta_deg:" + str(theta_deg)
        print "dTheta:" + str(dTheta)
        # if dTheta<-360: dTheta=dTheta+360
        # if dTheta>360: dTheta=dTheta-360

        if abs(dTheta)>10:
            if abs(theta_to_target - theta_deg)>180:
                speed.angular.z = -0.2
                speed.linear.x = 0.0
                pub.publish(speed)
            else:
                speed.angular.z = +0.2
                speed.linear.x = 0.0
                pub.publish(speed)
        else:
            speed.linear.x = 0.6


    pub.publish(speed)
    r.sleep()
Jonas
  • 121,568
  • 97
  • 310
  • 388
Michael
  • 3
  • 1
  • 3
  • Instead of fixing the maths aspect, can you make it so that your robot always goes in the shortest rotation direction? For example from `-179.99` to `+179.99` degrees would not be a full turn but `-0.02` degrees. I think `dθ = min(dθ%360, dθ%360 + 360, key=abs)` would work. – Guimoute Mar 14 '20 at 21:00
  • 1
    Thank you very much for your comment. This is exactly what I did and it turned out it was the correct way to go. Thanks again. – Michael Mar 15 '20 at 21:38

0 Answers0