diff --git a/.gitignore b/.gitignore index 8641f83..82ce0f9 100644 --- a/.gitignore +++ b/.gitignore @@ -173,4 +173,5 @@ deploy/workspaces/nav2_ws/log _isaac_sim .vscode -deploy/sgiaun-umut \ No newline at end of file +deploy/sgiaun-umut +examples/datasets \ No newline at end of file diff --git a/Go2Py/assets/calibration/imu/acceleration.png b/Go2Py/assets/calibration/imu/acceleration.png new file mode 100644 index 0000000..79a653d --- /dev/null +++ b/Go2Py/assets/calibration/imu/acceleration.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8b306743e3775c2b000f0ba490b99415e388f631731940066354fb8389e08e49 +size 1867195 diff --git a/Go2Py/assets/calibration/imu/gyro.png b/Go2Py/assets/calibration/imu/gyro.png new file mode 100644 index 0000000..f01c608 --- /dev/null +++ b/Go2Py/assets/calibration/imu/gyro.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:bb6a2b3f3090c86bc82e1c0728e7d2b96b57c1f6921f0298974fcafb8313a6ef +size 1672152 diff --git a/Go2Py/assets/calibration/imu/imu.yaml b/Go2Py/assets/calibration/imu/imu.yaml new file mode 100644 index 0000000..b9bca8e --- /dev/null +++ b/Go2Py/assets/calibration/imu/imu.yaml @@ -0,0 +1,11 @@ +#Accelerometer +accelerometer_noise_density: 0.004059677310589936 +accelerometer_random_walk: 7.228500058270378e-05 + +#Gyroscope +gyroscope_noise_density: 0.0010796822189602655 +gyroscope_random_walk: 9.093779634369827e-06 + +rostopic: '/go2/imu' #Make sure this is correct +update_rate: 500.0 #Make sure this is correct + diff --git a/examples/00-robot-interface.ipynb b/examples/00-robot-interface.ipynb index 253b6ac..ecc8730 100644 --- a/examples/00-robot-interface.ipynb +++ b/examples/00-robot-interface.ipynb @@ -13,7 +13,7 @@ "metadata": {}, "outputs": [], "source": [ - "from Go2Py.robot.interface.dds import GO2Real\n", + "from Go2Py.robot.interface import GO2Real\n", "import time\n", "robot = GO2Real(mode='lowlevel')" ] @@ -68,7 +68,7 @@ } ], "source": [ - "from Go2Py.robot.interface.dds import GO2Real\n", + "from Go2Py.robot.interface import GO2Real\n", "import time\n", "robot = GO2Real(mode='highlevel')" ] diff --git a/examples/imu-noise-id-dataset-collector.py b/examples/imu-noise-id-dataset-collector.py new file mode 100644 index 0000000..0f6d9ea --- /dev/null +++ b/examples/imu-noise-id-dataset-collector.py @@ -0,0 +1,55 @@ +from Go2Py.robot.interface import GO2Real +import time +import numpy as np + +from rosbags.rosbag2 import Writer +from rosbags.typesys import Stores, get_typestore +from rosbags.typesys.stores.ros2_foxy import ( + builtin_interfaces__msg__Time as Time, + sensor_msgs__msg__Imu as Imu, + std_msgs__msg__Header as Header, + geometry_msgs__msg__Vector3 as Vector3, + geometry_msgs__msg__Quaternion as Quaternion, +) + + +def main(): + robot = GO2Real(mode='lowlevel') + time.sleep(1) + soc = robot.getBatteryState() + imu = robot.getIMU() + accel = imu['accel'] + gyro = imu['gyro'] + """Iterate over IMAGES and save to output bag.""" + typestore = get_typestore(Stores.ROS2_FOXY) + with Writer('imu_noise_identification_dataset') as writer: + conn = writer.add_connection('/go2/imu', Imu.__msgtype__) + + while(soc>10): + soc = robot.getBatteryState() + imu = robot.getIMU() + a = imu['accel'] + g = imu['gyro'] + accel = Vector3(a[0], a[1], a[2]) + gyro = Vector3(g[0], g[1], g[2]) + q = Quaternion(0,0,0,1) + timestamp = time.time()*10**9 + msg = Imu( + Header( + stamp=Time(sec=int(timestamp // 10**9), nanosec=int(timestamp % 10**9)), + frame_id='base_link'), + linear_acceleration=accel, + angular_velocity= gyro, + orientation=q, + orientation_covariance=np.zeros(9), + linear_acceleration_covariance=np.zeros(9), + angular_velocity_covariance=np.zeros(9) + ) + writer.write(conn, timestamp, typestore.serialize_cdr(msg, msg.__msgtype__)) + time.sleep(0.02) + print(soc) + robot.close() + exit() + +if __name__=='__main__': + main() \ No newline at end of file