IMU node now supports two IMU models

Author: afdaniele

launchers/sensor-imu.sh

@@ -0,0 +1,26 @@
+#!/bin/bash
+
+source /environment.sh
+
+# initialize launch file
+dt-launchfile-init
+
+# YOUR CODE BELOW THIS LINE
+# ----------------------------------------------------------------------------
+
+
+# NOTE: Use the variable DT_REPO_PATH to know the absolute path to your code
+# NOTE: Use `dt-exec COMMAND` to run the main process (blocking process)
+
+
+# launching app
+dt-exec roslaunch --wait \
+    imu_driver imu_node.launch \
+    veh:="$VEHICLE_NAME"
+
+
+# ----------------------------------------------------------------------------
+# YOUR CODE ABOVE THIS LINE
+
+# wait for app to end
+dt-launchfile-join

packages/imu_driver/config/imu_node/duckiebot.yaml

@@ -1,9 +1,16 @@
 # Sensor Configuration
-imu_device_id: "0x71"
 polling_hz: 30
 sensor_name: bottom_center
+connectors:
+    # MPU 6050
+    -   bus: 1
+        address: 0x68
+    # TODO: model 2
+    -   bus: 1
+        address: 0x71
+
 
 # Calibrations
 ang_vel_offset: [ 0, 0, 0 ]
 accel_offset: [ 0, 0, 0 ]
-temp_offset: 0
+temp_offset: 0

packages/imu_driver/src/imu_node.py

@@ -1,13 +1,18 @@
 #!/usr/bin/env python3
+
 import math
+from typing import Optional
 
-import adafruit_mpu6050  # IMU Driver
+import adafruit_mpu6050
 import board
 import rospy
-from duckietown.dtros import DTROS, NodeType, DTParam, ParamType
+import yaml
+from adafruit_mpu6050 import MPU6050
 from sensor_msgs.msg import Imu, Temperature
 from std_srvs.srv import Empty
 
+from duckietown.dtros import DTROS, NodeType
+
 
 # TODO: calibration and loading custom config
 
@@ -17,38 +22,49 @@ def __init__(self):
         super(IMUNode, self).__init__(node_name="imu_node", node_type=NodeType.DRIVER)
 
         # get ROS/Duckiebot parameters
-        self._imu_device_id = rospy.get_param('~imu_device_id', 0x71)
-        adafruit_mpu6050._MPU6050_DEVICE_ID = 0x71  # Overwrite Adafruit default device ID being wrong
         self._veh = rospy.get_param('~veh')
-        polling_hz = rospy.get_param("~polling_hz")
+        self._i2c_connectors = rospy.get_param("~connectors", {})
+        self._polling_hz = rospy.get_param("~polling_hz")
         self._temp_offset = rospy.get_param("~temp_offset")
         self._gyro_offset = rospy.get_param("~ang_vel_offset")
         self._accel_offset = rospy.get_param("~accel_offset")
         self.loginfo("===============IMU Node Init Val===============")
-        self.loginfo(f"Op Rate: {polling_hz}")
-        self.loginfo(f"ADDR: {self._imu_device_id}")
+        self.loginfo(f"Op Rate: {self._polling_hz}")
         self.loginfo("Acceleration Offset: X:%.2f, Y: %.2f, Z: %.2f m/s^2" % tuple(self._accel_offset))
         self.loginfo("Gyro Offset X:%.2f, Y: %.2f, Z: %.2f degrees/s" % tuple(self._gyro_offset))
         self.loginfo("Temp Offset: %.2f C" % self._temp_offset)
         self.loginfo("===============END of IMU Init Val===============")
         # IMU Initialization
-        try:
-            self._imu = adafruit_mpu6050.MPU6050(board.I2C())
-            self.loginfo("===============Performing Initial Testing!===============")
-            self.loginfo("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" % self._imu.acceleration)
-            self.loginfo("Gyro X:%.2f, Y: %.2f, Z: %.2f degrees/s" % self._imu.gyro)
-            self.loginfo("Temperature: %.2f C" % self._imu.temperature)
-            self.loginfo("===============IMU Initialization Complete===============")
-        except Exception as IMUInitException:
-            self.logerr("IMU sensor not correctly setup! Error:")
-            self.logerr(IMUInitException)
+        self._sensor: Optional[MPU6050] = self._find_sensor()
+        if not self._sensor:
+            conns: str = yaml.safe_dump(self._i2c_connectors, indent=2, sort_keys=True)
+            self.logerr(f"No MPU6050 device found. These connectors were tested:\n{conns}\n")
             exit(1)
-
+        # ---
+        self.loginfo("===============Performing Initial Testing!===============")
+        self.loginfo("Acceleration: X:%.2f, Y: %.2f, Z: %.2f m/s^2" % self._sensor.acceleration)
+        self.loginfo("Gyro X:%.2f, Y: %.2f, Z: %.2f degrees/s" % self._sensor.gyro)
+        self.loginfo("Temperature: %.2f C" % self._sensor.temperature)
+        self.loginfo("===============IMU Initialization Complete===============")
         # ROS Pubsub initialization
-        self.pub = rospy.Publisher('~imu_data', Imu, queue_size=10)
-        self.temp_pub = rospy.Publisher('~temp_data', Temperature, queue_size=10)
+        self.pub = rospy.Publisher('~data', Imu, queue_size=10)
+        self.temp_pub = rospy.Publisher('~temperature', Temperature, queue_size=10)
         rospy.Service("~initialize_imu", Empty, self.zero_sensor)
-        self.timer = rospy.Timer(rospy.Duration.from_sec(1.0 / polling_hz), self.publish_data)
+        self.timer = rospy.Timer(rospy.Duration.from_sec(1.0 / self._polling_hz), self.publish_data)
+
+    def _find_sensor(self) -> Optional[MPU6050]:
+        for connector in self._i2c_connectors:
+            conn: str = "[bus:{bus}](0x{address:02X})".format(**connector)
+            self.loginfo(f"Trying to open device on connector {conn}")
+            # Overwrite Adafruit default device ID
+            adafruit_mpu6050._MPU6050_DEVICE_ID = connector["address"]
+            try:
+                sensor = MPU6050(board.I2C())
+            except Exception:
+                self.logwarn(f"No devices found on connector {conn}, but the bus exists")
+                continue
+            self.loginfo(f"Device found on connector {conn}")
+            return sensor
 
     def publish_data(self, event):
         # Message Blank
@@ -58,16 +74,14 @@ def publish_data(self, event):
         try:
             # You take the time immediately when you are polling imu
             msg.header.stamp = temp_msg.header.stamp = rospy.Time.now()
-            acc_data = self._imu.acceleration
-            gyro_data = self._imu.gyro
-            temp_data = self._imu.temperature
+            acc_data = self._sensor.acceleration
+            gyro_data = self._sensor.gyro
+            temp_data = self._sensor.temperature
             # Do it together so that the timestamp is honored
-
             # Populate Message
             msg.header.frame_id = temp_msg.header.frame_id = f"{self._veh}/imu"
-
-            # Orientation
-            msg.orientation.x = msg.orientation.y = msg.orientation.z = msg.orientation.w = 0  # We do not have this data
+            # Orientation (we do not have this data)
+            msg.orientation.x = msg.orientation.y = msg.orientation.z = msg.orientation.w = 0
             # If you have no estimate for one of the data elements
             # set element 0 of the associated covariance matrix to -1
             msg.orientation_covariance = [0.0 for _ in range(len(msg.orientation_covariance))]
@@ -90,10 +104,10 @@ def publish_data(self, event):
             pass
         return
 
-    def zero_sensor(self, req):
-        acc_data = self._imu.acceleration
-        gyro_data = self._imu.gyro
-        temp_data = self._imu.temperature
+    def zero_sensor(self, _):
+        acc_data = self._sensor.acceleration
+        gyro_data = self._sensor.gyro
+        temp_data = self._sensor.temperature
         self._gyro_offset = list(gyro_data)
         self._accel_offset = list(acc_data)
         self._temp_offset = temp_data