Kalman Filter

README.md

@@ -32,7 +32,7 @@ git clone git@github.com:memristor/mep3.git src/mep3
 
 # Install dependencies
 sudo apt install python3-vcstool
-vcs import src < src/mep3/mep3.repos
+vcs import --recursive src < src/mep3/mep3.repos
 rosdep update
 rosdep install --from-paths src --ignore-src -r
 

docker/Dockerfile.base

@@ -23,6 +23,7 @@ RUN apt-get update && apt-get install -y \
         ros-humble-laser-filters \
         ros-humble-domain-bridge \
         ros-humble-rmw-cyclonedds-cpp \
+        ros-humble-robot-localization \
         ros-humble-ros2-control \
         ros-humble-ros2-controllers \
         ros-humble-rqt-common-plugins \

docker/README.md

@@ -7,12 +7,16 @@
     sudo apt install git make curl
     curl -sSL https://get.docker.com | sh && sudo usermod -aG docker $USER
     ```
+    > NOTE: If you have an NVIDIA graphics card, install drivers. Tutorial is [here](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/install-guide.html).
+
     and reboot the PC.
+
 2) Clone this repository
     ```sh
     # Make sure to have your SSH keys added to GitHub
     git clone git@github.com:memristor/mep3.git
     ```
+
 3) Run provisioning script to build and run the container
    ```sh
    cd ./mep3/docker

mep3_bringup/launch/robot_launch.py

@@ -256,4 +256,4 @@ def generate_launch_description():
         nav2,
         tf_base_link_laser,
         driver,
-    ] + on_exit_events + get_initial_pose_transform(namespace, color))
+    ] + on_exit_events)

mep3_bringup/resource/default.rviz

@@ -10,13 +10,13 @@ Panels:
         - /TF1
         - /TF1/Frames1
         - /TF1/Tree1
-        - /LaserScan2
         - /Global Planner1/Global Costmap1
         - /Controller1/Local Costmap1
         - /Controller1/Local Plan1
         - /MarkerArray1
+        - /PoseWithCovariance1
       Splitter Ratio: 0.5833333134651184
-    Tree Height: 546
+    Tree Height: 551
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -357,6 +357,41 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: waypoints
       Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz_default_plugins/PoseWithCovariance
+      Color: 170; 170; 0
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Head Length: 0.10000000149011612
+      Head Radius: 0.10000000149011612
+      Name: PoseWithCovariance
+      Shaft Length: 0.20000000298023224
+      Shaft Radius: 0.019999999552965164
+      Shape: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/aruco_5
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -394,7 +429,7 @@ Visualization Manager:
   Value: true
   Views:
     Current:
-      Angle: 0
+      Angle: -1.5707000494003296
       Class: rviz_default_plugins/TopDownOrtho
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
@@ -407,8 +442,8 @@ Visualization Manager:
       Scale: 267.49932861328125
       Target Frame: <Fixed Frame>
       Value: TopDownOrtho (rviz_default_plugins)
-      X: 0.005600222386419773
-      Y: -0.10876844823360443
+      X: 0
+      Y: 0
     Saved: ~
 Window Geometry:
   Displays:
@@ -418,13 +453,13 @@ Window Geometry:
   Hide Right Dock: true
   Navigation 2:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd00000004000000000000016b0000039efc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000025f000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002a2000001390000013900fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003e20000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000016b000003a2fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000262000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb00000018004e0061007600690067006100740069006f006e0020003201000002a30000013a0000012300fffffffb0000001e005200650061006c00730065006e0073006500430061006d00650072006100000002c6000000c10000000000000000000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003e2000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Tool Properties:
     collapsed: false
   Views:
     collapsed: true
   Width: 1363
-  X: 2846
-  Y: 340
+  X: 557
+  Y: 27

mep3_hardware/resource/big_controllers.yaml

@@ -41,3 +41,5 @@ big:
       angular.z.max_acceleration: 25.0
 
       use_stamped_vel: false
+
+      twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.01]