Ask Your Question
0

Do you know what the meaning of this code ?

asked 2018-12-22 09:13:01 -0500

Ilhamasy gravatar image

function of fnGet2DRobotPose is to transform euler from quarternion, and the value is pos_x, pos_y, theta.

def GetRobotOdom(self, robot_odom_msg):
    if self.odom_diterima == False:
        self.odom_diterima = True 

    pos_x, pos_y, theta = self.fnGet2DRobotPose(robot_odom_msg)

    self.robot_2d_pose_x = pos_x
    self.robot_2d_pose_y = pos_y
    self.robot_2d_theta = theta

    if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
    elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
    else:
        d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)**

    self.total_robot_2d_theta = self.total_robot_2d_theta + d_theta
    self.previous_robot_2d_theta = self.robot_2d_theta

    self.robot_2d_theta = self.total_robot_2d_theta

please help me :(

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
0

answered 2018-12-23 06:46:01 -0500

hamid gravatar image

some times you want control yaw(theta) but when you transform quarternion to euler the angles are between pi and -pi and you have one break point in pi and it's bother controller for this reason in this part of code:

if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) - 2 * math.pi
elif (self.robot_2d_theta - self.previous_robot_2d_theta) < -5.:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta) + 2 * math.pi
else:
    d_theta = (self.robot_2d_theta - self.previous_robot_2d_theta)**

if we have jump in angle (this jump is bigger than 2 *pi(-pi to pi)) Compute d_theta and add the d_theta to total_robot_2d_theta

edit flag offensive delete link more

Comments

okay, thanks for your answer, but if (self.robot_2d_theta - self.previous_robot_2d_theta) > 5.: what is the meaning of >5 i still not understand that ? :(

Ilhamasy gravatar imageIlhamasy ( 2018-12-23 09:39:00 -0500 )edit
1

This is the problem with using magic numbers

jayess gravatar imagejayess ( 2018-12-23 11:55:46 -0500 )edit

assume your teta is 3 and you have rotation your teta change like this (3,3.1,3.14,-3.14 ,-3.1,-3) you have one jump in your teta in 3.14 to -3.14 for detecting this jump you can Compute (teta-last_teta) and if the result bigger than 5 or 6 or 4 depend on your decision you have jump in your teta

hamid gravatar imagehamid ( 2018-12-23 22:30:54 -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

1 follower

Stats

Asked: 2018-12-22 09:13:01 -0500

Seen: 289 times

Last updated: Dec 23 '18