|
@@ -84,8 +84,8 @@ class BaseController:
|
|
self.enc_B = 0
|
|
self.enc_B = 0
|
|
self.enc_C = 0
|
|
self.enc_C = 0
|
|
|
|
|
|
- self.x = 0 # position in xy plane
|
|
|
|
- self.y = 0
|
|
|
|
|
|
+ self.pose_x = 0 # position in xy plane
|
|
|
|
+ self.pose_y = 0
|
|
self.th = 0 # rotation in radians
|
|
self.th = 0 # rotation in radians
|
|
|
|
|
|
self.v_A = 0
|
|
self.v_A = 0
|
|
@@ -217,8 +217,8 @@ class BaseController:
|
|
delta_y = (vx*sin(self.th) + vy*cos(self.th))*dt
|
|
delta_y = (vx*sin(self.th) + vy*cos(self.th))*dt
|
|
delta_th = vth*dt
|
|
delta_th = vth*dt
|
|
|
|
|
|
- self.x += delta_x
|
|
|
|
- self.y += delta_y
|
|
|
|
|
|
+ self.pose_x += delta_x
|
|
|
|
+ self.pose_y += delta_y
|
|
self.th += delta_th
|
|
self.th += delta_th
|
|
|
|
|
|
quaternion = Quaternion()
|
|
quaternion = Quaternion()
|
|
@@ -231,8 +231,8 @@ class BaseController:
|
|
odom.header.frame_id = self.odom_name
|
|
odom.header.frame_id = self.odom_name
|
|
odom.child_frame_id = self.base_frame
|
|
odom.child_frame_id = self.base_frame
|
|
odom.header.stamp = time_now
|
|
odom.header.stamp = time_now
|
|
- odom.pose.pose.position.x = self.x
|
|
|
|
- odom.pose.pose.position.y = self.y
|
|
|
|
|
|
+ odom.pose.pose.position.x = self.pose_x
|
|
|
|
+ odom.pose.pose.position.y = self.pose_y
|
|
odom.pose.pose.position.z = 0
|
|
odom.pose.pose.position.z = 0
|
|
odom.pose.pose.orientation = quaternion
|
|
odom.pose.pose.orientation = quaternion
|
|
odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
|
|
odom.pose.covariance = [1e-9, 0, 0, 0, 0, 0,
|