better localization update with amcl or odom
Hi. I am running a differential robot. In my original code i use encoder information for odom update. Can i use amcl_pose to update odom information ?
I cant decide whether to use encoder information or amcl_pose for better localization.
Original Code
self.dth = self.vth * dt
self.pth += self.dth
if(self.pth >= two_pi):
self.pth-= two_pi
if(self.pth <= -two_pi):
self.pth += two_pi
self.vxx= self.vx * math.cos(self.vth)
self.vxy= (self.vx * math.sin(self.vth))
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt
self.px += self.dx #last position
self.py += self.dy
----------------------------------------
Update code with Amcl
I add 3 line to update position info
self.px=amcl_pose1.pose.pose.position.x
self.py=amcl_pose1.pose.pose.position.y
self.pth=amcl_pose1.pose.pose.orientation.z
self.dth = self.vth * dt
self.pth += self.dth
if(self.pth >= two_pi):
self.pth-= two_pi
if(self.pth <= -two_pi):
self.pth += two_pi
self.vxx= self.vx * math.cos(self.vth)
self.vxy= (self.vx * math.sin(self.vth))
self.dx = (self.vxx * math.cos(self.pth)-self.vxy*sin(self.pth)) * dt
self.dy = (self.vxx * math.sin(self.pth)+ self.vxy*cos(self.pth)) * dt
self.px += self.dx #last position
self.py += self.dy