438 integrate xsens imu

.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 \
@@ -59,6 +60,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
+  DIRECTORY franka husarion nmea_gps ouster velodyne realsense 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.license

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

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

@@ -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_utilities/alliander_utilities/config_objects.py

@@ -331,6 +331,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"
+
+
 # Configurations containing lists of platforms:
 @dataclass
 class PlatformList(Config):
@@ -342,7 +355,7 @@ class PlatformList(Config):
 
     platforms: List[
         Annotated[
-            Union[Platform, Arm, Vehicle, Lidar, Camera, GPS],
+            Union[Platform, Arm, Vehicle, Camera, GPS, IMU, Lidar],
             Discriminator(field="platform_type", include_supertypes=True),
         ]
     ] = field(default_factory=list)

alliander_gazebo/src/alliander_gazebo/launch/gazebo.launch.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_gps/alliander_gps.Dockerfile

@@ -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

alliander_gps/src/alliander_gps/launch/gps.launch.py

@@ -1,15 +1,14 @@
 # SPDX-FileCopyrightText: Alliander N. V.
 #
 # SPDX-License-Identifier: Apache-2.0
-from alliander_utilities.adapted_yaml import AdaptedYaml
 from alliander_utilities.config_objects import GPS
 from alliander_utilities.launch_argument import LaunchArgument
 from alliander_utilities.launch_utils import SKIP, state_publisher_node, static_tf_node
 from alliander_utilities.register import Register, RegisteredLaunchDescription
 from alliander_utilities.ros_utils import get_file_path
 from launch import LaunchContext, LaunchDescription
 from launch.actions import OpaqueFunction
-from launch_ros.actions import Node, SetParameter
+from launch_ros.actions import SetParameter
 
 T, F = True, False
 
@@ -50,57 +49,11 @@ def launch_setup(context: LaunchContext) -> list:
         {"platform_config": gps_config.to_str()},
     )
 
-    ekf_global_params = AdaptedYaml(
-        get_file_path("alliander_gps", ["config"], "ekf_global.yaml"),
-        {
-            "odom_frame": f"{gps_config.parent.namespace}/odom",
-            "base_link_frame": f"{gps_config.parent.namespace}/base_footprint",
-            "odom0": f"/{gps_config.parent.namespace}/odometry/wheels",
-            "odom1": f"/{gps_config.namespace}/odometry/gps",
-            "imu0": f"/{gps_config.parent.namespace}/imu/data",
-        },
-        root_key=gps_config.parent.namespace,
-    )
-    navsat_transform_params = AdaptedYaml(
-        get_file_path("alliander_gps", ["config"], "navsat_transform.yaml"),
-        {},
-        root_key=gps_config.parent.namespace,
-    )
-
-    navsat_transform = Node(
-        package="robot_localization",
-        executable="navsat_transform_node",
-        name="navsat_transform",
-        namespace=gps_config.namespace,
-        parameters=[navsat_transform_params.file],
-        remappings=[
-            ("imu", f"/{gps_config.parent.namespace}/imu/data"),
-            (
-                "odometry/filtered",
-                f"/{gps_config.parent.namespace}/odometry/global",
-            ),
-        ],
-    )
-
-    # Define EKF node that creates the tf between odom and map:
-    ekf_global = Node(
-        package="robot_localization",
-        executable="ekf_node",
-        name="ekf_global",
-        namespace=gps_config.parent.namespace,
-        parameters=[ekf_global_params.file],
-        remappings=[
-            ("odometry/filtered", f"/{gps_config.parent.namespace}/odometry/global"),
-        ],
-    )
-
     return [
         SetParameter(name="use_sim_time", value=gps_config.simulation),
         Register.on_start(state_publisher, context),
         Register.on_start(static_tf, context),
         Register.group(hardware, context) if not gps_config.simulation else SKIP,
-        Register.on_start(navsat_transform, context) if parent.link else SKIP,
-        Register.on_start(ekf_global, context) if parent.link else SKIP,
     ]
 
 

alliander_nav2/alliander_nav2.Dockerfile

@@ -11,6 +11,7 @@ ENV ROS_DISTRO=jazzy
 RUN apt update && apt install -y --no-install-recommends \
   ros-$ROS_DISTRO-navigation2 \
   ros-$ROS_DISTRO-nav2-bringup \
+  ros-$ROS_DISTRO-robot-localization \
   ros-$ROS_DISTRO-slam-toolbox \
   && rm -rf /var/lib/apt/lists/* \
   && apt autoremove -y \

alliander_nav2/src/alliander_nav2/config/nav2/ekf_global.yaml

@@ -0,0 +1,101 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+# Based on: https://github.com/husarion/husarion_ugv_ros/blob/ros2/husarion_ugv_localization/config/enu_localization_with_gps.yaml
+ekf_global:
+  ros__parameters:
+    frequency: 20.0
+    sensor_timeout: 0.05
+    two_d_mode: true
+
+    transform_time_offset: 0.0
+    transform_timeout: 0.05
+
+    world_frame: map
+    map_frame: map
+    odom_frame: substitute_me
+    base_link_frame: substitute_me
+    publish_tf: true
+    publish_acceleration: false
+
+    odom0: substitute_me
+    odom0_config: [false, false, false,
+                   false, false, false,
+                    true,  true, false,
+                   false, false,  true,
+                   false, false, false]
+    odom0_queue_size: 3
+    odom0_nodelay: true
+    odom0_differential: false
+    odom0_relative: true
+
+    odom1: substitute_me 
+    odom1_config: [true,  true, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false,
+                   false, false, false]
+    odom1_queue_size: 2
+    odom1_differential: false
+    odom1_relative: false
+
+    imu0: substitute_me
+    imu0_config: [false, false, false,
+                  false, false,  true,
+                  false, false, false,
+                  false, false,  true,
+                  false, false, false]
+    imu0_queue_size: 3
+    imu0_nodelay: true
+    imu0_differential: false
+    imu0_relative: false
+    imu0_remove_gravitational_acceleration: false
+
+    use_control: true
+    stamped_control: true
+    control_timeout: 0.5
+    control_config: [true, true, false, false, false, true]
+    acceleration_limits: [2.7, 1.5, 0.0, 0.0, 0.0, 5.7] # Values taken from WH01_controller.yaml and WH02_controller.yaml inside husarion_ugv_controller/config
+
+    predict_to_current_time: true
+
+    # Selected values ​​experimentally so as to ensure relatively fast convergence (values ​​should be about 10x higher than the sensor variance values)
+    dynamic_process_noise_covariance: true
+    process_noise_covariance: [5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0, 5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0, 5e-3,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0, 3e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 2e-4,  0.0,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5,  0.0,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5,  0.0,
+                                0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 5e-5]
+
+    initial_state: [0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0,
+                    0.0,  0.0,  0.0]
+
+    initial_estimate_covariance: [1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  1e5,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9,  0.0,
+                                   0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 1e-9]