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