robofsm: add ..._trans version to robot_get_est_pos() function

diff --git a/src/robofsm/robot.c b/src/robofsm/robot.c
index 8ae7b0bbd14d8e9915d51989ef47c7b4085f8f8a..f6e686501c542a9452f532918ce161be87e8c39d 100644 (file)
--- a/src/robofsm/robot.c
+++ b/src/robofsm/robot.c
@@ -276,6 +276,14 @@ void robot_destroy()
        DBG("robofsm: stop.\n");
 }
 
+void robot_get_est_pos_trans(double *x, double *y, double *phi)
+{
+       robot_get_est_pos(x, y, phi);
+       *x = __trans_x(*x);
+       *y = __trans_y(*y);
+       *phi = __trans_ang(*phi);
+}
+
 void robot_get_est_pos(double *x, double *y, double *phi)
 {
        if (robot.indep_odometry_works) {

diff --git a/src/robofsm/robot.h b/src/robofsm/robot.h
index 60b2b8ba3f5708a23c3d9da17ee8eba7ed1c11d1..fdb2c695858943e1d446690a1a061d4d30d5fc95 100644 (file)
--- a/src/robofsm/robot.h
+++ b/src/robofsm/robot.h
@@ -230,6 +230,7 @@ int robot_start() __attribute__ ((warn_unused_result));
 void robot_exit();
 void robot_destroy();
 
+void robot_get_est_pos_trans(double *x, double *y, double *phi);
 void robot_get_est_pos(double *x, double *y, double *phi);
 
 /* Hack to easily disable display if it is not configured */