robot orientation (#18)

* first commit

* Kalman estimate node

* some progress

* orientation fixed

* robot localization works as expected

* explain orientation

.devcontainer/Dockerfile

@@ -26,6 +26,7 @@ RUN mkdir -p /home/ws/build /home/ws/install /home/ws/log \
 RUN ( \
     echo "source /opt/ros/iron/setup.bash"; \
     echo "source /home/ws/install/setup.bash"; \
+    echo "source /home/ws/src/openmower/config/sim-config.env.sh"; \
     echo "export DISPLAY=:0"; \
   ) >> /home/$USERNAME/.bashrc
 

.devcontainer/devcontainer.json

@@ -19,5 +19,6 @@
 			]
         }
     },
-    "postCreateCommand": "sudo chown -R openmower /home/ws/ && sudo rosdep update && sudo rosdep install --from-paths src --ignore-src -y"
+    "containerUser": "openmower",
+    "postCreateCommand": "sudo apt update && rosdep update && make custom-deps deps"
 }

.devcontainer/docker-compose.yaml

@@ -21,7 +21,7 @@ services:
       - SYS_PTRACE
     ports:
       - "2222:2222"
-      - "8082:8082" # Foxglove Studio websocket
+      - "8765:8765" # Foxglove Studio websocket
       - "9002:9002" # Gazebo websocket
     volumes:
     - ../:/home/ws

docs/.vitepress/config.mts

@@ -33,6 +33,7 @@ export default withMermaid({
         items: [
           { text: 'ROS workspace', link: '/ros-workspace' },
           { text: 'Mainboard firmware', link: '/omros2-firmware' },
+          { text: 'Robot localization', link: '/localization' },
         ]
       },
       {

docs/clion-env.md

@@ -6,8 +6,10 @@ Devcontainer plugin in CLion is not as good as in VSCode. It's almost entirely b
 An alternative is to use Docker Compose to run the container and configure CLion to use it as a remote toolchain.
 
 ## Prerequisites
+
 - [Docker](https://docs.docker.com/get-docker/)
-- [CLion](https://www.jetbrains.com/clion/download/) + extra [Docker plugin](https://plugins.jetbrains.com/plugin/7724-docker)
+- [CLion](https://www.jetbrains.com/clion/download/) +
+  extra [Docker plugin](https://plugins.jetbrains.com/plugin/7724-docker)
 
 ## Getting Started
 
@@ -17,15 +19,32 @@ An alternative is to use Docker Compose to run the container and configure CLion
     cd .devcontainer/
     docker-compose up -d
     ```
-3. **Configure CLion**.
-      1. Open `Settings` -> `Build, Execution, Deployment` -> `Toolchains`.
-      2. Add new toolchain with remote host `localhost`, port `2222` and remote user `openmower` authenticated with password `openmower`. 
-      3. Inside workspace container run a command that retrieves ROS workspace env vars:
+3. **Setup container**
+   Unlike to VSCode devcontainer support, CLion will not setup container automatically. You have to do it manually.
+    1. **Shell into workspace container**.
+      ```bash
+      docker compose exec workspace bash
+      ```
+    2. **Setup ROS dependencies**.
+      ```bash
+      cd /home/ws && sudo apt update && rosdep update && make custom-deps deps
+      ```
+
+4. **Configure CLion**.
+    1. Open `Settings` -> `Build, Execution, Deployment` -> `Toolchains`.
+    2. Add new toolchain with remote host `localhost`, port `2222` and remote user `openmower` authenticated with
+       password `openmower`.
+    3. Inside workspace container run a command that retrieves ROS workspace env vars:
       ```bash
       docker compose exec workspace /home/ws/envs.sh
       ```
-      4. In `Settings` -> `Build, Execution, Deployment` -> `CMake` pick your new toolchain and fill in `Environment` with env vars from previous step.
-4. **Enjoy your development environment**.
+    4. In `Settings` -> `Build, Execution, Deployment` -> `CMake` pick your new toolchain and fill in `Environment` with
+       env vars from previous step.
+5. **Load targets**
+    1. Open `CMakeLists.txt` and click `Load CMake Project`.
+    2. Make sure every target you want to build is configured with your new toolchain and has the same env vars as for
+       CMake.
+6. **Enjoy your development environment**.
 
 ::: tip
 Please see [detailed information on containers](devcontainer#detailed).

docs/gps.md

@@ -0,0 +1,14 @@
+---
+title: GPS localization
+---
+# {{ $frontmatter.title }}
+
+::: tip
+Work in progress
+:::
+
+## Overview
+
+## RTCM
+
+NTRIP cluster client is force-enabled.

docs/localization.md

@@ -0,0 +1,40 @@
+---
+title: Robot localization
+---
+# {{ $frontmatter.title }}
+
+## Overview
+
+Robot pose is based on an absolute position from GPS and relative readings from wheel odometry and IMU.
+Orientation is not known on startup and defaults to 0. 
+
+As soon as robot starts moving, orientation is assumed based on robot motion and sensors reading.
+It might take a while to get an accurate orientation. Best to start moving in a straight line and do a few circles.
+
+Currently, there is no fallback scenario if had its position changed externally. For example, if you move the robot manually, it will not be able to recover position itself. It will require same procedure as on startup. 
+
+Later on, when undocking behavior is implemented, it will be possible to recover orientation knowing base station position.
+
+## Sensors
+
+[robot_localization documentation](http://docs.ros.org/en/melodic/api/robot_localization/html/integrating_gps.html) nicely describes
+how to get wheel odometry, GPS and IMU sensors data fusion to get an accurate localization.
+
+![Senor data flow](http://docs.ros.org/en/melodic/api/robot_localization/html/_images/navsat_transform_workflow.png)
+
+### Wheel odometry
+
+Default motor controller VESC reports wheel odometry.
+
+### IMU
+
+Accelerometer and gyroscope is required. Magnetometer is not fused.
+
+### GPU
+
+It's expected GPS is RTK capable. Otherwise, localization will be inaccurate.
+More on this in [GPS](gps.md) section.
+
+## Configuration
+
+<<< ../src/openmower/config/robot_localization.yaml{yaml}

docs/simulator.md

@@ -9,14 +9,12 @@ If you are not familiar with Gazebo, please refer to [Gazebo tutorials](http://g
 :::
 
 ::: warning
-This project uses new version of Gazebo formerly known as Ignition Gazebo.
+This project uses Gazebo Fortress. Formerly known as Ignition Fortress.
 :::
 
-## Getting started
+![Robot simulation](assets/gazebo.jpg)
 
-::: info
-Simulator is not yet fully functional. It is still under development.
-:::
+## Getting started
 
 Run the following command to start the simulator:
 
@@ -30,3 +28,19 @@ If run inside [devcontainer](devcontainer), Gazebo GUI will be displayed in a VN
 ::: tip
 Learn more about [VNC client](devcontainer#detailed).
 :::
+
+It's possible to run the simulator with GUI on external host machine. I haven't done it myself since GUI is unstable on MacOS.
+
+## Current state
+
+- :white_check_mark: Robot model
+- :white_check_mark: ros2_control controller
+- :white_check_mark: GPS sensor
+- :white_check_mark: IMU sensor
+- :white_check_mark: can of Coke
+- :construction: Emulate OpenMowerROS2 firmware
+
+## World definition
+
+<<< ../src/openmower/worlds/empty.sdf{xml}
+

src/openmower/CMakeLists.txt

@@ -16,26 +16,6 @@ find_package(ament_cmake REQUIRED)
 # further dependencies manually.
 # find_package(<dependency> REQUIRED)
 
-find_package(rclcpp REQUIRED)
-find_package(std_msgs REQUIRED)
-find_package(sensor_msgs REQUIRED)
-find_package(geographic_msgs REQUIRED)
-find_package(robot_localization REQUIRED)
-
-add_executable(openmower_datum_publisher_node
-        src/datum_publisher/node.cpp
-        src/datum_publisher/node.hpp
-        src/datum_publisher/node_main.cpp
-)
-
-ament_target_dependencies(openmower_datum_publisher_node
-        rclcpp
-        std_msgs
-        sensor_msgs
-        geographic_msgs
-        robot_localization
-)
-
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -53,9 +33,4 @@ INSTALL(
   DESTINATION share/${PROJECT_NAME}
 )
 
-# Installation and export info
-install(TARGETS openmower_datum_publisher_node
-        DESTINATION lib/${PROJECT_NAME}
-)
-
 ament_package()

src/openmower/config/robot_localization.yaml

@@ -1,6 +1,6 @@
 ekf_se_odom:
   ros__parameters:
-    frequency: 10.0
+    frequency: 30.0
     sensor_timeout: 0.1
     two_d_mode: true
     transform_time_offset: 0.0
@@ -20,9 +20,9 @@ ekf_se_odom:
     #ax    , ay    , az
 
     odom0: diff_drive_base_controller/odom
-    odom0_config: [true, true, false,
+    odom0_config: [false, false, false,
                    false, false, false,
-                   true, false, false,
+                   true,  true,  true,
                    false, false, true,
                    false, false, false]
     odom0_queue_size: 10
@@ -32,10 +32,10 @@ ekf_se_odom:
 
     imu0: imu/data
     imu0_config: [false, false, false,
-                  false, false, true,
+                  true,  true,  false,
                   false, false, false,
-                  false, false, true,
-                  true, false, false]
+                  true,  true,  true,
+                  true,  true,  true]
     imu0_nodelay: false
     imu0_differential: false
     imu0_relative: false
@@ -101,7 +101,7 @@ ekf_se_map:
     odom0: diff_drive_base_controller/odom
     odom0_config: [false, false, false,
                    false, false, false,
-                   true, false, false,
+                   true,  true,  true,
                    false, false, true,
                    false, false, false]
     odom0_queue_size: 10
@@ -122,11 +122,11 @@ ekf_se_map:
 
     imu0: imu/data
     imu0_config: [false, false, false,
-                  false, false, true,
+                  true,  true,  false,
                   false, false, false,
-                  false, false, true,
-                  true, false, false]
-    imu0_nodelay: false
+                  true,  true,  true,
+                  true,  true,  true]
+    imu0_nodelay: true
     imu0_differential: false
     imu0_relative: false
     imu0_queue_size: 10
@@ -168,9 +168,9 @@ ekf_se_map:
 
 navsat_transform_node:
   ros__parameters:
-    frequency: 5.0
+    frequency: 30.0
     delay: 3.0
     zero_altitude: true
     publish_filtered_gps: true
-    use_odometry_yaw: true
     wait_for_datum: true
+    use_odometry_yaw: false

src/openmower/config/sim-config.env.sh

@@ -0,0 +1,2 @@
+export OM_DATUM_LAT=-22.9
+export OM_DATUM_LONG=-43.2

src/openmower/description/gps.xacro

@@ -7,14 +7,16 @@
     </joint>
 
     <link name="gps">
-        <sensor name="navsat_sensor" type="navsat">
-          <always_on>1</always_on>
-          <update_rate>5</update_rate>
-          <topic>gps</topic>
-        </sensor>
     </link>
 
     <gazebo reference="gps">
+        <sensor name="gps" type="navsat">
+            <ignition_frame_id>gps</ignition_frame_id>
+            <always_on>1</always_on>
+            <update_rate>5</update_rate>
+            <topic>gps/fix</topic>
+        </sensor>
+
         <material>Gazebo/Black</material>
     </gazebo>
 </robot>

src/openmower/description/imu.xacro

@@ -10,6 +10,15 @@
     </link>
 
     <gazebo reference="imu">
+        <sensor name="imu" type="imu">
+            <ignition_frame_id>imu</ignition_frame_id>
+<!-- not possible to disable orientation via SDF, we will skip it during fusion-->
+            <always_on>1</always_on>
+            <update_rate>50</update_rate>
+            <visualize>true</visualize>
+            <topic>imu/data_raw</topic>
+        </sensor>
+
         <material>Gazebo/Red</material>
     </gazebo>
 </robot>

src/openmower/description/robot_core.xacro

@@ -86,13 +86,6 @@
         <xacro:inertial_box mass="0.5" x="${chassis_length}" y="${chassis_width}" z="${chassis_height}">
             <origin xyz="${chassis_length/2} 0 ${chassis_height/2}" rpy="0 0 0"/>
         </xacro:inertial_box>
-
-        <sensor name="imu_sensor" type="imu">
-          <always_on>1</always_on>
-          <update_rate>30</update_rate>
-          <visualize>true</visualize>
-          <topic>imu</topic>
-        </sensor>
     </link>
 
     <gazebo reference="chassis">

src/openmower/launch/localization.launch.py

@@ -103,7 +103,7 @@ def generate_launch_description():
             output='screen',
             parameters=[localization_params_path, {'use_sim_time': use_sim_time}],
             remappings=[
-                ('/imu/data', '/imu/raw_data'), # remove once imu has orientation
+                ('imu/data', 'imu/data_raw'),
             ],
         ),
 
@@ -114,8 +114,8 @@ def generate_launch_description():
             output='screen',
             parameters=[localization_params_path, {'use_sim_time': use_sim_time}],
             remappings=[
-                ('/imu/data', '/imu/raw_data'), # remove once imu has orientation
-                ('/odometry/filtered', '/odometry/filtered/map'),
+                ('imu/data', 'imu/data_raw'),
+                ('odometry/filtered', 'odometry/filtered/map'),
             ],
         ),
 
@@ -124,20 +124,13 @@ def generate_launch_description():
             executable='navsat_transform_node',
             name='navsat_transform_node',
             output='screen',
-            parameters=[localization_params_path, {'use_sim_time': use_sim_time}],
+            parameters=[localization_params_path, {
+                'use_sim_time': use_sim_time,
+                'datum': [float(os.getenv('OM_DATUM_LAT')), float(os.getenv('OM_DATUM_LONG')), 0.0],
+            }],
             remappings=[
-                ('/odometry/filtered', '/odometry/filtered/map'),
+                ('odometry/filtered', 'odometry/filtered/map'),
+                ('imu', 'gps/orientation'),
             ],
         ),
-
-        Node(
-            package='openmower',
-            executable='openmower_datum_publisher_node',
-            output='screen',
-            parameters=[{
-                'datum.latitude': float(os.getenv('OM_DATUM_LAT')),
-                'datum.longitude': float(os.getenv('OM_DATUM_LONG')),
-                'datum.publish_as_fix': True,
-            }],
-        )
     ])