From: Federica Di Lauro <federicadilauro1998@gmail.com>
Date: Wed, 12 Feb 2020 15:44:18 +0000 (+0100)
Subject: start odometry
X-Git-Url: http://git.leonardobizzoni.com/?a=commitdiff_plain;h=b432c705759ce0cfc0141c01fcef16e4df0e0978;p=pioneer-stm32

start odometry
---

diff --git a/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py b/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py
index 2b258eb..89f90ea 100755
--- a/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py
+++ b/utils/catkin_ws/src/joypad_bridge/scripts/serial_receiver.py
@@ -1,10 +1,17 @@
 #!/usr/bin/env python
 
 import rospy, serial, struct, time
-import otto_communication_pb2
-from nav_msgs.msg import Odometry
 from serial import SerialException
+import otto_communication_pb2
 from google.protobuf.message import DecodeError
+import math
+from math import sin, cos, pi
+from nav_msgs.msg import Odometry
+from geometry_msgs.msg import Point, Pose, Quaternion, Twist, Vector3
+import tf
+
+import numpy
+
 
 
 ser = serial.Serial(
@@ -39,6 +46,11 @@ def serial_receiver():
     encoded_buffer = otto_status.SerializeToString()
 
     status_length = len(encoded_buffer)
+
+    odom_values = numpy.array([0,0,0]) #x, y, th
+    icc_x = 0;
+    icc_y = 0;
+    radius = 0;
     
     while (1):
         encoded_buffer = ser.read(status_length)
@@ -46,6 +58,26 @@ def serial_receiver():
         try:
             otto_status.ParseFromString(encoded_buffer)
             print(otto_status)
+            
+            lin_vel = otto_status.linear_velocity
+            ang_vel = otto_status.angular_velocity
+            d_time = otto_status.delta_millis
+            d_time = d_time / 1000.0
+            print(d_time)
+            if(ang_vel != 0 and d_time > 0):
+                radius = lin_vel / ang_vel
+                icc_x = (odom_values[0] -radius*sin(odom_values[2]))
+                icc_y = (odom_values[1] + radius*cos(odom_values[2]))
+                rotation_matrix = numpy.array([[cos(ang_vel*d_time), -sin(ang_vel*d_time), 0], [sin(ang_vel*d_time), cos(ang_vel*d_time), 0], [0, 0, 1]])
+                translate_icc_matrix = numpy.array([odom_values[0] - icc_x, odom_values[1] - icc_y, odom_values[2]])
+                translate_back_matrix = numpy.array([icc_x, icc_y, ang_vel*d_time])
+                odom_values = rotation_matrix.dot(translate_icc_matrix) + translate_back_matrix
+            elif(d_time > 0):
+                odom_values = numpy.array([odom_values[0]+(lin_vel/d_time), odom_values[1]+(lin_vel/d_time), odom_values[2]])
+            
+            print(odom_values)
+            
+            
         except DecodeError:
             print("Decode Error")
       
diff --git a/utils/py_serial_examples/proto_transmit.py b/utils/py_serial_examples/proto_transmit.py
index ba86410..c78c926 100644
--- a/utils/py_serial_examples/proto_transmit.py
+++ b/utils/py_serial_examples/proto_transmit.py
@@ -21,8 +21,8 @@ print("open port")
 
 otto = otto_communication_pb2.StatusMessage()
 otto.linear_velocity = 0.3
-otto.angular_velocity = 0.5
-otto.delta_millis = 1000
+otto.angular_velocity = 0.0
+otto.delta_millis = 500
 otto.status = otto_communication_pb2.StatusMessage.Status.RUNNING
 
 encode_buffer = otto.SerializeToString()