From fa4b78df64cc59b627a2c0ecf26e778b3c264ee8 Mon Sep 17 00:00:00 2001
From: Ubuntu User <ubuntu@zeus.attlocal.net>
Date: Tue, 17 Dec 2019 06:10:19 +0000
Subject: [PATCH 1/4] Adding support for nav2D flat nav. Adding some existing
 ROS params to the move_basic.launch for better visibility from launch file

---
 .../launch/simple_navigation_flat.launch      | 23 +++++++++++++
 magni_nav/launch/aruco.launch                 | 11 +++++--
 magni_nav/launch/aruco_navigation_flat.launch | 32 +++++++++++++++++++
 magni_nav/launch/move_basic.launch            |  8 +++++
 4 files changed, 72 insertions(+), 2 deletions(-)
 create mode 100644 magni_demos/launch/simple_navigation_flat.launch
 create mode 100644 magni_nav/launch/aruco_navigation_flat.launch

diff --git a/magni_demos/launch/simple_navigation_flat.launch b/magni_demos/launch/simple_navigation_flat.launch
new file mode 100644
index 00000000..f7405ace
--- /dev/null
+++ b/magni_demos/launch/simple_navigation_flat.launch
@@ -0,0 +1,23 @@
+<!-- 
+    This launch file brings up a simple navigation system using fiducials [1]
+    and move_basic [2].
+
+    [1] http://wiki.ros.org/fiducials
+    [2] http://wiki.ros.org/move_basic
+-->
+<launch>
+    <!-- 
+        This parameter is used to specify what level of capabilites
+        the robot should have after this launch file.
+
+        Possible values are 'core', 'teleop', and 'navigation'
+
+        Since this launch brings up enough to do navigation, 
+        we set it to 'navigation'.
+    -->
+    <param name="ubiquity_robot_mode" type="string" value="navigation"/> 
+
+    <include file="$(find magni_nav)/launch/move_basic.launch" />
+    <include file="$(find magni_nav)/launch/aruco_navigation_flat.launch" />
+</launch>
+
diff --git a/magni_nav/launch/aruco.launch b/magni_nav/launch/aruco.launch
index f9af2a5f..fda009a6 100644
--- a/magni_nav/launch/aruco.launch
+++ b/magni_nav/launch/aruco.launch
@@ -3,6 +3,8 @@
 -->
 <launch>
   <arg name="mapping_mode" default="false"/>
+  <arg name="fiducial_len" default="0.14"/>
+  <arg name="verbose_info" default="false"/>
 
   <!-- camera -->
   <include file="$(find raspicam_node)/launch/camerav2_1280x960_10fps.launch" />
@@ -12,9 +14,10 @@
     <arg name="camera" value="/raspicam_node"/>
     <arg name="image" value="image"/>
     <arg name="transport" value="compressed"/>
-    
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+
     <!-- fiducial_len -->
-    <arg name="fiducial_len" value="0.140" />
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
   </include>
 
   <!-- Fiducial slam -->
@@ -23,5 +26,9 @@
     <arg name="odom_frame" value="odom"/>
     <arg name="base_frame" value="base_footprint"/>
     <arg name="future_date_transforms" value="0.5"/>
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+
+    <!-- fiducial_len -->
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
   </include>
 </launch>
diff --git a/magni_nav/launch/aruco_navigation_flat.launch b/magni_nav/launch/aruco_navigation_flat.launch
new file mode 100644
index 00000000..1e79031e
--- /dev/null
+++ b/magni_nav/launch/aruco_navigation_flat.launch
@@ -0,0 +1,32 @@
+<!--
+   Run the 3d fiducial localization for Nav2D flat floor navigation method
+   This navigation method treats the map.txt file as read only so cannot grow fiducial map.
+-->
+<launch>
+  <arg name="mapping_mode" default="false"/>
+  <arg name="fiducial_len" default="0.14"/>
+  <arg name="verbose_info" default="true"/>
+
+  <!-- camera -->
+  <include file="$(find raspicam_node)/launch/camerav2_1280x960_10fps.launch" />
+
+  <!-- Fiducial detection -->
+  <include file="$(find aruco_detect)/launch/aruco_detect.launch">
+    <arg name="camera" value="/raspicam_node"/>
+    <arg name="image" value="image"/>
+    <arg name="transport" value="compressed"/>
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+  </include>
+
+  <!-- Fiducial Navigation For Flat Floor -->
+  <include file="$(find fiducial_slam)/launch/fiducial_slam.launch">
+    <arg name="map_frame" value="map"/>
+    <arg name="odom_frame" value="odom"/>
+    <arg name="base_frame" value="base_link"/>
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+    <arg name="read_only_map" value="true"/>
+    <arg name="navigate_flat" value="true"/>
+  </include>
+</launch>
diff --git a/magni_nav/launch/move_basic.launch b/magni_nav/launch/move_basic.launch
index 3ef60109..f7ab72ac 100644
--- a/magni_nav/launch/move_basic.launch
+++ b/magni_nav/launch/move_basic.launch
@@ -5,6 +5,14 @@
         <param name="robot_front_length" value="0.1"/>
         <param name="robot_back_length" value="0.32"/>
 
+	<!-- speed   controls -->
+        <param name="max_linear_velocity" value="0.5"/>
+        <param name="max_angular_velocity" value="1.0"/>
+
+	<!-- End position tolerance -->
+        <param name="linear_tolerance" value="0.03"/>
+        <param name="angular_tolerance" value="0.01"/>
+
 	<!-- lateral control -->
         <param name="min_side_dist" value="0.3"/>
         <param name="max_side_dist" value="2"/>

From 8a2057b09ce89771f30986b9a39d8835586d9b30 Mon Sep 17 00:00:00 2001
From: Mark Johnston <mjstn2011@gmail.com>
Date: Thu, 9 Jan 2020 07:38:31 +0000
Subject: [PATCH 2/4] Changing the upward pose to proper translations

---
 magni_description/urdf/magni.urdf.xacro | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/magni_description/urdf/magni.urdf.xacro b/magni_description/urdf/magni.urdf.xacro
index d9b97bca..78b9e30c 100755
--- a/magni_description/urdf/magni.urdf.xacro
+++ b/magni_description/urdf/magni.urdf.xacro
@@ -176,7 +176,7 @@
     </xacro:if>
     <xacro:if value="${raspicam_mount == 'upward'}">
       <xacro:raspi_camera name="raspicam" connected_to="base_link">
-        <origin xyz="0.035 0.15 0.14" rpy="0 0 ${pi}"/>
+        <origin xyz="0.020 0.115 0.16" rpy="0 0 ${pi}"/>
       </xacro:raspi_camera>
     </xacro:if>
   </xacro:if>

From cd3d693c6269b151c8a51577a18c369727e619ec Mon Sep 17 00:00:00 2001
From: Mark Johston <mjstn2011@gmail.com>
Date: Mon, 20 Apr 2020 21:39:20 +0000
Subject: [PATCH 3/4] Taking the flat nav simplification one level deeper to
 allow maps to also be made by forcing ceiling fiducial poses to no roll or
 pitch thus avoiding the noise we have not been able to fix in other ways to
 date.   A launch param of  fiducials_flat set to true to enable this in
 mapping mode

---
 magni_demos/launch/simple_nav_map_flat.launch | 24 ++++++++++++++
 magni_nav/launch/aruco_nav_map_flat.launch    | 33 +++++++++++++++++++
 2 files changed, 57 insertions(+)
 create mode 100644 magni_demos/launch/simple_nav_map_flat.launch
 create mode 100644 magni_nav/launch/aruco_nav_map_flat.launch

diff --git a/magni_demos/launch/simple_nav_map_flat.launch b/magni_demos/launch/simple_nav_map_flat.launch
new file mode 100644
index 00000000..528fce36
--- /dev/null
+++ b/magni_demos/launch/simple_nav_map_flat.launch
@@ -0,0 +1,24 @@
+<!-- 
+    This launch file brings up a simple navigation system using fiducials [1]
+    and move_basic [2].
+    A special mode that removes roll and pitch from fiducials used for mapping is enabled
+
+    [1] http://wiki.ros.org/fiducials
+    [2] http://wiki.ros.org/move_basic
+-->
+<launch>
+    <!-- 
+        This parameter is used to specify what level of capabilites
+        the robot should have after this launch file.
+
+        Possible values are 'core', 'teleop', and 'navigation'
+
+        Since this launch brings up enough to do navigation, 
+        we set it to 'navigation'.
+    -->
+    <param name="ubiquity_robot_mode" type="string" value="navigation"/> 
+
+    <include file="$(find magni_nav)/launch/move_basic.launch" />
+    <include file="$(find magni_nav)/launch/aruco_nav_map_flat.launch" />
+</launch>
+
diff --git a/magni_nav/launch/aruco_nav_map_flat.launch b/magni_nav/launch/aruco_nav_map_flat.launch
new file mode 100644
index 00000000..fc2dbd45
--- /dev/null
+++ b/magni_nav/launch/aruco_nav_map_flat.launch
@@ -0,0 +1,33 @@
+<!--
+   Run the 3d fiducial localization for Nav2D flat floor navigation method
+   A special mode that removes roll and pitch from fiducials used for mapping is enabled
+-->
+<launch>
+  <arg name="mapping_mode" default="false"/>
+  <arg name="fiducial_len" default="0.11"/>
+  <arg name="verbose_info" default="true"/>
+
+  <!-- camera -->
+  <include file="$(find raspicam_node)/launch/arducam2p3mm_1296x972_10fps.launch" />
+
+  <!-- Fiducial detection -->
+  <include file="$(find aruco_detect)/launch/aruco_detect.launch">
+    <arg name="camera" value="/raspicam_node"/>
+    <arg name="image" value="image"/>
+    <arg name="transport" value="compressed"/>
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+  </include>
+
+  <!-- Fiducial Navigation For Flat Floor -->
+  <include file="$(find fiducial_slam)/launch/fiducial_slam.launch">
+    <arg name="map_frame" value="map"/>
+    <arg name="odom_frame" value="odom"/>
+    <arg name="base_frame" value="base_link"/>
+    <arg name="fiducial_len" value="$(arg fiducial_len)"/>
+    <arg name="verbose_info" value="$(arg verbose_info)"/>
+    <arg name="read_only_map" value="false"/>
+    <arg name="navigate_flat" value="false"/>
+    <arg name="fiducials_flat" value="true"/>
+  </include>
+</launch>

From 2d9934d71511cc72eb88319dec76595a0e89b8d1 Mon Sep 17 00:00:00 2001
From: Mark Johston <mjstn2011@gmail.com>
Date: Fri, 22 May 2020 09:22:33 +0000
Subject: [PATCH 4/4] Converted launch files to allow command line passing of
 new flat params all the way to fiducial_slam

---
 magni_demos/launch/simple_navigation.launch | 13 ++++++++++++-
 magni_nav/launch/aruco.launch               |  6 ++++++
 2 files changed, 18 insertions(+), 1 deletion(-)

diff --git a/magni_demos/launch/simple_navigation.launch b/magni_demos/launch/simple_navigation.launch
index a8b4b8ae..ff2565d0 100644
--- a/magni_demos/launch/simple_navigation.launch
+++ b/magni_demos/launch/simple_navigation.launch
@@ -6,6 +6,11 @@
     [2] http://wiki.ros.org/move_basic
 -->
 <launch>
+    <arg name="fiducial_len" default="0.140"/>
+    <arg name="read_only_map" default="false"/>
+    <arg name="navigate_flat" default="false"/>
+    <arg name="fiducials_flat" default="false"/>
+    <arg name="verbose_info" default="false"/>
     <!-- 
         This parameter is used to specify what level of capabilites
         the robot should have after this launch file.
@@ -18,6 +23,12 @@
     <param name="ubiquity_robot_mode" type="string" value="navigation"/> 
 
     <include file="$(find magni_nav)/launch/move_basic.launch" />
-    <include file="$(find magni_nav)/launch/aruco.launch" />
+    <include file="$(find magni_nav)/launch/aruco.launch">
+        <arg name="fiducial_len" value="$(arg fiducial_len)" />
+        <arg name="read_only_map" value="$(arg read_only_map)"/>
+        <arg name="navigate_flat" value="$(arg navigate_flat)"/>
+        <arg name="fiducials_flat" value="$(arg fiducials_flat)"/>
+        <arg name="verbose_info" value="$(arg verbose_info)"/>
+    </include>
 </launch>
 
diff --git a/magni_nav/launch/aruco.launch b/magni_nav/launch/aruco.launch
index fda009a6..7ecb970f 100644
--- a/magni_nav/launch/aruco.launch
+++ b/magni_nav/launch/aruco.launch
@@ -4,6 +4,9 @@
 <launch>
   <arg name="mapping_mode" default="false"/>
   <arg name="fiducial_len" default="0.14"/>
+  <arg name="read_only_map" default="false"/>
+  <arg name="navigate_flat" default="false"/>
+  <arg name="fiducials_flat" default="false"/>
   <arg name="verbose_info" default="false"/>
 
   <!-- camera -->
@@ -27,6 +30,9 @@
     <arg name="base_frame" value="base_footprint"/>
     <arg name="future_date_transforms" value="0.5"/>
     <arg name="verbose_info" value="$(arg verbose_info)"/>
+    <arg name="fiducials_flat" value="$(arg fiducials_flat)"/>
+    <arg name="read_only_map" value="$(arg read_only_map)"/>
+    <arg name="navigate_flat" value="$(arg navigate_flat)"/>
 
     <!-- fiducial_len -->
     <arg name="fiducial_len" value="$(arg fiducial_len)"/>