302 Add gps support to navigation (#323)

* First steps.

Signed-off-by: Jelmer de Wolde <[email protected]>

* WIP

Signed-off-by: Jelmer de Wolde <[email protected]>

* Fix local odometry and visualize in Rviz and Vizanti.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Create 3D world form open street map.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Add license.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Add osm2world install script to docker file.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Remove zip after unpacking.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Show disk space in post install.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Check disk space before and after chown.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Undo changes.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Load 3D world based on map.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Restructure sdf files.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Use dynamic namespaces.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Us logger instead of printing.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Fix test.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Change osm2world properties to not add road marking materials since this results in height differences.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Only remap gps topic when using gps.

Signed-off-by: Jelmer de Wolde <[email protected]>

* Skip joy gripper tests.

Signed-off-by: Jelmer de Wolde <[email protected]>

---------

Signed-off-by: Jelmer de Wolde <[email protected]>

Author: Jelmerdw

dockerfiles/install_scripts/dev_packages.sh

@@ -14,4 +14,5 @@ apt install -y \
     xvfb \
     ros-$ROS_DISTRO-nmea-navsat-driver \
     ros-$ROS_DISTRO-moveit-ros-perception \
-    ros-$ROS_DISTRO-topic-tools
+    ros-$ROS_DISTRO-topic-tools \
+    ros-$ROS_DISTRO-rviz-satellite

dockerfiles/install_scripts/osm2world.sh

@@ -0,0 +1,15 @@
+#!/bin/bash -i
+
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+set -e
+source /home/$UNAME/.bashrc
+apt update
+
+mkdir -p /home/$UNAME/osm2world
+cd /home/$UNAME/osm2world
+wget https://osm2world.org/download/files/latest/OSM2World-latest-bin.zip
+unzip OSM2World-latest-bin.zip
+rm OSM2World-latest-bin.zip

dockerfiles/rcdt_robotics.Dockerfile

@@ -41,6 +41,9 @@ RUN ./vizanti.sh
 COPY ./install_scripts/franka_lock_unlock.sh .
 RUN ./franka_lock_unlock.sh
 
+COPY ./install_scripts/osm2world.sh .
+RUN ./osm2world.sh
+
 COPY ./install_scripts/dev_packages.sh .
 RUN ./dev_packages.sh
 

pyproject.toml

@@ -28,8 +28,10 @@ dependencies = [
     "pillow>=11.3.0",
     "pre-commit>=4.2.0",
     "psutil>=7.0.0",
+    "pydantic>=2.11.10",
     "pydoclint>=0.6.6",
     "pymongo>=4.14.1",
+    "pyproj>=3.7.2",
     "pyrealsense2>=2.56.5.9235",
     "pytest<8.4.0",
     "pytest-timeout>=2.4.0",

ros2_ws/src/rcdt_gazebo/config/controllers.yaml

@@ -2,7 +2,9 @@
 #
 # SPDX-License-Identifier: Apache-2.0
 
-# Based on: https://github.com/husarion/panther_ros/blob/ros2/panther_controller/config/WH01_controller.yaml
+# Based on: 
+# https://github.com/husarion/husarion_ugv_ros/blob/ros2/husarion_ugv_controller/config/WH01_controller.yaml
+# https://github.com/frankarobotics/franka_ros2/blob/humble/franka_fr3_moveit_config/config/fr3_ros_controllers.yaml
 
 /**:
   controller_manager:
@@ -21,6 +23,9 @@
       drive_controller:
         type: diff_drive_controller/DiffDriveController
 
+      imu_broadcaster:
+        type: imu_sensor_broadcaster/IMUSensorBroadcaster
+
   fr3_arm_controller:
     ros__parameters:
       allow_nonzero_velocity_at_trajectory_end: true
@@ -48,6 +53,21 @@
       stall_timeout: 1.0
       stall_velocity_threshold: 0.001
 
+  # IMU specification: https://www.phidgets.com/?tier=3&catid=10&pcid=8&prodid=1025#Tab_Specifications
+  imu_broadcaster:
+    ros__parameters:
+      sensor_name: substitute_me
+      frame_id: substitute_me
+      # orientation_stddev: 4.3e-2 rad determined experimentally
+      static_covariance_orientation:
+        [1.8e-3, 0.0, 0.0, 0.0, 1.8e-3, 0.0, 0.0, 0.0, 1.8e-3]
+      # angular_velocity_stdev: 0.59 deg/s (0.01 rad/s) gyroscope white noise sigma, according to the manual
+      static_covariance_angular_velocity:
+        [1.0e-4, 0.0, 0.0, 0.0, 1.0e-4, 0.0, 0.0, 0.0, 1.0e-4]
+      # linear_acceleration_stdev: 2.8 mg (0.0275 m/s^2) accelerometer white noise sigma, according to the manual
+      static_covariance_linear_acceleration:
+        [7.6e-4, 0.0, 0.0, 0.0, 7.6e-4, 0.0, 0.0, 0.0, 7.6e-4]
+
   drive_controller:
     ros__parameters:
       left_wheel_names: [fl_wheel_joint, rl_wheel_joint]