Merge pull request #441 from alliander-opensource/438-integrate-xsens-imu

438 integrate xsens imu

Author: geurto

.devcontainer/dev/Dockerfile

@@ -18,6 +18,7 @@ RUN apt update && apt install -y --no-install-recommends \
 
 # Install MoveIt: 
 RUN apt update && apt install -y --no-install-recommends \
+    ros-$ROS_DISTRO-imu-filter-madgwick \
     ros-$ROS_DISTRO-moveit \
     ros-$ROS_DISTRO-moveit-servo \
     ros-$ROS_DISTRO-moveit-visual-tools \
@@ -64,6 +65,7 @@ COPY alliander_joystick/src/ /alliander/ros/src
 COPY alliander_meta/src/ /alliander/ros/src
 COPY alliander_moveit/src/ /alliander/ros/src
 COPY alliander_nav2/src/ /alliander/ros/src
+COPY alliander_xsens/src/ /alliander/ros/src
 
 # Build repo packages:
 RUN /alliander/colcon_build.sh

.gitignore

@@ -22,6 +22,7 @@ clangd/
 .venv
 
 # for LSP in neovim
+.nvim/
 **/compile_commands.json
 
 # generated compose files

alliander_core/src/alliander_description/CMakeLists.txt

@@ -11,7 +11,7 @@ find_package(ament_cmake_python REQUIRED)
 
 # Shared folders:
 install(
-  DIRECTORY franka husarion ouster velodyne realsense zed nmea_gps ewellix
+  DIRECTORY ewellix franka husarion nmea_gps ouster realsense velodyne xsens zed 
   DESTINATION share/${PROJECT_NAME}
 )
 

alliander_core/src/alliander_description/husarion/urdf/original/gazebo.urdf.xacro

@@ -141,4 +141,4 @@
     </gazebo>
   </xacro:macro>
 
-</robot>
+</robot>

alliander_core/src/alliander_description/xsens/meshes/MTi-6x0R.stl

@@ -0,0 +1,3 @@
+SPDX-FileCopyrightText: Alliander N. V.
+
+SPDX-License-Identifier: Apache-2.0

alliander_core/src/alliander_description/xsens/meshes/MTi-6x0R.stl.license

@@ -0,0 +1,106 @@
+<?xml version="1.0"?>
+<!--
+SPDX-FileCopyrightText: Alliander N. V.
+SPDX-License-Identifier: Apache-2.0
+-->
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="imu_module">
+
+  <xacro:macro name="xsens_imu"
+    params="
+    namespace
+    parent
+    name:=xsens
+    use_sim:=false
+    ">
+
+    <link name="base_link"/>
+    <link name="imu_link">
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://alliander_description/xsens/meshes/MTi-6x0R.stl" scale="0.001 0.001 0.001"/>
+        </geometry>
+        <material name="dark_grey">
+          <color rgba="0.2 0.2 0.2 1.0"/>
+        </material>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <!-- 56.5 x 40.9 x 24.75 mm in metres -->
+          <box size="0.0565 0.0409 0.02475"/>
+        </geometry>
+      </collision>
+
+      <inertial>
+        <!-- Approximate mass: 50g -->
+        <mass value="0.050"/>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <!-- Box inertia tensor: I = m/12 * (a² + b²) for each axis -->
+        <inertia ixx="8.22e-6" ixy="0" ixz="0"
+                               iyy="1.59e-5" iyz="0"
+                                              izz="2.03e-5"/>
+      </inertial>
+
+    </link>
+
+    <!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
+    <xacro:if value="${parent == 'world'}">
+      <link name="${parent}" />
+      <joint name="${parent}_joint" type="fixed">
+        <parent link="${parent}" />
+        <child link="base_link" />
+      </joint>
+    </xacro:if>
+
+    <!-- Since Xsens publishes data on imu_link on hardware -->
+    <joint name="imu_joint" type="fixed">
+      <parent link="base_link"/>
+      <child  link="imu_link"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+    </joint>
+
+    <!-- MTi-6x0 IMU sensor specification: https://www.xsens.com/hubfs/Downloads/Manuals/MTi-600-series-datasheet.pdf -->
+    <xacro:if value="${use_sim}">
+      <gazebo reference="imu_link">
+        <sensor name="${namespace}/imu" type="imu">
+          <always_on>true</always_on>
+          <update_rate>100</update_rate>
+          <topic>${namespace}/imu/data</topic>
+          <visualize>false</visualize>
+          <enable_metrics>false</enable_metrics>
+	  <gz_frame_id>${namespace}/imu_link</gz_frame_id>
+          <imu>
+            <orientation_reference_frame>
+              <localization>ENU</localization>
+            </orientation_reference_frame>
+            <angular_velocity>
+              <!-- Gyroscope noise density: 0.01 deg/s/sqrt(Hz), in-run bias: 2 deg/h -->
+              <x><noise type="gaussian"><mean>0.0</mean><stddev>1.75e-4</stddev><bias_mean>9.7e-6</bias_mean><bias_stddev>0.0</bias_stddev></noise></x>
+              <y><noise type="gaussian"><mean>0.0</mean><stddev>1.75e-4</stddev><bias_mean>9.7e-6</bias_mean><bias_stddev>0.0</bias_stddev></noise></y>
+              <z><noise type="gaussian"><mean>0.0</mean><stddev>1.75e-4</stddev><bias_mean>9.7e-6</bias_mean><bias_stddev>0.0</bias_stddev></noise></z>
+            </angular_velocity>
+            <linear_acceleration>
+              <!-- Accelerometer noise density: 0.003 m/s²/sqrt(Hz), in-run bias: 0.01 mg -->
+              <x><noise type="gaussian"><mean>0.0</mean><stddev>3.0e-3</stddev><bias_mean>9.8e-5</bias_mean><bias_stddev>0.0</bias_stddev></noise></x>
+              <y><noise type="gaussian"><mean>0.0</mean><stddev>3.0e-3</stddev><bias_mean>9.8e-5</bias_mean><bias_stddev>0.0</bias_stddev></noise></y>
+              <z><noise type="gaussian"><mean>0.0</mean><stddev>3.0e-3</stddev><bias_mean>9.8e-5</bias_mean><bias_stddev>0.0</bias_stddev></noise></z>
+            </linear_acceleration>
+          </imu>
+        </sensor>
+      </gazebo>
+    </xacro:if>
+
+  </xacro:macro>
+
+  <xacro:arg name="namespace" default="xsens" />
+  <xacro:arg name="parent" default="" />
+  <xacro:arg name="use_sim" default="false" />
+  <xacro:xsens_imu
+    namespace="$(arg namespace)"
+    parent="$(arg parent)"
+    use_sim="$(arg use_sim)" />
+
+</robot>

alliander_core/src/alliander_description/xsens/urdf/xsens.urdf.xacro

@@ -337,6 +337,19 @@ class GPS(Platform):
     diagnostic_timeouts: tuple[int, int, int] = (3, 5, 10)
 
 
+@dataclass
+class IMU(Platform):
+    """Configuration for an IMU platform.
+
+    Attributes:
+        platform_type (str): Type identifier for the platform.
+        usb_device (str): USB device path, e.g. /dev/imu.
+    """
+
+    platform_type: str = "IMU"
+    usb_device: str = "/dev/imu"
+
+
 @dataclass
 class Lift(Platform):
     """Configuration for a Lift platform.
@@ -359,7 +372,7 @@ class PlatformList(Config):
 
     platforms: List[
         Annotated[
-            Union[Platform, Arm, Vehicle, Lidar, Camera, GPS, Lift],
+            Union[Platform, Arm, Vehicle, Camera, GPS, IMU, Lidar, Lift],
             Discriminator(field="platform_type", include_supertypes=True),
         ]
     ] = field(default_factory=list)

alliander_core/src/alliander_utilities/alliander_utilities/config_objects.py

@@ -83,6 +83,10 @@ def get_bridge_topics(platforms: list[T]) -> list[str]:
             bridge_topics.append(
                 f"/{platform.namespace}/gps/fix@sensor_msgs/msg/NavSatFix@gz.msgs.NavSat"
             )
+        if platform.platform_type == "IMU":
+            bridge_topics.append(
+                f"/{platform.namespace}/imu/data@sensor_msgs/msg/Imu@gz.msgs.IMU"
+            )
     return bridge_topics
 
 

alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.py

@@ -11,7 +11,6 @@ ENV ROS_DISTRO=jazzy
 RUN apt update && apt install -y --no-install-recommends \
   ros-$ROS_DISTRO-husarion-components-description \
   ros-$ROS_DISTRO-nmea-navsat-driver \
-  ros-$ROS_DISTRO-robot-localization \
   && rm -rf /var/lib/apt/lists/* \
   && apt autoremove -y \
   && apt clean