Do you know what the meaning of this code ?
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 :(