Merge branch 'main' of https://git.soton.ac.uk/plw1g21/robobin

ros2_testing/.vscode/c_cpp_properties.json

+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "${default}",
+        "limitSymbolsToIncludedHeaders": false
+      },
+      "includePath": [
+        "/opt/ros/jazzy/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS",
+      "intelliSenseMode": "gcc-arm64",
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "gnu11",
+      "cppStandard": "c++14"
+    }
+  ],
+  "version": 4
+}
\ No newline at end of file

ros2_testing/.vscode/settings.json

+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/jazzy/lib/python3.12/site-packages"
+    ]
+}
\ No newline at end of file

ros2_testing/build/.built_by

+colcon

ros2_testing/build/testing/build/lib/testing/imu_node.py

+'''#!/usr/bin/env python3'''
+#!/home/robobib/robobin_ws/venv/bin/python
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+import smbus
+import math
+from ahrs.filters import Madgwick
+
+
+class IMUNode(Node):
+    def __init__(self):
+        super().__init__("imusd_node")
+        self.get_logger().info("hello")
+
+
+        self.publisher = self.create_publisher(Imu, 'imu/data_raw',10)
+    
+        self.bus = smbus.SMBus(1)
+        self.DEVICE_ADDRESS = 0x68  # MPU6050 device address
+        self.mpu_init()
+
+        self.filter = Madgwick()
+        self.orientation = [0.0, 0.0, 0.0, 1.0]
+
+
+
+        time_period = 0.01
+        self.timer = self.create_timer(time_period, self.timer_callback)
+
+
+
+    def mpu_init(self):
+        PWR_MGMT_1 = 0x6B
+        SMPLRT_DIV = 0x19
+        CONFIG = 0x1A
+        GYRO_CONFIG = 0x1B
+        ACCEL_CONFIG = 0x1C
+        INT_ENABLE = 0x38
+
+        # Write to sample rate register
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, SMPLRT_DIV, 7)
+        # Write to power management register to wake up the MPU6050
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, PWR_MGMT_1, 1)
+        # Write to configuration register
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, CONFIG, 0)
+        # Write to gyroscope configuration register
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, GYRO_CONFIG, 24)
+        # Write to accelerometer configuration register
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, ACCEL_CONFIG, 0)
+        # Enable interrupts
+        self.bus.write_byte_data(self.DEVICE_ADDRESS, INT_ENABLE, 1)
+
+    def read_raw_data(self, addr):
+        # Read two bytes of data from the given address
+        high = self.bus.read_byte_data(self.DEVICE_ADDRESS, addr)
+        low = self.bus.read_byte_data(self.DEVICE_ADDRESS, addr+1)
+        # Combine higher and lower bytes
+        value = (high << 8) | low
+        # Convert to signed integer
+        if value > 32768:
+            value = value - 65536
+
+        return value
+
+    def timer_callback(self):
+
+
+        ACCEL_XOUT_H = 0x3B
+        ACCEL_YOUT_H = 0x3D
+        ACCEL_ZOUT_H = 0x3F
+
+        GYRO_XOUT_H = 0x43
+        GYRO_YOUT_H = 0x45
+        GYRO_ZOUT_H = 0x47
+
+        # Read accelerometer data
+        acc_x = self.read_raw_data(ACCEL_XOUT_H)
+        acc_y = self.read_raw_data(ACCEL_YOUT_H)
+        acc_z = self.read_raw_data(ACCEL_ZOUT_H)
+        
+        # Read gyroscope data
+        gyro_x = self.read_raw_data(GYRO_XOUT_H)
+        gyro_y = self.read_raw_data(GYRO_YOUT_H)
+        gyro_z = self.read_raw_data(GYRO_ZOUT_H)
+
+
+        ax_offset = 0.01467432
+        ay_offset = -0.00603101
+        az_offset = 0.06171924
+
+        gx_offset = -0.17447328
+        gy_offset = 0.08720611
+        gz_offset = 0.16423664
+
+        Ax = (acc_x / 16384.0)  - (ax_offset) # Accelerometer sensitivity scale factor
+        Ay = (acc_y / 16384.0)  - (ay_offset) 
+        Az = (acc_z / 16384.0) - (az_offset) 
+        Ax = 9.81*Ax
+        Ay = 9.81*Ay
+        Az = 9.81*Az
+        
+        Gx = (gyro_x / 131.0) - (gx_offset)   # Gyroscope sensitivity scale factor
+        Gy = (gyro_y / 131.0) - (gy_offset) 
+        Gz = (gyro_z / 131.0)  - (gz_offset) 
+        
+        Gx = Gx/180 *math.pi
+        Gy = Gy/180 *math.pi
+        Gz = Gz/180 *math.pi
+
+        self.orientation = self.filter.updateIMU(self.orientation,
+                                                 [Gx, Gy, Gz],
+                                                 [Ax/9.81, Ay/9.81, Az/9.81])
+        
+
+
+        imu_msg = Imu()
+        imu_msg.header.stamp =self.get_clock().now().to_msg()
+        imu_msg.header.frame_id = 'imu_link'
+        imu_msg.orientation_covariance[0] = -1.0
+
+        imu_msg.angular_velocity.x = Gx
+        imu_msg.angular_velocity.y = Gy
+        imu_msg.angular_velocity.z = Gz
+
+        imu_msg.linear_acceleration.x = Ax
+        imu_msg.linear_acceleration.y = Ay
+        imu_msg.linear_acceleration.z = Az
+
+        imu_msg.orientation.x =self.orientation[0]
+        imu_msg.orientation.y =self.orientation[1]
+        imu_msg.orientation.z =self.orientation[2]
+        imu_msg.orientation.w =self.orientation[3]
+
+        imu_msg.orientation_covariance = [0.0025, 0, 0,
+                                          0, 0.0025, 0,
+                                          0, 0, 0.0025]
+
+        self.publisher.publish(imu_msg)
+
+        
+
+
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    imu_node = IMUNode()
+    rclpy.spin(imu_node)
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
\ No newline at end of file

ros2_testing/build/testing/colcon_build.rc

+0

ros2_testing/build/testing/colcon_command_prefix_setup_py.sh

+# generated from colcon_core/shell/template/command_prefix.sh.em

ros2_testing/build/testing/colcon_command_prefix_setup_py.sh.env

+AMENT_PREFIX_PATH=/home/robobin/robobin_ws/install/testing:/opt/ros/jazzy
+CMAKE_PREFIX_PATH=/opt/ros/jazzy/opt/gz_math_vendor:/opt/ros/jazzy/opt/gz_utils_vendor:/opt/ros/jazzy/opt/gz_cmake_vendor
+COLCON=1
+COLCON_PREFIX_PATH=/home/robobin/robobin_ws/install
+COLORTERM=truecolor
+DBUS_SESSION_BUS_ADDRESS=unix:path=/run/user/1002/bus
+DEBUGINFOD_URLS=https://debuginfod.ubuntu.com
+DESKTOP_SESSION=ubuntu
+DISPLAY=:0
+GDMSESSION=ubuntu
+GIO_LAUNCHED_DESKTOP_FILE=/usr/share/applications/terminator.desktop
+GIO_LAUNCHED_DESKTOP_FILE_PID=3305
+GJS_DEBUG_OUTPUT=stderr
+GJS_DEBUG_TOPICS=JS ERROR;JS LOG
+GNOME_DESKTOP_SESSION_ID=this-is-deprecated
+GNOME_SETUP_DISPLAY=:1
+GNOME_SHELL_SESSION_MODE=ubuntu
+GSM_SKIP_SSH_AGENT_WORKAROUND=true
+GTK_MODULES=gail:atk-bridge
+HOME=/home/robobin
+IM_CONFIG_PHASE=1
+INVOCATION_ID=e87884792f444964b769f6d1b1655102
+JOURNAL_STREAM=8:20557
+LANG=en_US.UTF-8
+LC_ALL=en_US.UTF-8
+LD_LIBRARY_PATH=/opt/ros/jazzy/opt/rviz_ogre_vendor/lib:/opt/ros/jazzy/lib/aarch64-linux-gnu:/opt/ros/jazzy/opt/gz_math_vendor/lib:/opt/ros/jazzy/opt/gz_utils_vendor/lib:/opt/ros/jazzy/opt/gz_cmake_vendor/lib:/opt/ros/jazzy/lib
+LESSCLOSE=/usr/bin/lesspipe %s %s
+LESSOPEN=| /usr/bin/lesspipe %s
+LOGNAME=robobin
+LS_COLORS=rs=0:di=01;34:ln=01;36:mh=00:pi=40;33:so=01;35:do=01;35:bd=40;33;01:cd=40;33;01:or=40;31;01:mi=00:su=37;41:sg=30;43:ca=00:tw=30;42:ow=34;42:st=37;44:ex=01;32:*.tar=01;31:*.tgz=01;31:*.arc=01;31:*.arj=01;31:*.taz=01;31:*.lha=01;31:*.lz4=01;31:*.lzh=01;31:*.lzma=01;31:*.tlz=01;31:*.txz=01;31:*.tzo=01;31:*.t7z=01;31:*.zip=01;31:*.z=01;31:*.dz=01;31:*.gz=01;31:*.lrz=01;31:*.lz=01;31:*.lzo=01;31:*.xz=01;31:*.zst=01;31:*.tzst=01;31:*.bz2=01;31:*.bz=01;31:*.tbz=01;31:*.tbz2=01;31:*.tz=01;31:*.deb=01;31:*.rpm=01;31:*.jar=01;31:*.war=01;31:*.ear=01;31:*.sar=01;31:*.rar=01;31:*.alz=01;31:*.ace=01;31:*.zoo=01;31:*.cpio=01;31:*.7z=01;31:*.rz=01;31:*.cab=01;31:*.wim=01;31:*.swm=01;31:*.dwm=01;31:*.esd=01;31:*.avif=01;35:*.jpg=01;35:*.jpeg=01;35:*.mjpg=01;35:*.mjpeg=01;35:*.gif=01;35:*.bmp=01;35:*.pbm=01;35:*.pgm=01;35:*.ppm=01;35:*.tga=01;35:*.xbm=01;35:*.xpm=01;35:*.tif=01;35:*.tiff=01;35:*.png=01;35:*.svg=01;35:*.svgz=01;35:*.mng=01;35:*.pcx=01;35:*.mov=01;35:*.mpg=01;35:*.mpeg=01;35:*.m2v=01;35:*.mkv=01;35:*.webm=01;35:*.webp=01;35:*.ogm=01;35:*.mp4=01;35:*.m4v=01;35:*.mp4v=01;35:*.vob=01;35:*.qt=01;35:*.nuv=01;35:*.wmv=01;35:*.asf=01;35:*.rm=01;35:*.rmvb=01;35:*.flc=01;35:*.avi=01;35:*.fli=01;35:*.flv=01;35:*.gl=01;35:*.dl=01;35:*.xcf=01;35:*.xwd=01;35:*.yuv=01;35:*.cgm=01;35:*.emf=01;35:*.ogv=01;35:*.ogx=01;35:*.aac=00;36:*.au=00;36:*.flac=00;36:*.m4a=00;36:*.mid=00;36:*.midi=00;36:*.mka=00;36:*.mp3=00;36:*.mpc=00;36:*.ogg=00;36:*.ra=00;36:*.wav=00;36:*.oga=00;36:*.opus=00;36:*.spx=00;36:*.xspf=00;36:*~=00;90:*#=00;90:*.bak=00;90:*.crdownload=00;90:*.dpkg-dist=00;90:*.dpkg-new=00;90:*.dpkg-old=00;90:*.dpkg-tmp=00;90:*.old=00;90:*.orig=00;90:*.part=00;90:*.rej=00;90:*.rpmnew=00;90:*.rpmorig=00;90:*.rpmsave=00;90:*.swp=00;90:*.tmp=00;90:*.ucf-dist=00;90:*.ucf-new=00;90:*.ucf-old=00;90:
+MANAGERPID=2016
+MEMORY_PRESSURE_WATCH=/sys/fs/cgroup/user.slice/user-1002.slice/user@1002.service/session.slice/org.gnome.Shell@wayland.service/memory.pressure
+MEMORY_PRESSURE_WRITE=c29tZSAyMDAwMDAgMjAwMDAwMAA=
+OLDPWD=/home/robobin/robobin_ws/src
+PATH=/opt/ros/jazzy/bin:/usr/local/sbin:/usr/local/bin:/usr/sbin:/usr/bin:/sbin:/bin:/usr/games:/usr/local/games:/snap/bin:/snap/bin
+PWD=/home/robobin/robobin_ws/build/testing
+PYTHONPATH=/home/robobin/robobin_ws/build/testing:/home/robobin/robobin_ws/install/testing/lib/python3.12/site-packages:/opt/ros/jazzy/lib/python3.12/site-packages
+QT_ACCESSIBILITY=1
+QT_IM_MODULE=ibus
+ROS_AUTOMATIC_DISCOVERY_RANGE=SUBNET
+ROS_DISTRO=jazzy
+ROS_PYTHON_VERSION=3
+ROS_VERSION=2
+SESSION_MANAGER=local/robobin-desktop:@/tmp/.ICE-unix/2228,unix/robobin-desktop:/tmp/.ICE-unix/2228
+SHELL=/bin/bash
+SHLVL=1
+SSH_AUTH_SOCK=/run/user/1002/keyring/ssh
+SYSTEMD_EXEC_PID=2258
+TERM=xterm-256color
+TERMINATOR_DBUS_NAME=net.tenshu.Terminator25ef4b219e3b005583550f2b0f9f990c3
+TERMINATOR_DBUS_PATH=/net/tenshu/Terminator2
+TERMINATOR_UUID=urn:uuid:ddb4ccd5-a07c-443b-bcd1-40a69528d9e9
+USER=robobin
+USERNAME=robobin
+VTE_VERSION=7600
+WAYLAND_DISPLAY=wayland-0
+XAUTHORITY=/run/user/1002/.mutter-Xwaylandauth.Y52WW2
+XDG_ACTIVATION_TOKEN=56665f42-1bb5-466d-8f55-6df020c6c830
+XDG_CONFIG_DIRS=/etc/xdg/xdg-ubuntu:/etc/xdg
+XDG_CURRENT_DESKTOP=ubuntu:GNOME
+XDG_DATA_DIRS=/usr/share/ubuntu:/usr/share/gnome:/usr/local/share/:/usr/share/:/var/lib/snapd/desktop
+XDG_MENU_PREFIX=gnome-
+XDG_RUNTIME_DIR=/run/user/1002
+XDG_SESSION_CLASS=user
+XDG_SESSION_DESKTOP=ubuntu
+XDG_SESSION_TYPE=wayland
+XMODIFIERS=@im=ibus
+_=/usr/bin/colcon

ros2_testing/build/testing/package.xml

+/home/robobin/robobin_ws/src/testing/package.xml
\ No newline at end of file

ros2_testing/build/testing/prefix_override/sitecustomize.py

+import sys
+if sys.prefix == '/usr':
+    sys.real_prefix = sys.prefix
+    sys.prefix = sys.exec_prefix = '/home/robobin/robobin_ws/install/testing'

ros2_testing/build/testing/resource/testing

+/home/robobin/robobin_ws/src/testing/resource/testing
\ No newline at end of file

ros2_testing/build/testing/setup.cfg

+/home/robobin/robobin_ws/src/testing/setup.cfg
\ No newline at end of file

ros2_testing/build/testing/setup.py

+/home/robobin/robobin_ws/src/testing/setup.py
\ No newline at end of file

ros2_testing/build/testing/share/testing/hook/pythonpath_develop.dsv

+prepend-non-duplicate;PYTHONPATH;/home/robobin/robobin_ws/build/testing

ros2_testing/build/testing/share/testing/hook/pythonpath_develop.ps1

+# generated from colcon_powershell/shell/template/hook_prepend_value.ps1.em
+
+colcon_prepend_unique_value PYTHONPATH "$env:COLCON_CURRENT_PREFIX\/home/robobin/robobin_ws/build/testing"

ros2_testing/build/testing/share/testing/hook/pythonpath_develop.sh

+# generated from colcon_core/shell/template/hook_prepend_value.sh.em
+
+_colcon_prepend_unique_value PYTHONPATH "/home/robobin/robobin_ws/build/testing"

ros2_testing/build/testing/testing

+/home/robobin/robobin_ws/src/testing/testing
\ No newline at end of file

ros2_testing/build/testing/testing.egg-info/PKG-INFO

+Metadata-Version: 2.1
+Name: testing
+Version: 0.0.0
+Summary: TODO: Package description
+Maintainer: robobin
+Maintainer-email: robobin@todo.todo
+License: TODO: License declaration