Проблема с ориентацией робота из-за углов Эйлера - PullRequest
Новая
       94

Проблема с ориентацией робота из-за углов Эйлера

0 голосов
/ 14 марта 2020

Я пытаюсь реализовать базовый c алгоритм планирования пути, используя python с 4-колесным роботом, созданным для ROS и Gazebo, RViz.

Единственное, что требуется для моего алгоритма, - это заставить мой алгоритм робот, ориентированный на заданную (с помощью мыши) точку в плоскости x, y.

Проблема, с которой я сталкиваюсь, заключается в том, что я преобразую кватернионы в углы Эйлера и всегда (неважно, как я манипулирую своим кодом) прийти к ситуации, подобной следующей: 

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

Итак:

Тета к цели: Эйлер [-180, 180] Угол в [градус] линии, соединяющей центр масс робот до заданной точки.

Theta_deg: Euler [-180, 180] Угол в [градусах] ориентации (вертикальный вектор на передней грани робота) к оси X.

Как видно из приведенных выше данных, Theta_deg испытывает непостоянный шаг от: 

Theta_deg:-179.99

до 

Theta_deg:179.99

Я знаю, что это проблема с использованием углов Эйлера , Как я могу решить эту проблему?

Относительный код: 

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()
...