From 1884e8a364b03331c4c61a0be3f948bd9c6f493d Mon Sep 17 00:00:00 2001
From: Fernando Herrero <fherrero@iri.upc.edu>
Date: Mon, 5 Jul 2021 09:42:25 +0000
Subject: [PATCH] Update odom_new.cpp
---
src/odom_new.cpp | 5 +++++
1 file changed, 5 insertions(+)
diff --git a/src/odom_new.cpp b/src/odom_new.cpp
index 2ca4689..932622a 100644
--- a/src/odom_new.cpp
+++ b/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;
--
GitLab