added robot_localization and fused imu and odom data

diff --git a/twil_2dnav/config/bt_navigator_params.yaml b/twil_2dnav/config/bt_navigator_params.yaml
index 296168b..06d7d8c 100644 (file)
--- a/twil_2dnav/config/bt_navigator_params.yaml
+++ b/twil_2dnav/config/bt_navigator_params.yaml
@@ -2,7 +2,7 @@ bt_navigator:
   ros__parameters:
     global_frame: map
     robot_base_frame: twil_origin
-    odom_topic: /twist_mrac_linearizing_controller/odom
+    odom_topic: /odometry/filtered
     bt_loop_duration: 10
     default_server_timeout: 20
     enable_groot_monitoring: True

diff --git a/twil_2dnav/config/ekf.yaml b/twil_2dnav/config/ekf.yaml
new file mode 100644 (file)
index 0000000..ed79a56
--- /dev/null
+++ b/twil_2dnav/config/ekf.yaml
@@ -0,0 +1,47 @@
+### ekf config file ###
+ekf_filter_node:
+    ros__parameters:
+# The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+# computation until it receives at least one message from one of theinputs. It will then run continuously at the
+# frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+# ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+# set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+# environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+# by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+# Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: true
+
+# Whether to broadcast the transformation over the /tf topic. Defaultsto true if unspecified.
+        publish_tf: false
+
+# 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+#     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of odom_frame.
+# 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set "world_frame"
+#    to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+# 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates from landmark
+#    observations) then:
+#     3a. Set your "world_frame" to your map_frame value
+#     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state estimation node
+#         from robot_localization! However, that instance should *not* fuse the global data.
+        map_frame: map              # Defaults to "map" if unspecified
+        odom_frame: odom            # Defaults to "odom" if unspecified
+        base_link_frame: twil_origin  # Defaults to "base_link" ifunspecified
+        world_frame: odom           # Defaults to the value ofodom_frame if unspecified
+
+        #odom0 topic is set on launch file
+        odom0_config: [true, true, false,
+                       false, false, false,
+                       false, false, false,
+                       false, false, true,
+                       false, false, false]
+
+        imu0: demo/imu
+        imu0_config: [false, false, false,
+                      false, false, true,
+                      false, false, false,
+                      false, false, true,
+                      true, false, false]
\ No newline at end of file

diff --git a/twil_2dnav/config/twil_2dnav.rviz b/twil_2dnav/config/twil_2dnav.rviz
index 9fedbe4..2e9a6e3 100644 (file)
--- a/twil_2dnav/config/twil_2dnav.rviz
+++ b/twil_2dnav/config/twil_2dnav.rviz
@@ -5,10 +5,11 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Ground Truth1/Topic1
+        - /Odometry1
         - /Global Plan1/Topic1
         - /Camera Color1/Visibility1
       Splitter Ratio: 0.570588231086731
-    Tree Height: 149
+    Tree Height: 189
   - Class: rviz_common/Selection
     Name: Selection
   - Class: rviz_common/Tool Properties
@@ -26,7 +27,7 @@ Panels:
     Experimental: false
     Name: Time
     SyncMode: 0
-    SyncSource: PointCloud2
+    SyncSource: ""
 Visualization Manager:
   Class: ""
   Displays:
@@ -148,6 +149,11 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        imu:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         left_wheel:
           Alpha: 1
           Show Axes: false
@@ -330,7 +336,7 @@ Visualization Manager:
         Filter size: 10
         History Policy: Keep Last
         Reliability Policy: Reliable
-        Value: /twist_mrac_linearizing_controller/odom
+        Value: /odometry/filtered
       Value: true
     - Alpha: 0.699999988079071
       Class: rviz_default_plugins/Map
@@ -676,10 +682,10 @@ Window Geometry:
     collapsed: false
   Displays:
     collapsed: false
-  Height: 895
+  Height: 987
   Hide Left Dock: false
   Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd000000040000000000000156000002e1fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000118000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800430061006d006500720061002000440065007000740068010000015b000000d90000002800fffffffb0000001800430061006d00650072006100200043006f006c006f0072010000023a000000e40000002800ffffff000000010000010f000002e1fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002e1000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000026300fffffffb0000000800540069006d006501000000000000045000000000000000000000028f000002e100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000341fc020000000afb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000013e000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000001800430061006d006500720061002000440065007000740068010000017f000000f50000002800fffffffb0000001800430061006d00650072006100200043006f006c006f0072010000027a000001020000002800ffffff000000010000010f00000341fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000341000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007740000003efc0100000002fb0000000800540069006d00650100000000000007740000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000005030000034100000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Selection:
     collapsed: false
   Time:
@@ -688,6 +694,6 @@ Window Geometry:
     collapsed: false
   Views:
     collapsed: false
-  Width: 1280
-  X: 640
+  Width: 1908
+  X: 0
   Y: 0

diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index 669a664..6e67166 100644 (file)
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -35,7 +35,7 @@
     <node pkg="nav2_controller" exec="controller_server" name="controller_server">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
         <param from="$(find-pkg-share twil_2dnav)/config/controller_params.yaml"/>
-        <remap from="odom" to="$(var controller)/odom"/>
+        <remap from="odom" to="odometry/filtered"/>
         <remap from="cmd_vel" to="$(var controller)/command"/>
     </node>
 
@@ -69,4 +69,10 @@
                        'bt_navigator']"/>
     </node>
 
+    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node" output="screen">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="odom0" value="$(var controller)/odom"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/ekf.yaml"/>
+    </node>
+
 </launch>

diff --git a/twil_2dnav/package.xml b/twil_2dnav/package.xml
index 7fceca3..326d8ae 100644 (file)
--- a/twil_2dnav/package.xml
+++ b/twil_2dnav/package.xml
@@ -13,6 +13,7 @@
   <depend>twil_bringup</depend>
   <depend>twil_description</depend>
   <depend>navigation2</depend>
+  <exec_depend>robot_localization</exec_depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>