diff --git a/stretch_core/README.md b/stretch_core/README.md
index a19b234..043ee54 100644
--- a/stretch_core/README.md
+++ b/stretch_core/README.md
@@ -8,7 +8,7 @@
 
 *detect_aruco_markers* : node that detects and estimates the pose of ArUco markers, including the markers on the robot's body
 
-*d435i_** : various nodes to help use the Stretch RE1's 3D camera
+*d435i_* : various nodes to help use the Stretch RE1's 3D camera
 
 *keyboard_teleop* : node that provides a keyboard interface to control the robot's joints
 
diff --git a/stretch_core/launch/d435i_basic.launch b/stretch_core/launch/d435i_basic.launch
index e8846f4..de50e5c 100644
--- a/stretch_core/launch/d435i_basic.launch
+++ b/stretch_core/launch/d435i_basic.launch
@@ -8,48 +8,38 @@
 
   <!-- REALSENSE D435i -->
   <include file="$(find realsense2_camera)/launch/rs_camera.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="enable_color" value="true"/>
-    <arg name="enable_depth" 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"/>
+    <arg name="enable_confidence" value="false"/>
+    <arg name="enable_infra1"     value="false"/>
+    <arg name="enable_infra2"     value="false"/>
+    <arg name="enable_infra"      value="false"/>
+    <arg name="enable_color"      value="true"/>
+    <arg name="enable_depth"      value="true"/>
+    <arg name="enable_accel"      value="true"/>
+    <arg name="enable_gyro"       value="true"/>
+
+    <arg name="color_fps"         value="15"/>
+    <arg name="depth_fps"         value="15"/>
+    <arg name="accel_fps"         value="63"/>
+    <arg name="gyro_fps"          value="200"/>
+
+    <arg name="color_width"       value="$(arg color_width)"/>
+    <arg name="color_height"      value="$(arg color_height)"/>
+    <arg name="depth_width"       value="$(arg depth_width)"/>
+    <arg name="depth_height"      value="$(arg depth_height)"/>
 
     <!-- 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 -->
+    <arg name="enable_pointcloud" value="true"/>
 
-    <!-- "enable_sync: gathers closest frames of different sensors,
+    <!-- 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 -->
+         pointcloud are enabled. -->
     <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
@@ -57,13 +47,10 @@
          colors set to 0,0,0. -->
     <arg name="allow_no_texture_points" value="true"/>
 
-    <!-- "initial_reset: On occasions the device was not closed
+    <!-- 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 -->
+         true, the device will reset prior to usage. -->
     <arg name="initial_reset" value="$(arg rs_initial_reset)"/>
-    
-    <!--<arg name="unite_imu_method" value="copy"/> linear_interpolation-->
 
     <!-- configure the depth image to the high accuracy visual preset -->
     <arg name="json_file_path" value="$(arg rs_initial_preset)" />
diff --git a/stretch_core/launch/d435i_high_resolution.launch b/stretch_core/launch/d435i_high_resolution.launch
index ef22242..15792c2 100644
--- a/stretch_core/launch/d435i_high_resolution.launch
+++ b/stretch_core/launch/d435i_high_resolution.launch
@@ -1,16 +1,14 @@
 <launch>
-  
-  <!-- REALSENSE D435i -->
+
+  <!-- D435i Base -->
   <include file="$(find stretch_core)/launch/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"/>
+    <!-- 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_width"  value="1280"/>
     <arg name="color_height" value="720"/>
   </include>
-  
+
 </launch>
diff --git a/stretch_core/launch/d435i_low_resolution.launch b/stretch_core/launch/d435i_low_resolution.launch
index 495f739..64e3108 100644
--- a/stretch_core/launch/d435i_low_resolution.launch
+++ b/stretch_core/launch/d435i_low_resolution.launch
@@ -1,17 +1,14 @@
 <launch>
 
-  <!-- REALSENSE D435i -->
+  <!-- D435i Base -->
   <include file="$(find stretch_core)/launch/d435i_basic.launch" pass_all_args="true">
-    <!--
-	LOWEST RESOLUTION, but also has the lowest minimum depth
-	(105mm 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="424"/>
+    <!-- LOWEST RESOLUTION, but also has the lowest minimum depth
+         (105mm 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="424"/>
     <arg name="depth_height" value="240"/>
-    <arg name="color_width" value="424"/>
+    <arg name="color_width"  value="424"/>
     <arg name="color_height" value="240"/>
-    
   </include>
 
 </launch>
diff --git a/stretch_core/launch/stretch.launch b/stretch_core/launch/stretch.launch
new file mode 100644
index 0000000..aa8f097
--- /dev/null
+++ b/stretch_core/launch/stretch.launch
@@ -0,0 +1,26 @@
+<launch>
+
+  <arg name="lidar_odom" default="true"  doc="whether the odom TF is estimated with lidar odometry fused, or just wheel odometry" />
+  <arg name="respeaker"  default="true"  doc="whether to launch the respeaker microphone array/speaker drivers" />
+  <arg name="rviz"       default="false" doc="whether to show Rviz" />
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true" unless="$(arg lidar_odom)" />
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true" />
+
+  <!-- D435i CAMERA -->
+  <include file="$(find stretch_core)/launch/stretch_realsense.launch" pass_all_args="true" />
+
+  <!-- LASER RANGE FINDER + SCAN MATCHER ODOMETRY -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" if="$(arg lidar_odom)" />
+  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" if="$(arg lidar_odom)" />
+
+  <!-- RESPEAKER MICROPHONE ARRAY -->
+  <group ns="stretch">
+    <include file="$(find respeaker_ros)/launch/respeaker.launch" if="$(arg respeaker)" />
+  </group>
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_core)/rviz/stretch.rviz" if="$(arg rviz)" />
+
+</launch>
diff --git a/stretch_core/launch/stretch_driver.launch b/stretch_core/launch/stretch_driver.launch
index a0ef9b0..cc80f7a 100644
--- a/stretch_core/launch/stretch_driver.launch
+++ b/stretch_core/launch/stretch_driver.launch
@@ -41,7 +41,9 @@
     <param name="controller_calibration_file" type="string" value="$(arg calibrated_controller_yaml_file)"/>
   </node>
 
+  <!--
   <node name="aggregator_node" pkg="diagnostic_aggregator" type="aggregator_node">
     <rosparam command="load" file="$(find stretch_core)/config/diagnostics.yaml" />
   </node>
+  -->
 </launch>
diff --git a/stretch_core/launch/stretch_realsense.launch b/stretch_core/launch/stretch_realsense.launch
new file mode 100644
index 0000000..fed1fb5
--- /dev/null
+++ b/stretch_core/launch/stretch_realsense.launch
@@ -0,0 +1,24 @@
+<launch>
+
+  <arg name="resolution"          default="high"  doc="resolution of the color/depth images ('low' or 'high')" />
+  <arg name="publish_upright_img" default="false" doc="whether to publish a rotated upright color image" />
+  <arg name="publish_frustum_viz" default="false" doc="whether to publish frustum field of view visualizers" />
+
+  <!-- D435i DRIVER -->
+  <include file="$(find stretch_core)/launch/d435i_$(arg resolution)_resolution.launch" />
+
+  <!-- VISUAL PRESET CONFIGURATION -->
+  <node name="d435i_configure" pkg="stretch_core" type="d435i_configure" output="screen" />
+
+  <!-- UPRIGHT ROTATED CAMERA VIEW -->
+  <node name="upright_rotater" pkg="image_rotate" type="image_rotate" if="$(arg publish_upright_img)">
+    <remap from="image" to="/camera/color/image_raw" />
+    <remap from="rotated/image" to="/camera/color/upright_image_raw" />
+    <param name="target_frame_id" type="str" value="" />
+    <param name="target_x" type="double" value="-1.5708" />
+  </node>
+
+  <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
+  <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" if="$(arg publish_frustum_viz)" />
+
+</launch>
diff --git a/stretch_core/nodes/d435i_accel_correction b/stretch_core/nodes/d435i_accel_correction
index 618312d..2bb6d5c 100755
--- a/stretch_core/nodes/d435i_accel_correction
+++ b/stretch_core/nodes/d435i_accel_correction
@@ -8,11 +8,11 @@ class D435iAccelCorrectionNode:
     def __init__(self):
         self.num_samples_to_skip = 4
         self.sample_count = 0
-        
+
     def accel_callback(self, accel):
         self.accel = accel
         self.sample_count += 1
-        if (self.sample_count % self.num_samples_to_skip) == 0: 
+        if (self.sample_count % self.num_samples_to_skip) == 0:
             # This can avoid issues with the D435i's time stamps being too
             # far ahead for TF.
             self.accel.header.stamp = rospy.Time.now()
@@ -26,7 +26,7 @@ class D435iAccelCorrectionNode:
 
             self.corrected_accel_pub.publish(self.accel)
 
-            
+
     def main(self):
         rospy.init_node('D435iAccelCorrectionNode')
         self.node_name = rospy.get_name()
@@ -34,7 +34,7 @@ class D435iAccelCorrectionNode:
 
         self.topic_name = '/camera/accel/sample'
         self.accel_subscriber = rospy.Subscriber(self.topic_name, Imu, self.accel_callback)
-        
+
         self.corrected_accel_pub = rospy.Publisher('/camera/accel/sample_corrected', Imu, queue_size=1)
 
 
diff --git a/stretch_core/nodes/d435i_configure b/stretch_core/nodes/d435i_configure
index 9c478b4..63f4b8a 100755
--- a/stretch_core/nodes/d435i_configure
+++ b/stretch_core/nodes/d435i_configure
@@ -10,21 +10,21 @@ class D435iConfigureNode:
         self.rate = 1.0
         self.visual_preset = None
         self.mode_lock = threading.Lock()
-        
+
     def turn_on_default_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 1
             self.locked_mode_name = 'Default'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def turn_on_high_accuracy_mode(self):
-        with self.mode_lock: 
+        with self.mode_lock:
             self.locked_mode_id = 3
             self.locked_mode_name = 'High Accuracy'
             self.parameter_client.update_configuration({'visual_preset' : self.locked_mode_name})
             rospy.loginfo("Set D435i to {0} mode".format(self.locked_mode_name))
-        
+
     def default_mode_service_callback(self, request):
         self.turn_on_default_mode()
         return TriggerResponse(
@@ -58,9 +58,9 @@ class D435iConfigureNode:
         while not rospy.is_shutdown():
             rate.sleep()
 
-            
+
 if __name__ == '__main__':
-    try: 
+    try:
         node = D435iConfigureNode()
         node.main()
     except KeyboardInterrupt:
diff --git a/stretch_core/nodes/d435i_frustum_visualizer b/stretch_core/nodes/d435i_frustum_visualizer
index 132226d..3b00a21 100755
--- a/stretch_core/nodes/d435i_frustum_visualizer
+++ b/stretch_core/nodes/d435i_frustum_visualizer
@@ -10,7 +10,7 @@ from visualization_msgs.msg import MarkerArray
 from geometry_msgs.msg import Transform, Pose, Vector3, Quaternion, Point
 from sensor_msgs.msg import CameraInfo
 
-    
+
 class FrustumNode:
     def __init__(self):
         self.color_count = 0
@@ -18,8 +18,8 @@ class FrustumNode:
         self.camera_types = ['depth', 'color']
         # only publish a single frustum for every N camera info
         # messages received
-        self.skip_publishing = 5 
-        
+        self.skip_publishing = 5
+
         # minimum-Z depth for D435i
         # 1280x720 0.28  m
         # 848x480  0.195 m
@@ -43,12 +43,12 @@ class FrustumNode:
         if (self.depth_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='depth')
         self.depth_count = self.depth_count + 1
-        
+
     def color_camera_info_callback(self, camera_info):
         if (self.color_count % self.skip_publishing) == 0:
             self.camera_info_callback(camera_info, camera_type='color')
         self.color_count = self.color_count + 1
-        
+
     def camera_info_callback(self, camera_info, camera_type=None):
         if camera_type not in self.camera_types:
             print('WARNING: FrustumNode camera_info_callback camera_type = {0} unrecognized. Valid types = {1}.'.format(camera_type, self.camera_types))
@@ -101,28 +101,28 @@ class FrustumNode:
         marker.type = marker.TRIANGLE_LIST
         marker.action = marker.ADD
         marker.lifetime = rospy.Duration(0.5) #0.2
-        marker.text = 'D435i_frustum'
         marker.header.frame_id = frame_id
         marker.header.stamp = timestamp
         marker.id = 0
         marker.scale.x = 1.0
         marker.scale.y = 1.0
         marker.scale.z = 1.0
-        if camera_type == 'depth': 
+        marker.pose.orientation.w = 1.0
+        if camera_type == 'depth':
             # frustum color and transparency
             # gray
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 1.0
             marker.color.a = 0.4
-        elif camera_type == 'color': 
+        elif camera_type == 'color':
             # frustum color and transparency
             # yellow
             marker.color.r = 1.0
             marker.color.g = 1.0
             marker.color.b = 0.0
             marker.color.a = 0.4
-            
+
         def adjacent_corners(c0, c1):
             # return True if the corners are adjacent to one another and False otherwise
             faces0 = c0[1]
@@ -170,23 +170,23 @@ class FrustumNode:
 
         points = []
         quad_ids = ['near', 'far', 'top', 'bottom', 'left', 'right']
-        for s in quad_ids: 
+        for s in quad_ids:
             quad_xyz = corners_to_quad(frustum_corners_xyz, s)
             triangles = quad_to_triangles(quad_xyz)
             points.extend(triangles)
         marker.points = points
-        
+
         if camera_type == 'depth':
             self.depth_frustum_marker_pub.publish(marker)
         elif camera_type == 'color':
             self.color_frustum_marker_pub.publish(marker)
 
-        
+
     def main(self):
         rospy.init_node('FrustumNode')
         self.node_name = rospy.get_name()
         rospy.loginfo("{0} started".format(self.node_name))
-        
+
         self.color_camera_topic = '/camera/color/camera_info'
         self.depth_camera_topic = '/camera/depth/camera_info'
         self.depth_camera_info_subscriber = rospy.Subscriber(self.depth_camera_topic, CameraInfo, self.depth_camera_info_callback)
diff --git a/stretch_core/nodes/stretch_driver b/stretch_core/nodes/stretch_driver
index 99e504f..f7681d5 100755
--- a/stretch_core/nodes/stretch_driver
+++ b/stretch_core/nodes/stretch_driver
@@ -543,7 +543,7 @@ class StretchDriverNode:
         self.last_twist_time = rospy.get_time()
 
         # start action server for joint trajectories
-        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', True)
+        self.fail_out_of_range_goal = rospy.get_param('~fail_out_of_range_goal', False)
         self.joint_trajectory_action = JointTrajectoryAction(self)
         self.joint_trajectory_action.server.start()
         self.diagnostics = StretchDiagnostics(self, self.robot)
diff --git a/stretch_core/rviz/stretch.rviz b/stretch_core/rviz/stretch.rviz
new file mode 100644
index 0000000..0fa3581
--- /dev/null
+++ b/stretch_core/rviz/stretch.rviz
@@ -0,0 +1,582 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /RobotModel1/Status1
+        - /TF1/Frames1
+      Splitter Ratio: 0.6321839094161987
+    Tree Height: 539
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: PointCloud2
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_straight_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_pitch:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_roll:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw_bottom:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Alpha: 1
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
+    - Class: rviz/Marker
+      Enabled: false
+      Marker Topic: /frustum_marker/color_camera
+      Name: Color Camera Frustum
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Class: rviz/Marker
+      Enabled: false
+      Marker Topic: /frustum_marker/depth_camera
+      Name: Depth Camera Frustum
+      Namespaces:
+        {}
+      Queue Size: 100
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 1
+      Size (m): 0.009999999776482582
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
+      Enabled: false
+      History Length: 1
+      Name: Camera IMU
+      Queue Size: 10
+      Topic: /camera/accel/sample_corrected
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan_filtered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /camera/color/image_raw
+      Name: RGB Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: compressed
+      Unreliable: false
+      Value: true
+      Visibility:
+        Camera IMU: true
+        Color Camera Frustum: true
+        Depth Camera: true
+        Depth Camera Frustum: true
+        Effort (Revolute Joints Only): true
+        Grid: true
+        LaserScan: true
+        Mobile Base IMU: true
+        Odometry: true
+        PointCloud2: true
+        RobotModel: true
+        TF: true
+        Value: true
+        Wrist Accelerometer: true
+      Zoom Factor: 1
+    - Class: rviz/Camera
+      Enabled: true
+      Image Rendering: background and overlay
+      Image Topic: /camera/aligned_depth_to_color/image_raw
+      Name: Depth Camera
+      Overlay Alpha: 0.5
+      Queue Size: 2
+      Transport Hint: compressedDepth
+      Unreliable: false
+      Value: true
+      Visibility:
+        Camera IMU: true
+        Color Camera Frustum: true
+        Depth Camera Frustum: true
+        Effort (Revolute Joints Only): true
+        Grid: true
+        LaserScan: true
+        Mobile Base IMU: true
+        Odometry: true
+        PointCloud2: true
+        RGB Camera: true
+        RobotModel: true
+        TF: true
+        Value: true
+        Wrist Accelerometer: true
+      Zoom Factor: 1
+    - Alpha: 1
+      Class: rviz/Effort
+      Enabled: false
+      History Length: 1
+      Joints:
+        joint_gripper_finger_left:
+          Value: true
+        joint_gripper_finger_right:
+          Value: true
+        joint_head_pan:
+          Value: true
+        joint_head_tilt:
+          Value: true
+        joint_wrist_pitch:
+          Value: true
+        joint_wrist_roll:
+          Value: true
+        joint_wrist_yaw:
+          Value: true
+      Name: Effort (Revolute Joints Only)
+      Queue Size: 10
+      Robot Description: robot_description
+      Scale: 1
+      TF Prefix: ""
+      Topic: /joint_states
+      Unreliable: false
+      Value: false
+      Width: 0.019999999552965164
+    - Alpha: 1
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
+      Enabled: false
+      History Length: 1
+      Name: Mobile Base IMU
+      Queue Size: 10
+      Topic: /imu_mobile_base
+      Unreliable: false
+      Value: false
+    - Alpha: 1
+      Class: rviz_plugin_tutorials/Imu
+      Color: 204; 51; 204
+      Enabled: false
+      History Length: 1
+      Name: Wrist Accelerometer
+      Queue Size: 10
+      Topic: /imu_wrist
+      Unreliable: false
+      Value: false
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/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: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 252; 233; 79
+        Head Length: 0.05000000074505806
+        Head Radius: 0.05000000074505806
+        Shaft Length: 0.10000000149011612
+        Shaft Radius: 0.019999999552965164
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 10
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7853981852531433
+      Target Frame: <Fixed Frame>
+      Yaw: 0.7853981852531433
+    Saved: ~
+Window Geometry:
+  Depth Camera:
+    collapsed: false
+  Displays:
+    collapsed: false
+  Height: 836
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 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
+  RGB Camera:
+    collapsed: false
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1600
+  X: 0
+  Y: 27
diff --git a/stretch_demos/launch/grasp_object.launch b/stretch_demos/launch/grasp_object.launch
index e2fa9b9..c621c5f 100644
--- a/stretch_demos/launch/grasp_object.launch
+++ b/stretch_demos/launch/grasp_object.launch
@@ -1,47 +1,27 @@
 <launch>
 
-  <arg name="debug_directory" default="$(env HELLO_FLEET_PATH)/debug/"/>
-  <arg name="dryrun" default="false" />
-  <arg name="rviz" default="true" />
-
-  <!-- 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>
-  <!-- -->
+  <arg name="launch_drivers"  default="true"                           doc="whether to launch Stretch, D435i, Lidar, and other drivers" />
+  <arg name="debug_directory" default="$(env HELLO_FLEET_PATH)/debug/" doc="path to directory where debug planning images/logs are stored" />
+  <arg name="dryrun"          default="false"                          doc="whether run perception/planning without actually moving" />
+  <arg name="rviz"            default="true"                           doc="whether to launch Rviz" />
+  <arg name="respeaker"       value="false" />
 
-  <!-- STRETCH DRIVER -->
-  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="false"/>
-  <param name="/stretch_driver/fail_out_of_range_goal" type="bool" value="false"/>
-  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
-  <!-- -->
+  <!-- STRETCH DRIVERS -->
+  <include file="$(find stretch_core)/launch/stretch.launch" if="$(arg launch_drivers)" pass_all_args="true" />
 
   <!-- MAPPING -->
   <node name="funmap" pkg="stretch_funmap" type="funmap" output="screen" >
     <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
   </node>
-  <!-- -->
-
-  <!-- LASER RANGE FINDER -->
-  <include file="$(find stretch_core)/launch/rplidar.launch" />
-  <!-- -->
-
-  <!-- LASER SCAN MATCHER FOR ODOMETRY -->
-  <include file="$(find stretch_core)/launch/stretch_scan_matcher.launch" />
-  <!-- -->
 
   <!-- ALIGNED GRIPPER LINK -->
   <node name="aligned_gripper_link_tf_publisher" pkg="tf" type="static_transform_publisher" args="0 0 0 -0.5 0.4999968 0.5 0.5000032 /link_straight_gripper /link_straight_gripper_aligned 100" />
-  <!-- -->
 
   <!-- GRASP OBJECT -->
   <node name="grasp_object" pkg="stretch_demos" type="grasp_object" output="screen">
     <param name="debug_directory" type="string" value="$(arg debug_directory)"/>
     <param name="dryrun" type="bool" value="$(arg dryrun)"/>
   </node>
-  <!-- -->
 
   <!-- KEYBOARD TELEOP -->
   <node name="keyboard_teleop" pkg="stretch_core" type="keyboard_teleop" output="screen" args='--mapping_on --grasp_object_on'/>
diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index f479e09..0cfba9a 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/stretch_gazebo/launch/gazebo.launch
@@ -11,6 +11,7 @@
   <arg name="world" default="worlds/empty.world"/>
   <arg name="dex_wrist" default="false"/>
   <param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
+  <arg name="publish_upright_img" default="false" doc="whether to pub rotated upright color image" />
 
   <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
   <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
@@ -39,7 +40,7 @@
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/>
 
   <rosparam command="load"
-            file="$(find stretch_gazebo)/config/joints.yaml" 
+            file="$(find stretch_gazebo)/config/joints.yaml"
             ns="stretch_joint_state_controller" />
 
   <rosparam command="load"
@@ -68,4 +69,12 @@
 
   <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
 
+  <!-- UPRIGHT ROTATED CAMERA VIEW -->
+  <node name="upright_rotater" pkg="image_rotate" type="image_rotate" if="$(arg publish_upright_img)">
+    <remap from="image" to="/camera/color/image_raw" />
+    <remap from="rotated/image" to="/camera/color/upright_image_raw" />
+    <param name="target_frame_id" type="str" value="" />
+    <param name="target_x" type="double" value="-1.5708" />
+  </node>
+
 </launch>