Update odom_new.cpp

src/odom_new.cpp

@@ -69,6 +69,11 @@ class OdomNew
       //this->x  = 
       //this->y  = 
       //this->th = 
+      vx        = this->radius*( left_vel  + right_vel) / 2.0;
+      vth       = this->radius*( right_vel - left_vel ) / this->wheel_distance;
+      this->x  += vx * cos(this->th) * dt;
+      this->y  += vx * sin(this->th) * dt;
+      this->th += vth * dt;
       //////// TODO //////
       
       this->odom_msg.header.stamp = time;