Removed ekf filter

stretch_rtabmap/launch/robot.launch

@@ -1,11 +1,17 @@
 <launch>
     <!-- REALSENSE D435i -->
-    <include file="$(find stretch_core)/launch/d435i_high_resolution.launch"></include>
-    <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
-    <!--<param name="initial_mode" type="string" value="Default"/>-->
-    <param name="initial_mode" type="string" value="High Accuracy"/>
-    </node>
-    <!-- -->
+  <include file="$(find stretch_rtabmap)/launch/robot/d435i_basic.launch" pass_all_args="true">
+    <arg name="depth_width" value="640"/>
+    <arg name="depth_height" value="480"/>
+    <arg name="color_width" value="640"/>
+    <arg name="color_height" value="480"/>
+  </include>
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen">
+    <param name="initial_mode" type="string" value="Default"/>
+    <!-- <param name="initial_mode" type="string" value="Medium Density"/> -->
+    <!-- <param name="initial_mode" type="string" value="High Accuracy"/> -->
+  </node>
+  <!-- -->
 
     <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
     <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
@@ -31,6 +37,6 @@
     <!-- -->
 
     <!-- ROBOT LOCALIZATION FILTER -->
-    <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
+    <!-- <include file="$(find stretch_core)/launch/stretch_ekf.launch" /> -->
     <!-- -->
-</launch>
+</launch>

stretch_rtabmap/launch/robot/d435i_basic.launch

@@ -0,0 +1,64 @@
+<launch>
+
+  <!-- REDUCE RATE AND USE NUC TIME -->
+  <node name="d435i_accel_correction" pkg="stretch_core" type="d435i_accel_correction" output="screen" />
+
+  <!-- REALSENSE D435i -->
+  <include file="$(find realsense2_camera)/launch/rs_rgbd.launch">
+    <!-- "The D435i depth camera generates and transmits the gyro and
+         accelerometer samples independently, as the inertial sensors
+         exhibit different FPS rates (200/400Hz for gyro, 63/250Hz for
+         accelerometer)."
+         https://realsense.intel.com/how-to-getting-imu-data-from-d435i-and-t265/
+         https://github.com/intel-ros/realsense
+         -->
+    <arg name="accel_fps" value="63"/>
+    <arg name="gyro_fps" value="200"/>
+    <arg name="depth_fps" value="15"/>
+    <arg name="enable_infra1" value="false"/>
+    <arg name="enable_infra2" value="false"/>
+    <arg name="enable_accel" value="true"/>
+
+    <arg name="depth_width" value="$(arg depth_width)"/>
+    <arg name="depth_height" value="$(arg depth_height)"/>
+    <arg name="color_width" value="$(arg color_width)"/>
+    <arg name="color_height" value="$(arg color_height)"/>
+  
+    <arg name="color_fps" value="15"/>
+
+    <!-- publish depth streams aligned to other streams -->
+    <arg name="align_depth" value="true"/>
+
+    <!-- publish an RGBD point cloud -->
+    <arg name="filters" value="pointcloud"/>
+    
+    <!-- "tf_prefix: By default all frame's ids have the same prefix -
+         camera_. This allows changing it per camera."
+         https://github.com/intel-ros/realsense -->
+
+    <!-- "enable_sync: gathers closest frames of different sensors,
+         infra red, color and depth, to be sent with the same
+         timetag. This happens automatically when such filters as
+         pointcloud are enabled."
+         https://github.com/intel-ros/realsense -->
+    <arg name="enable_sync" value="true"/>
+    
+    <!-- "You can have a full depth to pointcloud, coloring the
+         regions beyond the texture with zeros..." -->
+    <!-- Set to true in order to make use of the full field of view of
+         the depth image instead of being restricted to the field of
+         view associated with the narrower RGB camera. Note that
+         points out of the RGB camera's field of view will have their
+         colors set to 0,0,0. -->
+    <!-- <arg name="allow_no_texture_points" value="true"/> -->
+
+    <!-- "initial_reset: On occasions the device was not closed
+         properly and due to firmware issues needs to reset. If set to
+         true, the device will reset prior to usage."
+         https://github.com/intel-ros/realsense -->
+    <!--<arg name="initial_reset" value="true"/>-->
+    
+    <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
+  </include>
+
+</launch>

stretch_rtabmap/launch/robot/d435i_high_resolution.launch

@@ -0,0 +1,16 @@
+<launch>
+  
+  <!-- REALSENSE D435i -->
+  <include file="$(find stretch_rtabmap)/launch/robot/d435i_basic.launch" pass_all_args="true">
+    <!--
+	HIGHEST RESOLUTION, but also has the highest minimum depth
+	(280mm Min-Z) below which objects generate bad noise, such as
+	when the arm and gripper are raised close to the camera.
+    -->
+    <arg name="depth_width" value="1280"/>
+    <arg name="depth_height" value="720"/>
+    <arg name="color_width" value="1280"/>
+    <arg name="color_height" value="720"/>
+  </include>
+  
+</launch>