added visual odom and refactored nav2 odom

diff --git a/twil_2dnav/config/twil_2dnav.rviz b/twil_2dnav/config/twil_2dnav.rviz
index b36eec3..f3f66c1 100644 (file)
--- a/twil_2dnav/config/twil_2dnav.rviz
+++ b/twil_2dnav/config/twil_2dnav.rviz
@@ -5,10 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Ground Truth1/Topic1
-        - /Odometry1
         - /Global Plan1/Topic1
         - /Camera Color1/Visibility1
-        - /LaserScan1
       Splitter Ratio: 0.570588231086731
     Tree Height: 189
   - Class: rviz_common/Selection
@@ -135,21 +133,6 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
-        front_caster_base:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_caster_support:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
-        front_caster_wheel:
-          Alpha: 1
-          Show Axes: false
-          Show Trail: false
-          Value: true
         imu:
           Alpha: 1
           Show Axes: false
@@ -319,7 +302,7 @@ Visualization Manager:
         Value: true
       Enabled: true
       Keep: 42
-      Name: Odometry
+      Name: Ekf Odom
       Position Tolerance: 0.30000001192092896
       Shape:
         Alpha: 1
@@ -339,6 +322,45 @@ Visualization Manager:
         Reliability Policy: Reliable
         Value: /odometry/filtered
       Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz_default_plugins/Odometry
+      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
+      Keep: 100
+      Name: Visual Odom
+      Position Tolerance: 0.10000000149011612
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 255; 25; 0
+        Head Length: 0.30000001192092896
+        Head Radius: 0.10000000149011612
+        Shaft Length: 1
+        Shaft Radius: 0.05000000074505806
+        Value: Arrow
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        Filter size: 10
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /rtabmap/icp_viodom
+      Value: true
     - Alpha: 0.699999988079071
       Class: rviz_default_plugins/Map
       Color Scheme: map
@@ -529,6 +551,7 @@ Visualization Manager:
       Value: true
       Visibility:
         Camera Depth: false
+        Ekf Odom: false
         Global Costmap: false
         Global Plan: false
         Grid: true
@@ -537,7 +560,6 @@ Visualization Manager:
         Local Costmap: false
         Local Plan: false
         Map: true
-        Odometry: false
         PointCloud2: false
         Pose Reference: false
         Received Global Plan: false
@@ -545,6 +567,7 @@ Visualization Manager:
         TF: true
         Transformed Global Plan: false
         Value: true
+        Visual Odom: true
       Zoom Factor: 1
     - Class: rviz_default_plugins/Camera
       Enabled: true
@@ -561,6 +584,7 @@ Visualization Manager:
       Value: true
       Visibility:
         Camera Color: false
+        Ekf Odom: false
         Global Costmap: false
         Global Plan: false
         Grid: true
@@ -569,7 +593,6 @@ Visualization Manager:
         Local Costmap: false
         Local Plan: false
         Map: true
-        Odometry: false
         PointCloud2: true
         Pose Reference: false
         Received Global Plan: false
@@ -577,6 +600,7 @@ Visualization Manager:
         TF: true
         Transformed Global Plan: false
         Value: true
+        Visual Odom: true
       Zoom Factor: 1
     - Alpha: 1
       Autocompute Intensity Bounds: true

diff --git a/twil_2dnav/launch/gazebo.launch.xml b/twil_2dnav/launch/gazebo.launch.xml
index 2edc592..88299ce 100644 (file)
--- a/twil_2dnav/launch/gazebo.launch.xml
+++ b/twil_2dnav/launch/gazebo.launch.xml
@@ -27,8 +27,9 @@
        <arg name="gps" default="false"/>
        <arg name="world" default ="$(find-pkg-share ufrgs_gazebo)/worlds/centenario.world"/>
        <arg name="controller" default="twist_mrac_linearizing_controller"/>
+       <arg name="odom" default="rtabmap_viodom"/>
        <arg name="localization" default="static"/>
-       <arg name="config" default="$(find-pkg-share twil_2dnav)/config/$(var controller)_no_tf.yaml"/>
+       <arg name="controller_config" default="$(find-pkg-share twil_2dnav)/config/$(var controller)_no_tf.yaml"/>
 
        <include file="$(find-pkg-share twil_bringup)/launch/gazebo.launch.xml">
                <arg name="pause" value="$(var pause)"/>
@@ -39,13 +40,14 @@
                <arg name="gps" value="$(var gps)"/>
                <arg name="world" value="$(var world)"/>
                <arg name="controller" value="$(var controller)"/>
-               <arg name="config" value="$(var config)"/>
+               <arg name="config" value="$(var controller_config)"/>
        </include>
 
        <include file="$(find-pkg-share twil_2dnav)/launch/nav2_navigator.launch.xml">
                <arg name="use_sim_time" value="$(var use_sim_time)"/>
                <arg name="controller" value="$(var controller)"/>
         <arg name="localization" value="$(var localization)"/>
+        <arg name="odom" value="$(var odom)"/>
        </include>
 
        <node name="rviz" pkg="rviz2" exec="rviz2" args="-d $(find-pkg-share twil_2dnav)/config/twil_2dnav.rviz">

diff --git a/twil_2dnav/launch/localization_slam_toolbox_mapping.launch.xml b/twil_2dnav/launch/localization_slam_toolbox.launch.xml
similarity index 100%
rename from twil_2dnav/launch/localization_slam_toolbox_mapping.launch.xml
rename to twil_2dnav/launch/localization_slam_toolbox.launch.xml

diff --git a/twil_2dnav/launch/nav2_navigator.launch.xml b/twil_2dnav/launch/nav2_navigator.launch.xml
index 98f2543..addee59 100644 (file)
--- a/twil_2dnav/launch/nav2_navigator.launch.xml
+++ b/twil_2dnav/launch/nav2_navigator.launch.xml
@@ -24,7 +24,8 @@
     <arg name="map" default="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
     <arg name="autostart" default="true"/>
     <arg name="controller" default=""/>
-    <arg name="localization" default=""/>
+    <arg name="localization" default="static"/>
+    <arg name="odom" default="rtabmap_viodom"/>
 
     <node pkg="nav2_controller" exec="controller_server" name="controller_server">
         <param name="use_sim_time" value="$(var use_sim_time)"/>
@@ -55,14 +56,12 @@
         <param name="yaml_filename" value="$(find-pkg-share ufrgs_maps)/map/centenario1.yaml"/>
     </node>
 
-    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
-        <param name="use_sim_time" value="$(var use_sim_time)"/>
-        <param name="odom0" value="/fake_odom"/>
-        <param from="$(find-pkg-share twil_2dnav)/config/ekf_params.yaml"/>
-    </node>
-
        <include file="$(find-pkg-share twil_2dnav)/launch/localization_$(var localization).launch.xml">
         <arg name="use_sim_time" value="$(var use_sim_time)"/>
        </include>
 
+       <include file="$(find-pkg-share twil_2dnav)/launch/odom_$(var odom).launch.xml">
+        <arg name="controller" value="$(var controller)"/>
+       </include>
+
 </launch>

diff --git a/twil_2dnav/launch/odom_controller.launch.xml b/twil_2dnav/launch/odom_controller.launch.xml
new file mode 100644 (file)
index 0000000..6b4e6aa
--- /dev/null
+++ b/twil_2dnav/launch/odom_controller.launch.xml
@@ -0,0 +1,9 @@
+<launch>
+    <arg name="controller" default=""/>
+
+    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
+        <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_params.yaml"/>
+    </node>
+</launch>
\ No newline at end of file

diff --git a/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml b/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml
new file mode 100644 (file)
index 0000000..c45e09b
--- /dev/null
+++ b/twil_2dnav/launch/odom_noisy_ground_truth.launch.xml
@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="robot_localization" exec="ekf_node" name="ekf_filter_node">
+        <param name="use_sim_time" value="$(var use_sim_time)"/>
+        <param name="odom0" value="/fake_odom"/>
+        <param from="$(find-pkg-share twil_2dnav)/config/ekf_params.yaml"/>
+    </node>
+</launch>

diff --git a/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
new file mode 100644 (file)
index 0000000..6e9f7b7
--- /dev/null
+++ b/twil_2dnav/launch/odom_rtabmap_viodom.launch.xml
@@ -0,0 +1,26 @@
+
+<launch>
+    <node pkg="rtabmap_odom" exec="rgbd_odometry" name="rgbd_odometry" output="screen">
+        <param name="frame_id" value="twil_origin"/>
+        <param name="odom_frame_id" value="odom"/>
+        <param name="scan_normal_k" value="10"/>
+        <param name="initial_pose" value="0 0 0 0 0 0"/>
+        <param name="subscribe_scan_cloud" value="true"/>
+        <param name="publish_tf" value="true"/>
+
+        <remap from="imu" to="/sensor/imu" />
+        <remap from="scan_cloud" to="/camera/depth/points"/>
+        <remap from="depth/image" to="/camera/depth/image_raw"/>
+        <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
+        <remap from="rgb/image" to="/camera/color/image_raw"/>
+
+        <remap from="odom" to="/rtabmap/icp_viodom" />
+        <!-- <param name="scan_normal_k"   type="int" value="10"/>
+        <param name="subscribe_scan_cloud" type="bool" value="true"/>
+
+        <param name="Icp/PointToPlane"  type="string" value="true"/>
+        <param name="Icp/VoxelSize"     type="string" value="0"/>
+        <param name="Icp/PM"            type="string" value="$(arg pm)"/>
+        <param name="Icp/PMOutlierRatio" type="string" value="0.65"/> -->
+    </node>
+</launch>
\ No newline at end of file