From e014bcf21f14e4404aa173776d3b62aab9153547 Mon Sep 17 00:00:00 2001
From: Stevedan Ogochukwu Omodolor <stevedan.o.omodolor@gmail.com>
Date: Thu, 2 Jun 2022 19:42:20 +0200
Subject: [PATCH] finihsed

---
 config/omniwheels_config.yaml               |   4 +--
 scripts/Buffer.py                           |  14 +++++++++
 scripts/Buffer.pyc                          | Bin 0 -> 899 bytes
 scripts/__pycache__/Buffer.cpython-37.pyc   | Bin 0 -> 913 bytes
 scripts/__pycache__/SimplePI.cpython-37.pyc | Bin 1146 -> 1146 bytes
 scripts/crazyflie_logger_node.py            |  32 ++++++++++++++------
 6 files changed, 38 insertions(+), 12 deletions(-)
 create mode 100644 scripts/Buffer.py
 create mode 100644 scripts/Buffer.pyc
 create mode 100644 scripts/__pycache__/Buffer.cpython-37.pyc

diff --git a/config/omniwheels_config.yaml b/config/omniwheels_config.yaml
index 8bd041b..7da0065 100644
--- a/config/omniwheels_config.yaml
+++ b/config/omniwheels_config.yaml
@@ -1,6 +1,6 @@
 # configuration file for all the omniwheels 
-name: "omniwheel2"
-id: 2
+name: "omniwheel1"
+id: 1
 channel: 90 # change to the correct one
 type: default
 position_offset: 0.11 # ofsset from crazyflie module to the cenyter of the omniwheels
diff --git a/scripts/Buffer.py b/scripts/Buffer.py
new file mode 100644
index 0000000..6313d5b
--- /dev/null
+++ b/scripts/Buffer.py
@@ -0,0 +1,14 @@
+import numpy as np 
+
+
+class Buffer:
+    def __init__(self, buffer_size):
+        self.buf = np.zeros(buffer_size)
+        
+    def get_mean(self):
+        return self.buf.mean()
+    def fill(self, data):
+        self.buf[0:-2] = self.buf[1:-1]
+        self.buf[-1] = data
+    def print_buffer(self):
+        print(self.buf)
diff --git a/scripts/Buffer.pyc b/scripts/Buffer.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..243ba8424069612b9e7ad7825fa08ae9c9b1a785
GIT binary patch
literal 899
zcmZSn%*z$`a7I!x0~9bbFfceUFfbG=GcYiuFfgPrGUPBYL@_d?FfpXCFr+Xuv@kF<
zGcZI(F@fZQHCP!K7&8C=|Nq~wgpGlL!KpMYEwu=493w~wWSkBI14AmrAO?^T5bZ4t
z3{lJ=qd^Rq=~1i<LBSd<3=9m(ObiSRFzgI6MvQ@hp@e}Ui;<y*k)fH9p@xAWo(bg6
zU=5H}ei|V6mw>e76_kLSR+U<mUt9tbO)5>(07;a9OfF8%Nh{%IU|>iByEnc#vno{s
z6a-+Rm=ok|u#8?oWeEoZ14Dd#W?p7Vd_2g^VvsQmjL9GyVHoT>UIqq`i?YBDY-V79
zISQmc2;>H^-MOiWd0^KDfm{N%5KLgPH9fT?9;6)P0+_8J!$26M8EmUI+}0FuG%40F
zFk~@+qAG=jLA!<#>ZBAV2AdR+n?PRh%LF+b9B-K*Z)g7d|NlS4Q6P7M0}AXsu!~a?
zOA^6O1QTfP1i2$EGbe`&6h9yi1EUm{u;GEb(%l2>43Go-kgNpp!B&DT0TXC8fs809
z%FHW?hr}8vxS^q<0ZMz&h{{XMO^uH)0l6nWJ~uz5G$%DaJ_w{B2o#P%Ac-JQ+=J~v
z5U92Vfr16(onnwCHbzcnP9{!fh-y#}fm2srX>LJf5ZL7)<q%mo3)Mid>uezIu>)xV
IJDrCa0F+UQg8%>k

literal 0
HcmV?d00001

diff --git a/scripts/__pycache__/Buffer.cpython-37.pyc b/scripts/__pycache__/Buffer.cpython-37.pyc
new file mode 100644
index 0000000000000000000000000000000000000000..b063a054839cc29247a2074c1200ab964583dbd5
GIT binary patch
literal 913
zcmZ?b<>g{vU|@)QI3vl0k%8echy%mS3=9ko3=9m#QVa|XDGVu$ISf&Z?hGkRDa<Vl
zDa_4GQA{a}!3>%#FF`u|l0hU4Gr{!gFfcHrGDI<^Fo0C2Ftsp5F{d!6u(U8lv81r3
zu(dEmv8J%6aI`Q)v88YZGiY+%Vsk1@OG_<E2AKge4@869;tX<&00RR<3Bv-$8pef;
zH4O1gDGb33nv8y$%(s~G3U0Ahr55EE-(pTGP19t$#ZsJ_lXi<c32a<^ab{KON`@j1
z1_p*-uKF4IxvBaEnfk>gsb#5o@g*6l#hJzN<;D8NMala4xp|r88L6o`#qr7cc_l^p
zIXS6C`o+mbnFS@q`Vi;o6;$5hh>y?A%PfhHX9YP8<W2_0Dt@RuOfH!b<Ov1_1`rKW
z1NH|f*lQTF7#1)tg!+J~2xQ7FmfY0DJWa+THU<WWW5F&i;$&c8xW$p4S`rVE2YI3x
zWEPTrFgcWPV_{%m0Nba=z`y{qFNHCMsg+3*!e&lkX=Sous9^-lFxfDGLzC6-CCI5S
z|NsC0zX;6w_y7NYO-8W$G?~CIyTy`{Sds{F8ptsa_pzj9=H!5URt&P2fl-37iW?dl
zAQ|i-!3U1f8ip*!W=2Mc3mE-0nQpNb6lLa>KrF=`96VqZ@sRlCMT!DpsHMpAewyr{
z_{vMnO^uJg#T6f)o1ape17h>U#}}3+=0IeMK+Z4X0R_Gwh!6%5U>_m~P+Sy&LLcPI
yVvsloBL_1F69+R`RFnA@YhGz?L1hsr)q;hP0t%!Y5?UNK5GUAy!WbNz9LxZFfWtBX

literal 0
HcmV?d00001

diff --git a/scripts/__pycache__/SimplePI.cpython-37.pyc b/scripts/__pycache__/SimplePI.cpython-37.pyc
index 367ecb7e4cf6ad3b1c9d85960607279f4cfee9b2..e97544cd15523b4b1ed1322cad87a432aedc926b 100644
GIT binary patch
delta 20
bcmeyx@r#4oiI<m)fq{X+|Lu&8-1#g3J1Yfw

delta 20
bcmeyx@r#4oiI<m)fq{WRrK@TqcRmXMHx>lV

diff --git a/scripts/crazyflie_logger_node.py b/scripts/crazyflie_logger_node.py
index cce46f6..6798272 100755
--- a/scripts/crazyflie_logger_node.py
+++ b/scripts/crazyflie_logger_node.py
@@ -22,7 +22,7 @@ from omniwheels_controller.msg import motorCommand
 from geometry_msgs.msg import Twist
 from geometry_msgs.msg import Pose
 from geometry_msgs.msg import Vector3
-
+from Buffer import Buffer
 
 uri = uri_helper.uri_from_env(default='usb://0') # Connection-uri for crazyflie via USB
 
@@ -58,6 +58,14 @@ class ParameterLogging:
         self._pose_data_ = Pose()
         self._vel_data_ = Twist()
         self._acc_data_ = Vector3()
+        # Buffer
+        self.bs = 30
+        self.pos_x_buf = Buffer(self.bs)
+        self.pos_y_buf = Buffer(self.bs)
+        self.pos_yaw_buf = Buffer(self.bs)
+        self.vel_x_buf = Buffer(self.bs)
+        self.vel_y_buf = Buffer(self.bs)
+        self.vel_z_buf = Buffer(self.bs)
         # create publishers 
         self._position_pub = rospy.Publisher("pose", Pose,queue_size=100)
         self._velocity_pub = rospy.Publisher("vel", Twist,queue_size=100)
@@ -205,14 +213,14 @@ class ParameterLogging:
         print('Error when logging %s: %s' % (logconf.name, msg))
 
     def _pos_log_data(self, timestamp, data, logconf):
-        x = data['stateEstimateZ.x']/1000.0
-        y = data['stateEstimateZ.y']/1000.0
+        self.pos_x_buf.fill(data['stateEstimateZ.x']/1000.0)
+        self.pos_y_buf.fill(data['stateEstimateZ.y']/1000.0)
         z = data['stateEstimateZ.z']/1000.0
         yaw = (data['stateEstimate.yaw'])
-        yaw_ = np.mod(yaw-30, 360);
+        self.pos_yaw_buf.fill(np.mod(yaw-30, 360));
 
        
-        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(yaw_))
+        quat_val = get_quaternion_from_euler(0,0,np.deg2rad(self.pos_yaw_buf.get_mean()))
         
 
         
@@ -220,17 +228,21 @@ class ParameterLogging:
         self._pose_data_.orientation.y =quat_val[1]
         self._pose_data_.orientation.z = quat_val[2]
         self._pose_data_.orientation.w = quat_val[3] 
-        self._pose_data_.position.x =x+ np.sin(np.deg2rad(yaw))*self._position_offset;
-        self._pose_data_.position.y = y- np.cos(np.deg2rad(yaw))*self._position_offset
+        self._pose_data_.position.x =self.pos_x_buf.get_mean()+ np.sin(np.deg2rad(yaw))*self._position_offset;
+        self._pose_data_.position.y = self.pos_x_buf.get_mean()- np.cos(np.deg2rad(yaw))*self._position_offset
         self._pose_data_.position.z = z;
         
              
         self._position_pub.publish(self._pose_data_)
        
     def _vel_log_data(self, timestamp,data, logconf):
-        self._vel_data_.linear.x = data['stateEstimateZ.vx']/1000.0
-        self._vel_data_.linear.y = data['stateEstimateZ.vy']/1000.0
-        self._vel_data_.angular.z = data['stateEstimateZ.rateYaw']/1000.0
+        self.vel_x_buf.fill(data['stateEstimateZ.vx']/1000.0)
+        self.vel_y_buf.fill(data['stateEstimateZ.vy']/1000.0)
+        self.vel_z_buf.fill(data['stateEstimateZ.rateYaw']/1000.0)
+
+        self._vel_data_.linear.x = self.vel_x_buf.get_mean()
+        self._vel_data_.linear.y = self.vel_y_buf.get_mean()
+        self._vel_data_.angular.z = self.vel_z_buf.get_mean()
         self._velocity_pub.publish(self._vel_data_)
         
     def _acc_log_data(self, timestamp,data, logconf):
-- 
GitLab