diff --git a/.github/workflows/crates.yml b/.github/workflows/crates.yml
index 0764c7b..e7f4b28 100644
--- a/.github/workflows/crates.yml
+++ b/.github/workflows/crates.yml
@@ -19,4 +19,4 @@ jobs:
         uses: actions-rs/cargo@v1
         with:
           command: publish
-          args: --token ${{ secrets.CRATES_IO_TOKEN }}
\ No newline at end of file
+          args: --token ${{ secrets.CRATES_IO_TOKEN }}
diff --git a/.github/workflows/r2r_galactic.yml b/.github/workflows/r2r_galactic.yml
index 45a609f..eac5370 100644
--- a/.github/workflows/r2r_galactic.yml
+++ b/.github/workflows/r2r_galactic.yml
@@ -16,6 +16,6 @@ jobs:
   tests_galactic:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v2
-    - run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic
-    - run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon
+      - uses: actions/checkout@v2
+      - run: docker build . --file ./tests/Dockerfile_r2r_galactic --tag r2r_galactic
+      - run: docker run r2r_galactic cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_humble.yml b/.github/workflows/r2r_humble.yml
index a88752c..137f0fb 100644
--- a/.github/workflows/r2r_humble.yml
+++ b/.github/workflows/r2r_humble.yml
@@ -16,6 +16,6 @@ jobs:
   tests_humble:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v2
-    - run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble
-    - run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon
+      - uses: actions/checkout@v2
+      - run: docker build . --file ./tests/Dockerfile_r2r_humble --tag r2r_humble
+      - run: docker run r2r_humble cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/r2r_iron.yml b/.github/workflows/r2r_iron.yml
index ef3ab9d..7745166 100644
--- a/.github/workflows/r2r_iron.yml
+++ b/.github/workflows/r2r_iron.yml
@@ -16,6 +16,6 @@ jobs:
   tests_humble:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v2
-    - run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron
-    - run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon
+      - uses: actions/checkout@v2
+      - run: docker build . --file ./tests/Dockerfile_r2r_iron --tag r2r_iron
+      - run: docker run r2r_iron cargo test --features r2r_msg,derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_humble.yml b/.github/workflows/rclrs_humble.yml
index 1c0f88b..0103664 100644
--- a/.github/workflows/rclrs_humble.yml
+++ b/.github/workflows/rclrs_humble.yml
@@ -16,6 +16,6 @@ jobs:
   tests_humble:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v2
-    - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble
-    - run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon
+      - uses: actions/checkout@v2
+      - run: docker build . --file ./tests/Dockerfile_rclrs_humble --tag rclrs_humble
+      - run: docker run rclrs_humble cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rclrs_iron.yml b/.github/workflows/rclrs_iron.yml
index 390419b..cdd8526 100644
--- a/.github/workflows/rclrs_iron.yml
+++ b/.github/workflows/rclrs_iron.yml
@@ -16,6 +16,6 @@ jobs:
   tests_iron:
     runs-on: ubuntu-latest
     steps:
-    - uses: actions/checkout@v2
-    - run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron
-    - run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon
+      - uses: actions/checkout@v2
+      - run: docker build . --file ./tests/Dockerfile_rclrs_iron --tag rclrs_iron
+      - run: docker run rclrs_iron cargo test --features derive,nalgebra,rayon
diff --git a/.github/workflows/rosrust_noetic.yml b/.github/workflows/rosrust_noetic.yml
index 40a5c12..bbf6403 100644
--- a/.github/workflows/rosrust_noetic.yml
+++ b/.github/workflows/rosrust_noetic.yml
@@ -13,11 +13,11 @@ jobs:
   build:
     runs-on: ubuntu-20.04
     env:
-      ROS_CI_DESKTOP: "`lsb_release -cs`"  # e.g. [trusty|xenial|...]
+      ROS_CI_DESKTOP: "`lsb_release -cs`" # e.g. [trusty|xenial|...]
       # CI_SOURCE_PATH: $(pwd)
       ROSINSTALL_FILE: $CI_SOURCE_PATH/dependencies.rosinstall
       CATKIN_OPTIONS: $CI_SOURCE_PATH/catkin.options
-      ROS_PARALLEL_JOBS: '-j8 -l6'
+      ROS_PARALLEL_JOBS: "-j8 -l6"
       # Set the python path manually to include /usr/-/python2.7/dist-packages
       # as this is where apt-get installs python packages.
       PYTHONPATH: $PYTHONPATH:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages
@@ -27,81 +27,81 @@ jobs:
         uses: actions/checkout@v1
       - name: Install latest rust
         run: |
-            curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
-            rustc --version
-            cargo --version
+          curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
+          rustc --version
+          cargo --version
       - name: Configure ROS for install
         run: |
-            sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
-            sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
-            sudo apt-get update -qq
-            sudo apt-get install dpkg
-            sudo apt-get install -y libyaml-cpp-dev
+          sudo sh -c "echo \"deb http://packages.ros.org/ros/ubuntu $ROS_CI_DESKTOP main\" > /etc/apt/sources.list.d/ros-latest.list"
+          sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654
+          sudo apt-get update -qq
+          sudo apt-get install dpkg
+          sudo apt-get install -y libyaml-cpp-dev
       - name: Install ROS basic packages
         run: |
-            sudo apt-get install -y python3-catkin-pkg
-            sudo apt-get install -y python3-catkin-tools
-            sudo apt-get install -y python3-rosdep
-            sudo apt-get install -y python3-wstool
-            sudo apt-get install -y python3-osrf-pycommon
-            sudo apt-get install -y ros-cmake-modules
-            sudo apt-get install -y ros-$ROS_DISTRO-ros-base
-            source /opt/ros/$ROS_DISTRO/setup.bash
-            sudo rosdep init
-            rosdep update  # --include-eol-distros  # Support EOL distros.
-      - name: Install ROS additional packages (TODO maybe don't need these)
+          sudo apt-get install -y python3-catkin-pkg
+          sudo apt-get install -y python3-catkin-tools
+          sudo apt-get install -y python3-rosdep
+          sudo apt-get install -y python3-wstool
+          sudo apt-get install -y python3-osrf-pycommon
+          sudo apt-get install -y ros-cmake-modules
+          sudo apt-get install -y ros-$ROS_DISTRO-ros-base
+          source /opt/ros/$ROS_DISTRO/setup.bash
+          sudo rosdep init
+          rosdep update  # --include-eol-distros  # Support EOL distros.
+      - name: Install ROS additional packages
         # Does installing these mean rosrust_msg builds more messages, which is a better
         # check?  Or does it just slow down the action?
         run: |
-            sudo apt-get install -y ros-$ROS_DISTRO-actionlib
-            sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager
-            sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport
-            sudo apt-get install -y ros-$ROS_DISTRO-catkin
-            sudo apt-get install -y ros-$ROS_DISTRO-class-loader
-            sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules
-            sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge
-            sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure
-            sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure
-            # Not in noetic yet
-            # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python
-            sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions
-            sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-genmsg
-            sudo apt-get install -y ros-$ROS_DISTRO-image-geometry
-            sudo apt-get install -y ros-$ROS_DISTRO-image-proc
-            sudo apt-get install -y ros-$ROS_DISTRO-image-transport
-            sudo apt-get install -y ros-$ROS_DISTRO-message-generation
-            sudo apt-get install -y ros-$ROS_DISTRO-message-runtime
-            # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core
-            # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools
-            # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions
-            # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros
-            sudo apt-get install -y ros-$ROS_DISTRO-pluginlib
-            sudo apt-get install -y ros-$ROS_DISTRO-roscpp
-            sudo apt-get install -y ros-$ROS_DISTRO-roslib
-            sudo apt-get install -y ros-$ROS_DISTRO-roslint
-            sudo apt-get install -y ros-$ROS_DISTRO-rospy
-            sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter
-            sudo apt-get install -y ros-$ROS_DISTRO-rostest
-            sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-std-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-tf
-            sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions
-            sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs
-            sudo apt-get install -y ros-$ROS_DISTRO-tf2-py
-            sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros
-            sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-camera-info-manager
+          sudo apt-get install -y ros-$ROS_DISTRO-compressed-image-transport
+          sudo apt-get install -y ros-$ROS_DISTRO-catkin
+          sudo apt-get install -y ros-$ROS_DISTRO-class-loader
+          sudo apt-get install -y ros-$ROS_DISTRO-cmake-modules
+          sudo apt-get install -y ros-$ROS_DISTRO-cv-bridge
+          sudo apt-get install -y ros-$ROS_DISTRO-dynamic-reconfigure
+          sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure
+          # Not in noetic yet
+          # sudo apt-get install -y ros-$ROS_DISTRO-ddynamic-reconfigure-python
+          sudo apt-get install -y ros-$ROS_DISTRO-eigen-conversions
+          sudo apt-get install -y ros-$ROS_DISTRO-geometry-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-genmsg
+          sudo apt-get install -y ros-$ROS_DISTRO-image-geometry
+          sudo apt-get install -y ros-$ROS_DISTRO-image-proc
+          sudo apt-get install -y ros-$ROS_DISTRO-image-transport
+          sudo apt-get install -y ros-$ROS_DISTRO-message-generation
+          sudo apt-get install -y ros-$ROS_DISTRO-message-runtime
+          # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-core
+          # sudo apt-get install -y ros-$ROS_DISTRO-nodelet-topic-tools
+          # sudo apt-get install -y ros-$ROS_DISTRO-pcl-conversions
+          # sudo apt-get install -y ros-$ROS_DISTRO-pcl-ros
+          sudo apt-get install -y ros-$ROS_DISTRO-pluginlib
+          sudo apt-get install -y ros-$ROS_DISTRO-roscpp
+          sudo apt-get install -y ros-$ROS_DISTRO-roslib
+          sudo apt-get install -y ros-$ROS_DISTRO-roslint
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy-message-converter
+          sudo apt-get install -y ros-$ROS_DISTRO-rostest
+          sudo apt-get install -y ros-$ROS_DISTRO-sensor-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-std-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf
+          sudo apt-get install -y ros-$ROS_DISTRO-tf-conversions
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-geometry-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-msgs
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-py
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-ros
+          sudo apt-get install -y ros-$ROS_DISTRO-tf2-sensor-msgs
       - name: build
         run: |
           source /opt/ros/$ROS_DISTRO/setup.bash
           cargo build  # --verbose
       - name: Install ROS packages for tests
         run: |
-            sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials
-            sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials
-            sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials
+          sudo apt-get install -y ros-$ROS_DISTRO-actionlib-tutorials
+          sudo apt-get install -y ros-$ROS_DISTRO-roscpp-tutorials
+          sudo apt-get install -y ros-$ROS_DISTRO-rospy-tutorials
       - name: test
         run: |
           source /opt/ros/$ROS_DISTRO/setup.bash
diff --git a/.github/workflows/tests.yml b/.github/workflows/tests.yml
index 04c5cee..c15a23f 100644
--- a/.github/workflows/tests.yml
+++ b/.github/workflows/tests.yml
@@ -19,9 +19,9 @@ jobs:
       - uses: actions/checkout@v2
       - name: Install latest Rust
         run: |
-            curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
-            rustc --version
-            cargo --version
+          curl --proto '=https' --tlsv1.2 https://sh.rustup.rs -sSf | sh -s -- -y
+          rustc --version
+          cargo --version
       - name: Linting
         run: cargo clippy --all-targets -- -D warnings
       - name: Tests