finihsed

config/omniwheels_config.yaml

 # 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

scripts/Buffer.py

+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)

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):