Fix inertial unit readings (#215)

* Update imu_device.py

* Add inertial unit to TurtleBot3

* Update turtlebot3_burger_example.wbt

* Fix orientation

Author: lukicdarkoo

webots_ros2_core/webots_ros2_core/devices/imu_device.py

@@ -87,10 +87,11 @@ def step(self):
                 msg.angular_velocity.y = interpolate_lookup_table(raw_data[1], self.__gyro.getLookupTable())
                 msg.angular_velocity.z = interpolate_lookup_table(raw_data[2], self.__gyro.getLookupTable())
             if self.__inertial_unit:
-                raw_data = self.__inertial_unit.getValues()
-                msg.orientation.x = interpolate_lookup_table(raw_data[0], self.__inertial_unit.getLookupTable())
-                msg.orientation.y = interpolate_lookup_table(raw_data[1], self.__inertial_unit.getLookupTable())
-                msg.orientation.z = interpolate_lookup_table(raw_data[2], self.__inertial_unit.getLookupTable())
+                raw_data = self.__inertial_unit.getQuaternion()
+                msg.orientation.x = raw_data[0]
+                msg.orientation.y = raw_data[1]
+                msg.orientation.z = raw_data[2]
+                msg.orientation.w = raw_data[3]
             self._publisher.publish(msg)
         else:
             self.__disable_imu()

webots_ros2_turtlebot/webots_ros2_turtlebot/turtlebot_driver.py

@@ -34,7 +34,7 @@ def __init__(self, args):
         self.start_device_manager({
             'robot': {'publish_base_footprint': True},
             'LDS-01': {'topic_name': '/scan'},
-            'accelerometer+gyro': {'frame_id': 'imu_link', 'topic_name': '/imu'}
+            'inertial_unit+accelerometer+gyro': {'frame_id': 'imu_link', 'topic_name': '/imu'}
         })
 
 

webots_ros2_turtlebot/worlds/turtlebot3_burger_example.wbt

@@ -1,13 +1,13 @@
-#VRML_SIM R2020b utf8
+#VRML_SIM R2021a utf8
 WorldInfo {
   info [
     "Simple TurtleBot3 Burger simulation that can be controlled by a ROS."
   ]
   title "ROS2 simulation of the TurtleBot3 Burger robot"
 }
 Viewpoint {
-  orientation 0.10641236421720016 -0.25207787332085546 -0.9618384243331832 2.039170527287739
-  position -3.8306574674676366 4.499047968479621 23.02378266720472
+  orientation -0.1500994671353544 -0.4810853207937876 -0.8637285824160397 3.5119475446104667
+  position 9.974034902718383 6.242698326848728 4.5715004345304
   follow "TurtleBot3Burger:LDS-01"
 }
 TexturedBackground {
@@ -21,6 +21,14 @@ TurtleBot3Burger_enu {
   controllerArgs [
     ""
   ]
+  extensionSlot [
+    InertialUnit {
+      rotation 0 1 0 3.14
+      name "inertial_unit"
+    }
+    RobotisLds01 {
+    }
+  ]
 }
 PottedTree {
   translation 7 -4 0