Merge branch 'feature/rtab' into feature/rtab_pr_merge2

.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/opt/ros/noetic/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}

.vscode/settings.json

@@ -0,0 +1,10 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages",
+        "/home/csrobot/catkin_ws/devel/lib/python3/dist-packages"
+    ],
+    "python.analysis.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages",
+        "/home/csrobot/catkin_ws/devel/lib/python3/dist-packages"
+    ]
+}

frames.gv

@@ -0,0 +1,49 @@
+digraph G {
+"camera_link" -> "camera_accel_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_bottom_screw_frame" -> "camera_link"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_accel_frame" -> "camera_accel_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_color_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_color_frame" -> "camera_color_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_depth_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_depth_frame" -> "camera_depth_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_gyro_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_gyro_frame" -> "camera_gyro_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_infra1_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_infra1_frame" -> "camera_infra1_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_link" -> "camera_infra2_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"camera_infra2_frame" -> "camera_infra2_optical_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_head_tilt" -> "camera_bottom_screw_frame"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_head_pan" -> "link_head_tilt"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"base_link" -> "caster_link"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"odom" -> "base_link"[label=" Broadcaster: /gazebo\nAverage rate: 50.201\nBuffer length: 4.98\nMost recent transform: 187.027\nOldest transform: 182.047\n"];
+"link_lift" -> "link_arm_l4"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_mast" -> "link_lift"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_arm_l0" -> "link_aruco_inner_wrist"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_arm_l1" -> "link_arm_l0"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"base_link" -> "link_aruco_left_base"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "link_aruco_right_base"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_lift" -> "link_aruco_shoulder"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_arm_l0" -> "link_aruco_top_wrist"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_gripper" -> "link_grasp_center"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_wrist_yaw" -> "link_gripper"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_arm_l0" -> "link_wrist_yaw"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_gripper_finger_left" -> "link_gripper_fingertip_left"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_gripper" -> "link_gripper_finger_left"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_gripper_finger_right" -> "link_gripper_fingertip_right"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_gripper" -> "link_gripper_finger_right"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_mast" -> "link_head"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "link_mast"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"base_link" -> "laser"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_mast" -> "respeaker_base"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 10000.0\nBuffer length: 0.0\nMost recent transform: 0.0\nOldest transform: 0.0\n"];
+"link_arm_l2" -> "link_arm_l1"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_arm_l3" -> "link_arm_l2"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_arm_l4" -> "link_arm_l3"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"link_head" -> "link_head_pan"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"base_link" -> "link_left_wheel"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"base_link" -> "link_right_wheel"[label=" Broadcaster: /robot_state_publisher\nAverage rate: 25.202\nBuffer length: 4.96\nMost recent transform: 187.007\nOldest transform: 182.047\n"];
+"map" -> "odom"[label=" Broadcaster: /amcl\nAverage rate: 6.541\nBuffer length: 4.739\nMost recent transform: 186.994\nOldest transform: 182.255\n"];
+edge [style=invis];
+ subgraph cluster_legend { style=bold; color=black; label ="view_frames Result";
+"Recorded at time: 187.044"[ shape=plaintext ] ;
+}->"map";
+}

output.txt

@@ -0,0 +1,277 @@
+... logging to /home/nwright/.ros/log/fdd90a4a-eb0f-11eb-bdd6-2fa0df3c7078/roslaunch-nwright-ubuntu-301125.log
+Checking log directory for disk usage. This may take a while.
+Press Ctrl-C to interrupt
+Done checking log file disk usage. Usage is <1GB.
+]2;/home/nwright/catkin_ws/src/stretch_ros/stretch_navigation/launch/navigation_3d.launch
+started roslaunch server http://nwright-ubuntu:37765/
+
+SUMMARY
+========
+
+PARAMETERS
+ * /amcl/amcl/ros__parameters/alpha1: 0.2
+ * /amcl/amcl/ros__parameters/alpha2: 0.2
+ * /amcl/amcl/ros__parameters/alpha3: 0.2
+ * /amcl/amcl/ros__parameters/alpha4: 0.2
+ * /amcl/amcl/ros__parameters/alpha5: 0.2
+ * /amcl/amcl/ros__parameters/always_reset_initial_pose: False
+ * /amcl/amcl/ros__parameters/base_frame_id: base_link
+ * /amcl/amcl/ros__parameters/beam_skip_distance: 0.5
+ * /amcl/amcl/ros__parameters/beam_skip_error_threshold: 0.9
+ * /amcl/amcl/ros__parameters/beam_skip_threshold: 0.3
+ * /amcl/amcl/ros__parameters/do_beamskip: False
+ * /amcl/amcl/ros__parameters/global_frame_id: map
+ * /amcl/amcl/ros__parameters/lambda_short: 0.1
+ * /amcl/amcl/ros__parameters/laser_likelihood_max_dist: 2.0
+ * /amcl/amcl/ros__parameters/laser_max_range: 100.0
+ * /amcl/amcl/ros__parameters/laser_min_range: -1.0
+ * /amcl/amcl/ros__parameters/laser_model_type: likelihood_field
+ * /amcl/amcl/ros__parameters/map_topic: /map
+ * /amcl/amcl/ros__parameters/max_beams: 60
+ * /amcl/amcl/ros__parameters/max_particles: 2000
+ * /amcl/amcl/ros__parameters/min_particles: 500
+ * /amcl/amcl/ros__parameters/odom_frame_id: odom
+ * /amcl/amcl/ros__parameters/pf_err: 0.05
+ * /amcl/amcl/ros__parameters/pf_z: 0.99
+ * /amcl/amcl/ros__parameters/recovery_alpha_fast: 0.0
+ * /amcl/amcl/ros__parameters/recovery_alpha_slow: 0.0
+ * /amcl/amcl/ros__parameters/resample_interval: 1
+ * /amcl/amcl/ros__parameters/robot_model_type: differential
+ * /amcl/amcl/ros__parameters/save_pose_rate: 0.5
+ * /amcl/amcl/ros__parameters/scan_topic: /scan
+ * /amcl/amcl/ros__parameters/set_initial_pose: False
+ * /amcl/amcl/ros__parameters/sigma_hit: 0.2
+ * /amcl/amcl/ros__parameters/tf_broadcast: True
+ * /amcl/amcl/ros__parameters/transform_tolerance: 1.0
+ * /amcl/amcl/ros__parameters/update_min_a: 0.2
+ * /amcl/amcl/ros__parameters/update_min_d: 0.25
+ * /amcl/amcl/ros__parameters/use_map_topic: True
+ * /amcl/amcl/ros__parameters/z_hit: 0.5
+ * /amcl/amcl/ros__parameters/z_max: 0.05
+ * /amcl/amcl/ros__parameters/z_rand: 0.5
+ * /amcl/amcl/ros__parameters/z_short: 0.05
+ * /move_base/DWAPlannerROS/acc_lim_theta: 4
+ * /move_base/DWAPlannerROS/acc_lim_trans: 0.7
+ * /move_base/DWAPlannerROS/acc_lim_x: 0.7
+ * /move_base/DWAPlannerROS/acc_lim_y: 0
+ * /move_base/DWAPlannerROS/forward_point_distance: 0.3
+ * /move_base/DWAPlannerROS/goal_distance_bias: 60
+ * /move_base/DWAPlannerROS/latch_xy_goal_tolerance: True
+ * /move_base/DWAPlannerROS/max_vel_theta: 1.5
+ * /move_base/DWAPlannerROS/max_vel_trans: 0.6
+ * /move_base/DWAPlannerROS/max_vel_x: 0.6
+ * /move_base/DWAPlannerROS/max_vel_y: 0
+ * /move_base/DWAPlannerROS/min_vel_theta: 0.4
+ * /move_base/DWAPlannerROS/min_vel_trans: 0.1
+ * /move_base/DWAPlannerROS/min_vel_x: 0
+ * /move_base/DWAPlannerROS/min_vel_y: 0
+ * /move_base/DWAPlannerROS/occdist_scale: 1.4
+ * /move_base/DWAPlannerROS/path_distance_bias: 80
+ * /move_base/DWAPlannerROS/prune_plan: True
+ * /move_base/DWAPlannerROS/sim_time: 1.1
+ * /move_base/DWAPlannerROS/vth_samples: 15
+ * /move_base/DWAPlannerROS/vx_samples: 5
+ * /move_base/DWAPlannerROS/vy_samples: 0
+ * /move_base/DWAPlannerROS/xy_goal_tolerance: 0.2
+ * /move_base/DWAPlannerROS/yaw_goal_tolerance: 0.1
+ * /move_base/GlobalPlanner/allow_unknown: True
+ * /move_base/GlobalPlanner/cost_factor: 0.5
+ * /move_base/GlobalPlanner/default_tolerance: 0.2
+ * /move_base/GlobalPlanner/lethal_cost: 253
+ * /move_base/GlobalPlanner/neutral_cost: 66
+ * /move_base/GlobalPlanner/orientation_mode: 0
+ * /move_base/GlobalPlanner/orientation_window_size: 1
+ * /move_base/GlobalPlanner/outline_map: False
+ * /move_base/GlobalPlanner/publish_potential: False
+ * /move_base/TrajectoryPlannerROS/acc_lim_theta: 0.8
+ * /move_base/TrajectoryPlannerROS/acc_lim_x: 0.35
+ * /move_base/TrajectoryPlannerROS/acc_lim_y: 0.0
+ * /move_base/TrajectoryPlannerROS/angular_sim_granularity: 0.025
+ * /move_base/TrajectoryPlannerROS/dwa: False
+ * /move_base/TrajectoryPlannerROS/goal_distance_bias: 0.75
+ * /move_base/TrajectoryPlannerROS/heading_lookahead: 0.325
+ * /move_base/TrajectoryPlannerROS/heading_scoring: True
+ * /move_base/TrajectoryPlannerROS/heading_scoring_timestep: 0.8
+ * /move_base/TrajectoryPlannerROS/holonomic_robot: False
+ * /move_base/TrajectoryPlannerROS/latch_xy_goal_tolerance: True
+ * /move_base/TrajectoryPlannerROS/max_vel_theta: 1.5
+ * /move_base/TrajectoryPlannerROS/max_vel_x: 1.0
+ * /move_base/TrajectoryPlannerROS/max_vel_y: 0.0
+ * /move_base/TrajectoryPlannerROS/meter_scoring: True
+ * /move_base/TrajectoryPlannerROS/min_in_place_vel_theta: 0.3
+ * /move_base/TrajectoryPlannerROS/min_vel_theta: -1.5
+ * /move_base/TrajectoryPlannerROS/min_vel_x: 0.15
+ * /move_base/TrajectoryPlannerROS/min_vel_y: 0.0
+ * /move_base/TrajectoryPlannerROS/occdist_scale: 0.00625
+ * /move_base/TrajectoryPlannerROS/oscillation_reset_dist: 0.05
+ * /move_base/TrajectoryPlannerROS/path_distance_bias: 0.5
+ * /move_base/TrajectoryPlannerROS/publish_cost_grid_pc: False
+ * /move_base/TrajectoryPlannerROS/sim_granularity: 0.025
+ * /move_base/TrajectoryPlannerROS/sim_time: 1.0
+ * /move_base/TrajectoryPlannerROS/vtheta_samples: 10
+ * /move_base/TrajectoryPlannerROS/vx_samples: 3
+ * /move_base/TrajectoryPlannerROS/xy_goal_tolerance: 0.1
+ * /move_base/TrajectoryPlannerROS/yaw_goal_tolerance: 0.1
+ * /move_base/base_global_planner: global_planner/Gl...
+ * /move_base/base_local_planner: dwa_local_planner...
+ * /move_base/conservative_reset_dist: 3.0
+ * /move_base/controller_frequency: 7.0
+ * /move_base/global_costmap/2d_obstacles/laser/clearing: True
+ * /move_base/global_costmap/2d_obstacles/laser/data_type: LaserScan
+ * /move_base/global_costmap/2d_obstacles/laser/expected_update_rate: 0.3
+ * /move_base/global_costmap/2d_obstacles/laser/marking: True
+ * /move_base/global_costmap/2d_obstacles/laser/topic: scan
+ * /move_base/global_costmap/2d_obstacles/laser/track_unknown_space: True
+ * /move_base/global_costmap/2d_obstacles/observation_sources: laser
+ * /move_base/global_costmap/3d_obstacles/combination_method: 1
+ * /move_base/global_costmap/3d_obstacles/enabled: True
+ * /move_base/global_costmap/3d_obstacles/footprint_clearing_enabled: True
+ * /move_base/global_costmap/3d_obstacles/mark_threshold: 0
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/clearing: False
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/data_type: PointCloud2
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/decay_acceleration: 1
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/enabled: True
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/horizontal_fov_angle: 0.942478
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/marking: True
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/max_obstacle_height: -0.2
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/max_z: 0.1
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/min_obstacle_height: -5
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/min_z: -6
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/model_type: 0
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/topic: /camera/depth/col...
+ * /move_base/global_costmap/3d_obstacles/negative_obstacle/vertical_fov_angle: 0.785398
+ * /move_base/global_costmap/3d_obstacles/observation_sources: positive_obstacle...
+ * /move_base/global_costmap/3d_obstacles/origin_z: 0.0
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/clearing: True
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/data_type: PointCloud2
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/decay_acceleration: 1
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/enabled: True
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/horizontal_fov_angle: 0.942478
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/marking: True
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/max_obstacle_height: 1.5
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/max_z: 6
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/min_obstacle_height: 0.05
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/min_z: -0.1
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/model_type: 0
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/topic: /camera/depth/col...
+ * /move_base/global_costmap/3d_obstacles/positive_obstacle/vertical_fov_angle: 0.785398
+ * /move_base/global_costmap/3d_obstacles/publish_voxel_map: True
+ * /move_base/global_costmap/3d_obstacles/unknown_threshold: 15
+ * /move_base/global_costmap/3d_obstacles/z_resolution: 0.05
+ * /move_base/global_costmap/3d_obstacles/z_voxels: 16
+ * /move_base/global_costmap/footprint_padding: 0.0
+ * /move_base/global_costmap/global_frame: map
+ * /move_base/global_costmap/inflation_layer/cost_scaling_factor: 2.5
+ * /move_base/global_costmap/inflation_layer/inflation_radius: 0.2
+ * /move_base/global_costmap/map_type: costmap
+ * /move_base/global_costmap/obstacle_range: 2.5
+ * /move_base/global_costmap/plugins: [{'name': 'static...
+ * /move_base/global_costmap/publish_frequency: 0.5
+ * /move_base/global_costmap/raytrace_range: 3.0
+ * /move_base/global_costmap/robot_base_frame: base_link
+ * /move_base/global_costmap/robot_radius: 0.5
+ * /move_base/global_costmap/rolling_window: True
+ * /move_base/global_costmap/static_map_2d/map_topic: /map
+ * /move_base/global_costmap/static_map_3d_negative/map_topic: /projected_map_ne...
+ * /move_base/global_costmap/static_map_3d_positive/map_topic: /projected_map
+ * /move_base/global_costmap/transform_tolerance: 0.5
+ * /move_base/global_costmap/update_frequency: 10.0
+ * /move_base/local_costmap/2d_obstacles/laser/clearing: True
+ * /move_base/local_costmap/2d_obstacles/laser/data_type: LaserScan
+ * /move_base/local_costmap/2d_obstacles/laser/expected_update_rate: 0.3
+ * /move_base/local_costmap/2d_obstacles/laser/marking: True
+ * /move_base/local_costmap/2d_obstacles/laser/topic: scan
+ * /move_base/local_costmap/2d_obstacles/laser/track_unknown_space: True
+ * /move_base/local_costmap/2d_obstacles/observation_sources: laser
+ * /move_base/local_costmap/3d_obstacles/combination_method: 1
+ * /move_base/local_costmap/3d_obstacles/enabled: True
+ * /move_base/local_costmap/3d_obstacles/footprint_clearing_enabled: True
+ * /move_base/local_costmap/3d_obstacles/mark_threshold: 0
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/clearing: False
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/data_type: PointCloud2
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/decay_acceleration: 1
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/enabled: True
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/horizontal_fov_angle: 0.942478
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/marking: True
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/max_obstacle_height: -0.2
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/max_z: 0.1
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/min_obstacle_height: -5
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/min_z: -6
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/model_type: 0
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/topic: /camera/depth/col...
+ * /move_base/local_costmap/3d_obstacles/negative_obstacle/vertical_fov_angle: 0.785398
+ * /move_base/local_costmap/3d_obstacles/observation_sources: positive_obstacle...
+ * /move_base/local_costmap/3d_obstacles/origin_z: 0.0
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/clearing: True
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/data_type: PointCloud2
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/decay_acceleration: 1
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/enabled: True
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/horizontal_fov_angle: 0.942478
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/marking: True
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/max_obstacle_height: 1.5
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/max_z: 6
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/min_obstacle_height: 0.05
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/min_z: -0.1
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/model_type: 0
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/topic: /camera/depth/col...
+ * /move_base/local_costmap/3d_obstacles/positive_obstacle/vertical_fov_angle: 0.785398
+ * /move_base/local_costmap/3d_obstacles/publish_voxel_map: True
+ * /move_base/local_costmap/3d_obstacles/unknown_threshold: 15
+ * /move_base/local_costmap/3d_obstacles/z_resolution: 0.05
+ * /move_base/local_costmap/3d_obstacles/z_voxels: 16
+ * /move_base/local_costmap/footprint_padding: 0.0
+ * /move_base/local_costmap/global_frame: odom
+ * /move_base/local_costmap/height: 5.0
+ * /move_base/local_costmap/inflation_layer/cost_scaling_factor: 2.5
+ * /move_base/local_costmap/inflation_layer/inflation_radius: 0.2
+ * /move_base/local_costmap/map_type: costmap
+ * /move_base/local_costmap/obstacle_range: 2.5
+ * /move_base/local_costmap/plugins: [{'name': '3d_obs...
+ * /move_base/local_costmap/publish_frequency: 5.0
+ * /move_base/local_costmap/raytrace_range: 3.0
+ * /move_base/local_costmap/resolution: 0.06
+ * /move_base/local_costmap/robot_base_frame: base_link
+ * /move_base/local_costmap/robot_radius: 0.5
+ * /move_base/local_costmap/rolling_window: True
+ * /move_base/local_costmap/transform_tolerance: 0.5
+ * /move_base/local_costmap/update_frequency: 10.0
+ * /move_base/local_costmap/width: 5.0
+ * /move_base/max_planning_retries: 20
+ * /move_base/planner_patience: 10.0
+ * /octomap_server/base_frame_id: base_link
+ * /octomap_server/frame_id: map
+ * /octomap_server/occupancy_max_z: 2
+ * /octomap_server/occupancy_min_z: 0.23
+ * /octomap_server_negative/base_frame_id: base_link
+ * /octomap_server_negative/frame_id: map
+ * /octomap_server_negative/occupancy_max_z: -0.23
+ * /octomap_server_negative/occupancy_min_z: -10
+ * /rosdistro: noetic
+ * /rosversion: 1.15.11
+
+NODES
+  /
+    amcl (amcl/amcl)
+    map_server (map_server/map_server)
+    move_base (move_base/move_base)
+    octomap_server (octomap_server/octomap_server_node)
+    octomap_server_negative (octomap_server/octomap_server_node)
+    rviz (rviz/rviz)
+
+ROS_MASTER_URI=http://localhost:11311
+]2;/home/nwright/catkin_ws/src/stretch_ros/stretch_navigation/launch/navigation_3d.launch http://localhost:11311
+process[map_server-1]: started with pid [301150]
+process[octomap_server-2]: started with pid [301151]
+process[octomap_server_negative-3]: started with pid [301152]
+process[amcl-4]: started with pid [301153]
+process[move_base-5]: started with pid [301154]
+process[rviz-6]: started with pid [301159]
+[rviz-6] killing on exit
+[move_base-5] killing on exit
+[amcl-4] killing on exit
+[octomap_server_negative-3] killing on exit
+[octomap_server-2] killing on exit
+[map_server-1] killing on exit
+shutting down processing monitor...
+... shutting down processing monitor complete
+done

stretch_gazebo/launch/gazebo.launch

@@ -9,7 +9,7 @@
   <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
   <arg name="gpu_lidar" default="false"/>
   <arg name="visualize_lidar" default="false"/>
-  <arg name="world" default="worlds/empty.world"/>
+  <arg name="world" default="worlds/willowgarage.world"/>
 
   <include file="$(find gazebo_ros)/launch/empty_world.launch">
     <arg name="world_name" value="$(arg world)" />

stretch_gazebo/urdf/stretch_gazebo.urdf.xacro

@@ -258,7 +258,7 @@
             </horizontal>
           </scan>
           <range>
-            <min>0.15</min>
+            <min>0.2</min>
             <max>12.0</max>
             <resolution>0.01</resolution>
           </range>
@@ -294,7 +294,7 @@
             </horizontal>
           </scan>
           <range>
-            <min>0.15</min>
+            <min>0.2</min>
             <max>12.0</max>
             <resolution>0.01</resolution>
           </range>

stretch_navigation/.vscode/c_cpp_properties.json

@@ -0,0 +1,16 @@
+{
+  "configurations": [
+    {
+      "browse": {
+        "databaseFilename": "",
+        "limitSymbolsToIncludedHeaders": true
+      },
+      "includePath": [
+        "/opt/ros/noetic/include/**",
+        "/usr/include/**"
+      ],
+      "name": "ROS"
+    }
+  ],
+  "version": 4
+}

stretch_navigation/.vscode/settings.json

@@ -0,0 +1,6 @@
+{
+    "python.autoComplete.extraPaths": [
+        "/opt/ros/noetic/lib/python3/dist-packages",
+        "/home/csrobot/catkin_ws/devel/lib/python3/dist-packages"
+    ]
+}

stretch_navigation/launch/mapping.launch

@@ -6,21 +6,22 @@
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
   <param name="/stretch_driver/mode" type="string" value="navigation" />
-  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+  <!-- <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/> -->
 
-  <!-- LASER RANGE FINDER -->
-  <include file="$(find stretch_core)/launch/rplidar.launch" />
+  <!-- <include file="$(find stretch_core)/launch/rplidar.launch" /> -->
 
-  <!-- TELEOP -->
-  <include file="$(find stretch_core)/launch/teleop_twist.launch">
-    <arg name="teleop_type" value="$(arg teleop_type)" />
-    <arg name="linear" value="0.04" />
-    <arg name="angular" value="0.1" />
-    <arg name="twist_topic" value="/stretch/cmd_vel" />
-  </include>
+  <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
+    <param name="speed" type="double" value="0.04" />
+    <param name="turn" type="double" value="0.1" />
+    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
+  </node>
+
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_navigation)/rviz/octomap_mapper.rviz" />
 
-  <!-- MAPPING -->
-  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="log" />
+  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="screen" />
+  <include file="$(find stretch_navigation)/launch/octomap_mapper.launch">
+    <arg name="pointcloud_topic" value="/camera/depth/color/points"/>
+  </include>
 
   <!-- VISUALIZE -->
   <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />

stretch_navigation/launch/navigation.launch

@@ -1,18 +1,20 @@
 <launch>
 
-  <arg name="map_yaml"                doc="filepath to previously captured map (required)" />
-  <arg name="rviz"     default="true" doc="whether to show Rviz" />
+  <arg name="map_yaml" />
+  <arg name="octomap_location" />
 
   <!-- STRETCH DRIVER -->
   <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
   <param name="/stretch_driver/mode" type="string" value="navigation" />
-  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+  <!-- <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/> -->
 
-  <!-- LASER RANGE FINDER -->
-  <include file="$(find stretch_core)/launch/rplidar.launch" />
+  <!-- <include file="$(find stretch_core)/launch/rplidar.launch" /> -->
 
   <!-- MAP SERVER -->
   <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
+  <include file="$(find stretch_navigation)/launch/octomap_server.launch">
+    <arg name="map" value="$(arg octomap_location)"/>
+  </include>
 
   <!-- LOCALIZATION -->
   <include file="$(find amcl)/examples/amcl_diff.launch" />
@@ -24,10 +26,9 @@
     <rosparam file="$(find stretch_navigation)/config/local_costmap_params.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
     <rosparam file="$(find stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
-    <remap from="/cmd_vel" to="/stretch/cmd_vel" />
+    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
   </node>
 
-  <!-- VISUALIZE -->
-  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
+  <node name="rviz" pkg="rviz" type="rviz" output="screen" args="-d $(find stretch_navigation)/rviz/navigation.rviz" />
 
 </launch>

stretch_navigation/launch/navigation/map_server.launch

@@ -0,0 +1,8 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/map_server?distro=kinetic -->
+
+  <arg name="map_name" default="map" />
+
+  <!-- launch map server -->
+	<node name="map_server" pkg="map_server" type="map_server" output="screen" args="$(arg map_name)"/>
+</launch>

stretch_navigation/launch/navigation/move_base.launch

@@ -0,0 +1,30 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/move_base?distro=kinetic -->
+
+  <arg name="config"/>
+  
+  <arg name="cmd_vel_topic"/>
+  <arg name="odom_topic"/>
+
+  <!-- Use global costmap-->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <!-- Include robot specific parameters from the robot config folder -->
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/costmap_height.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/costmap_height.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/base_local_planner_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/global_planner_params.yaml" command="load" />
+
+    <param name="conservative_reset_dist" type="double" value="3.0" />
+    <param name="controller_frequency" type="double" value="7.0" /> <!-- 20 -->
+    <param name="planner_patience" value="10.0" />
+    <param name="max_planning_retries" value="20" />
+
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)" />
+    <remap from="odom" to="$(arg odom_topic)" />
+  </node>
+
+</launch>

stretch_navigation/launch/navigation/octomap_server.launch

@@ -0,0 +1,33 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/octomap_server?distro=kinetic -->
+  
+  <arg name="map" default="map" />
+  <arg name="base_link" default="base_link" />
+
+  <!-- launch server for positive map-->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server" output="screen" args="$(arg map)" >
+    <remap from="cloud_in" to="pointcloud_static_octomap_ignore" />
+    <param name="base_frame_id" value="$(arg base_link)" />
+    <param name="frame_id" type="string" value="map" />
+    <param name="occupancy_min_z" value="0.23" />
+    <param name="occupancy_max_z" value="2" />
+  </node>
+
+  <!-- launch server for negative map-->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server_negative" output="screen" args="$(arg map)" >
+    <remap from="cloud_in" to="pointcloud_static_octomap_ignore" />
+    <param name="base_frame_id" value="$(arg base_link)" />
+    <param name="frame_id" type="string" value="map" />
+    <param name="occupancy_max_z" value="-0.23" />
+    <param name="occupancy_min_z" value="-10" />
+
+    <!-- rename all colliding topics and services-->
+    <remap from="octomap_binary" to="octomap_binary_negative" />
+    <remap from="octomap_full" to="octomap_full_negative" />
+    <remap from="occupied_cells_vis_array" to="occupied_cells_vis_array_negative" />
+    <remap from="octomap_point_cloud_centers" to="octomap_point_cloud_centers_negative" />
+    <remap from="projected_map" to="projected_map_negative" />
+    <remap from="clear_bbx" to="clear_bbx_negative" />
+    <remap from="reset" to="reset_negative" />
+  </node>
+</launch>

stretch_navigation/launch/navigation_3d.launch

@@ -0,0 +1,46 @@
+<launch>
+  <!-- The navigation_3d launch file includes all of the launch files needed for a robot to accomplish 3D navigation.  
+  The nodes needed for navigation are map_server, octomap_server, amcl, and move_base.-->
+
+  <!-- Map name -->
+  <arg name="map" default="map"/>
+  <!-- Move_base configuration folder. -->
+  <arg name="move_base_config" value="3d" />
+
+  <!-- Robot sensor topic mapping-->
+  <arg name="laser_topic" default="scan"/>
+  <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
+  <arg name="cmd_vel_topic" default="/stretch_diff_drive_controller/cmd_vel"/>
+  <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+  <arg name="odom_link" default="odom"/>
+  <arg name="base_link" default="base_link"/>
+
+  <!-- Launch Map Server -->
+  <include file="$(find stretch_navigation)/launch/navigation/map_server.launch">
+    <arg name="map_name" value="$(find stretch_navigation)/resources/static_maps/2d/$(arg map).yaml" />
+  </include>
+
+  <!-- Launch positive and negative Octomap Server -->
+  <include file="$(find stretch_navigation)/launch/navigation/octomap_server.launch">
+    <arg name="map" value="$(find stretch_navigation)/resources/static_maps/3d/$(arg map).bt" />
+    <arg name="base_link" value="$(arg base_link)" />
+  </include>
+
+  <!--- Launch AMCL (Laser Localization) -->
+    <node pkg="amcl" type="amcl" name="amcl" output="screen">
+      <rosparam file="$(find stretch_navigation)/resources/config/amcl.yaml" command="load" />
+    </node>
+
+
+  <!-- Launch move_base -->
+  <!-- Note that you will also need to change the relevant .yaml files (or make new copies)
+       if you wish to use another namespace/robot_name. -->
+  <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
+      <arg name="odom_topic" value="$(arg odom_topic)" />
+  </include>
+
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find stretch_navigation)/rviz/octomap.rviz" />
+
+</launch>

stretch_navigation/launch/octomap_mapper.launch

@@ -0,0 +1,17 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/octomap_server?distro=kinetic -->
+  
+  <arg name="pointcloud_topic" />
+  <arg name="base_link" default="base_link" />
+
+  <!-- you can load an exisiting tree with <node ... args="tree.bt"> !-->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_mapper" >
+    <remap from="cloud_in" to="$(arg pointcloud_topic)" />
+    <param name="base_frame_id" value="$(arg base_link)" />
+    <param name="resolution" value="0.05" />
+    <param name="frame_id" type="string" value="map" />
+    <param name="sensor_model/max_range" value="3.0" />
+    <param name="filter_ground" value="false" />
+      
+  </node>
+</launch>

stretch_navigation/launch/rtab.launch

@@ -0,0 +1,61 @@
+<launch>
+  
+  <arg name="database_path"     default="rtabmap.db"/>
+  <arg name="args"              default=""/>
+  
+  <arg name="wait_for_transform"  default="0.2"/> 
+
+  <arg name="move_base_config" value="2d" />
+  <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
+  <arg name="cmd_vel_topic" default="/stretch_diff_drive_controller/cmd_vel"/>
+  <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+  
+  <!-- Navigation stuff (move_base) -->
+  <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
+      <arg name="odom_topic" value="$(arg odom_topic)" />
+  </include>
+  
+  <!-- Mapping -->
+  <group ns="rtabmap">
+
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+      <param name="database_path"       type="string" value="$(arg database_path)"/>
+      <param name="frame_id"            type="string" value="base_link"/>
+      <param name="odom_frame_id"       type="string" value="odom"/>
+      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
+      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_scan"      type="bool"   value="true"/>
+    
+      <!-- inputs -->
+      <remap from="scan"            to="/scan"/>
+      <remap from="rgb/image"       to="/camera/color/image_raw"/>
+      <remap from="depth/image"     to="/camera/depth/image_raw"/>
+      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
+      
+      <!-- output -->
+      <remap from="grid_map" to="/map"/>
+    
+      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
+      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
+      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
+      <param name="Reg/Strategy"                 type="string" value="1"/>      
+      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
+      <param name="Vis/MinInliers"               type="string" value="6"/>      
+      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
+      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
+      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
+      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
+      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
+      <param name="Optimizer/Slam2D"             type="string" value="true"/>
+      <param name="Reg/Force3DoF"                type="string" value="true"/>   
+      
+    </node>
+   
+    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+  </group>
+</launch>

stretch_navigation/launch/rtab/robot.launch

@@ -0,0 +1,36 @@
+<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>
+    <!-- -->
+
+    <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
+    <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
+    <!-- -->
+
+    <!-- STRETCH DRIVER -->
+    <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+    <param name="/stretch_driver/mode" type="string" value="navigation" />
+    <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"/>
+    <!-- -->
+
+    <!-- IMU FILTER -->
+    <!-- <include file="$(find stretch_core)/launch/imu_filter.launch" /> -->
+    <!-- -->
+
+    <!-- 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" />
+    <!-- -->
+
+    <!-- ROBOT LOCALIZATION FILTER -->
+    <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
+    <!-- -->
+</launch>

stretch_navigation/launch/rtab/rtab.launch

@@ -0,0 +1,57 @@
+<launch>
+  <arg name="localization"/>
+  <arg name="args"/>
+  <arg name="database_path"/>
+  <arg name="wait_for_transform"/> 
+  <arg name="pointcloud_topic"/>
+  <arg name="cmd_vel_topic"/>
+  <arg name="odom_topic"/>
+  <arg name="scan"/>
+  <arg name="rgb_topic"/>
+  <arg name="rgb_camera_info"/>
+  <!-- Mapping -->
+  <group ns="rtabmap">
+
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+      <param name="database_path"       type="string" value="$(arg database_path)"/>
+      <param name="frame_id"            type="string" value="base_link"/>
+      <param name="odom_frame_id"       type="string" value="odom"/>
+      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
+      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
+      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
+      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_scan"      type="bool"   value="true"/>
+    
+      <!-- inputs -->
+      <remap from="scan"            to="$(arg scan)"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg pointcloud_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg rgb_camera_info)"/>
+      
+      <!-- output -->
+      <remap from="grid_map" to="/map"/>
+    
+      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
+      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
+      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
+      <param name="Reg/Strategy"                 type="string" value="1"/>      
+      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
+      <param name="Vis/MinInliers"               type="string" value="6"/>      
+      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
+      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
+      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
+      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
+      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
+      <param name="Reg/Force3DoF"                type="string" value="true"/>  
+      <param name="Grid/FromDepth"               type="string" value="true"/>
+      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> 
+      
+    </node>
+    
+    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+  </group>
+
+</launch>

stretch_navigation/launch/rtab_gazebo.launch

@@ -0,0 +1,61 @@
+<launch>
+  
+  <arg name="database_path"     default="rtabmap.db"/>
+  <arg name="args"              default=""/>
+  
+  <arg name="wait_for_transform"  default="0.2"/> 
+
+  <arg name="move_base_config" value="2d" />
+  <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
+  <arg name="cmd_vel_topic" default="/stretch_diff_drive_controller/cmd_vel"/>
+  <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+  
+  <!-- Navigation stuff (move_base) -->
+  <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
+      <arg name="odom_topic" value="$(arg odom_topic)" />
+  </include>
+  
+  <!-- Mapping -->
+  <group ns="rtabmap">
+
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+      <param name="database_path"       type="string" value="$(arg database_path)"/>
+      <param name="frame_id"            type="string" value="base_link"/>
+      <param name="odom_frame_id"       type="string" value="odom"/>
+      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
+      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_scan"      type="bool"   value="true"/>
+    
+      <!-- inputs -->
+      <remap from="scan"            to="/scan"/>
+      <remap from="rgb/image"       to="/camera/color/image_raw"/>
+      <remap from="depth/image"     to="/camera/depth/image_raw"/>
+      <remap from="rgb/camera_info" to="/camera/color/camera_info"/>
+      
+      <!-- output -->
+      <remap from="grid_map" to="/map"/>
+    
+      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
+      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
+      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
+      <param name="Reg/Strategy"                 type="string" value="1"/>      
+      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
+      <param name="Vis/MinInliers"               type="string" value="6"/>      
+      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
+      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
+      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
+      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
+      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
+      <param name="Optimizer/Slam2D"             type="string" value="true"/>
+      <param name="Reg/Force3DoF"                type="string" value="true"/>   
+      
+    </node>
+   
+    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+  </group>
+</launch>

stretch_navigation/launch/rtab_generic.launch

@@ -0,0 +1,65 @@
+<launch>
+  <arg name="sim" />
+  <arg name="localization" default="false" />
+
+  <arg name="database_path"     default="rtabmap.db"/>
+  <arg name="args"              default=""/>
+
+  <arg name="wait_for_transform"  default="0.2"/> 
+  <arg name="move_base_config" value="2d" />
+
+
+  <group if="$(arg sim)">
+    <arg name="pointcloud_topic" default="/camera/depth/image_raw"/>
+    <arg name="cmd_vel_topic" value="/stretch_diff_drive_controller/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/camera/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+
+    <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+      <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="args" value="$(arg args)"/>
+      <arg name="database_path"     default="$(arg database_path)"/>
+      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
+      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
+      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+      <arg name="odom_topic" default="$(arg odom_topic)"/>
+      <arg name="scan" default="$(arg scan_topic)"/>
+      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
+      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
+    </include>
+  </group>
+  <group unless="$(arg sim)">
+    <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
+    <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/camera/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+
+    <include file="$(find stretch_navigation)/launch/rtab/robot.launch"/>
+    <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+      <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="args" value="$(arg args)"/>
+      <arg name="database_path"     default="$(arg database_path)"/>
+      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
+      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
+      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+      <arg name="odom_topic" default="$(arg odom_topic)"/>
+      <arg name="scan" default="$(arg scan_topic)"/>
+      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
+      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
+    </include>
+  </group>
+</launch>

stretch_navigation/launch/rtab_robot.launch

@@ -0,0 +1,51 @@
+<launch>
+  <arg name="sim" />
+  <arg name="localization" default="false" />
+
+  <arg name="database_path"     default="rtabmap.db"/>
+  <arg name="args"              default=""/>
+
+  <arg name="wait_for_transform"  default="0.2"/> 
+  <arg name="move_base_config" value="2d" />
+
+
+  <group if="$(arg sim)">
+    <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
+    <arg name="cmd_vel_topic" value="/stretch_diff_drive_controller/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/camera/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+
+    <include file="$(find stretch_navigation)/launch/rtab/move_base.launch">
+      <arg name="move_base_config" value="$(arg move_base_config)" />
+    </include>
+    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+    </include>
+  </group>
+  <group unless="$(arg sim)">
+    <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
+    <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/camera/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+
+    <include file="$(find stretch_navigation)/launch/rtab/robot.launch"/>
+    <include file="$(find stretch_navigation)/launch/rtab/move_base.launch">
+      <arg name="move_base_config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+      <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    <include file="$(find stretch_navigation)/launch/rtab/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
+      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+      <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+      <arg name="scan_topic" default="/scan"/>
+      <arg name="rgb_topic" default="/camera/color/image_raw"/>
+      <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+    </include>
+  </group>
+</launch>

stretch_navigation/launch/rtabmapviz.launch

@@ -0,0 +1,8 @@
+<launch>
+<group ns="rtabmap">  
+  <!-- Visualisation RTAB-Map -->
+  <node pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
+     <remap from="odom"              to="/stretch_diff_drive_controller/odom"/>
+  </node>
+</group>
+</launch>

stretch_navigation/launch/rviz_rtab.launch

@@ -0,0 +1,3 @@
+<launch>
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find stretch_navigation)/rviz/rtab_mapping.rviz"/>
+</launch>

stretch_navigation/launch/teleop.launch

@@ -0,0 +1,7 @@
+<launch>
+  <node name="teleop_twist_keyboard" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" output="screen" >
+    <param name="speed" type="double" value="0.04" />
+    <param name="turn" type="double" value="0.1" />
+    <remap from="/cmd_vel" to="/stretch/cmd_vel" />
+  </node>
+</launch>

stretch_navigation/resources/config/2d/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.20
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.20
+  max_vel_x: 0.25
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 0.25
+  min_vel_theta: -0.25
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 3                   #was 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 32
+  goal_distance_bias: 24
+  occdist_scale: 0.1
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_navigation/resources/config/2d/costmap_common_params.yaml

@@ -0,0 +1,26 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+# robot_radius: 0.55
+#             FL            BL              BR              FR
+footprint: [[0.05,0.175],[-0.30,0.175],[-0.30,-0.175],[0.05,-0.175]]
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.3
+  cost_scaling_factor: 2.5

stretch_navigation/resources/config/2d/global_costmap_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+  plugins:
+  - {name: static_map, type: "costmap_2d::StaticLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 0.5
+  static_map: 
+    map_topic: /map
+    
+

stretch_navigation/resources/config/2d/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_navigation/resources/config/2d/local_costmap_params.yaml

@@ -0,0 +1,12 @@
+local_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.05

stretch_navigation/resources/config/2d_unknown/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.35
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.8
+  max_vel_x: 1.0
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 80
+  goal_distance_bias: 60
+  occdist_scale: 1.4
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_navigation/resources/config/2d_unknown/costmap_common_params.yaml

@@ -0,0 +1,24 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+robot_radius: 0.50
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.75
+  cost_scaling_factor: 2.5

stretch_navigation/resources/config/2d_unknown/global_costmap_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 2.0
+  publish_frequency: 0.0
+  static_map: false
+  rolling_window: true
+  width: 500.0
+  height: 500.0
+  resolution: 0.1

stretch_navigation/resources/config/2d_unknown/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_navigation/resources/config/2d_unknown/local_costmap_params.yaml

@@ -0,0 +1,12 @@
+local_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.06

stretch_navigation/resources/config/3d/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.35
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.8
+  max_vel_x: 1.0
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 32
+  goal_distance_bias: 24
+  occdist_scale: 1.4
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_navigation/resources/config/3d/costmap_common_params.yaml

@@ -0,0 +1,66 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+robot_radius: 0.50
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+3d_obstacles:
+  enabled: true
+  footprint_clearing_enabled: true
+  publish_voxel_map: true
+  origin_z: 0.0
+  z_resolution: 0.05
+  z_voxels: 16
+  unknown_threshold: 15
+  mark_threshold: 0
+  observation_sources: positive_obstacle negative_obstacle
+  combination_method: 1
+  positive_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.05                  #default 0, meters
+    #robot config defines max height here
+    min_z: -0.1
+    max_z: 6
+    vertical_fov_angle: 0.785398       #default 0.7, radians
+    horizontal_fov_angle: 0.942478   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+    
+  negative_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: false
+    min_obstacle_height: -5                   #default 0, meters
+    max_obstacle_height: -0.2
+    min_z: -6
+    max_z: 0.1
+    vertical_fov_angle: 0.785398      #default 0.7, radians
+    horizontal_fov_angle: 0.942478   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.2
+  cost_scaling_factor: 2.5

stretch_navigation/resources/config/3d/global_costmap_params.yaml

@@ -0,0 +1,20 @@
+global_costmap:
+  plugins:
+  - {name: static_map_3d_negative, type: "costmap_2d::StaticLayer"}
+  - {name: static_map_3d_positive, type: "costmap_2d::StaticLayer"}
+  - {name: static_map_2d, type: "costmap_2d::StaticLayer"}
+  - {name: 3d_obstacles, type: "costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 0.5
+  rolling_window: false #idk about this one
+  static_map_2d: 
+    map_topic: /map
+  static_map_3d_positive: 
+    map_topic: /projected_map
+  static_map_3d_negative: 
+    map_topic: /projected_map_negative
+    

stretch_navigation/resources/config/3d/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_navigation/resources/config/3d/local_costmap_params.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom  #might use map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 5.0
+  height: 5.0
+  resolution: 0.06

stretch_navigation/resources/config/3d_unknown/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.35
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.8
+  max_vel_x: 1.0
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 80
+  goal_distance_bias: 60
+  occdist_scale: 1.4
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_navigation/resources/config/3d_unknown/costmap_common_params.yaml

@@ -0,0 +1,73 @@
+obstacle_range: 2.5
+raytrace_range: 2.5
+
+robot_radius: 0.50
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+3d_obstacles:
+  enabled: true
+  voxel_decay: 15     #seconds if linear, e^n if exponential
+  decay_model: 0      #0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.05   #meters
+  track_unknown_space: true   #default space is unknown
+  observation_persistence: 0.0  #seconds
+  unknown_threshold: 15     #voxel height
+  mark_threshold: 0      #voxel height
+  update_footprint_enabled: true
+  combination_method: 1      #1=max, 0=override
+
+  origin_z: 0.0    #meters
+  publish_voxel_map: true   # default off
+  transform_tolerance: 0.4    
+  mapping_mode: false  # default off, saves map not for navigation
+  map_save_duration: 60     #default 60s, how often to autosave
+  observation_sources: positive_obstacle negative_obstacle
+  positive_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.05                  #default 0, meters
+    #robot config defines max height here
+    min_z: -0.1
+    max_z: 6
+    vertical_fov_angle: 0.7      #default 0.7, radians
+    horizontal_fov_angle: 1.04   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+    
+  negative_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: false
+    min_obstacle_height: -5                   #default 0, meters
+    max_obstacle_height: -0.2
+    min_z: -6
+    max_z: 0.1
+    vertical_fov_angle: 0.785398      #default 0.7, radians
+    horizontal_fov_angle: 0.942478   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: /scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.75
+  cost_scaling_factor: 2.5

stretch_navigation/resources/config/3d_unknown/global_costmap_params.yaml

@@ -0,0 +1,15 @@
+global_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 2.0
+  publish_frequency: 0.0
+  static_map: false
+  rolling_window: true
+  width: 500.0
+  height: 500.0
+  resolution: 0.1
+    

stretch_navigation/resources/config/3d_unknown/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_navigation/resources/config/3d_unknown/local_costmap_params.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 5.0
+  height: 5.0
+  resolution: 0.06

stretch_navigation/resources/config/amcl.yaml

@@ -0,0 +1,43 @@
+amcl:
+  ros__parameters:
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_link"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    set_initial_pose: false
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    always_reset_initial_pose: false
+    use_map_topic: true
+    scan_topic: "/scan"
+    map_topic: "/map"

stretch_navigation/resources/config/costmap_height.yaml

@@ -0,0 +1,3 @@
+3d_obstacles:
+  positive_obstacle:
+    max_obstacle_height: 1.5

stretch_navigation/resources/static_maps/2d/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+resolution: 0.050000
+origin: [-100.000000, -100.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

stretch_navigation/rviz/mapping.rviz

@@ -5,6 +5,7 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Odometry1/Shape1
+        - /OccupancyGrid1
       Splitter Ratio: 0.5
     Tree Height: 539
   - Class: rviz/Selection
@@ -291,6 +292,34 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - 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: false
+      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): 4
+      Size (m): 0.004000000189989805
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
     - Angle Tolerance: 0.10000000149011612
       Class: rviz/Odometry
       Covariance:
@@ -336,6 +365,18 @@ Visualization Manager:
       Unreliable: false
       Use Timestamp: false
       Value: true
+    - Class: octomap_rviz_plugin/OccupancyGrid
+      Enabled: true
+      Max. Height Display: inf
+      Max. Octree Depth: 16
+      Min. Height Display: -inf
+      Name: OccupancyGrid
+      Octomap Topic: /octomap_full
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

stretch_navigation/rviz/navigation.rviz

@@ -340,7 +340,7 @@ Visualization Manager:
       Keep: 30
       Name: Odometry
       Position Tolerance: 0.10000000149011612
-      Queue Size: 30
+      Queue Size: 10
       Shape:
         Alpha: 1
         Axes Length: 1
@@ -450,6 +450,28 @@ Visualization Manager:
       Topic: /move_base/NavfnROS/plan
       Unreliable: false
       Value: true
+    - Class: octomap_rviz_plugin/OccupancyGrid
+      Enabled: false
+      Max. Height Display: inf
+      Max. Octree Depth: 16
+      Min. Height Display: -inf
+      Name: OccupancyGrid
+      Octomap Topic: /octomap_full
+      Queue Size: 5
+      Value: false
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /projected_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48

stretch_navigation/rviz/octomap.rviz

@@ -0,0 +1,405 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /OccupancyMap1
+        - /Map1
+        - /Path1
+        - /Path2
+      Splitter Ratio: 0.5
+    Tree Height: 1082
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+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.699999988079071
+      Class: octomap_rviz_plugin/OccupancyMap
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Max. Octree Depth: 16
+      Name: OccupancyMap
+      Octomap Binary Topic: /octomap_full
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 24.759632110595703
+      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: -4.327556133270264
+        Y: -6.250387668609619
+        Z: -5.1258134841918945
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004c4000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000004c4000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000028f000004c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1280
+  X: 0
+  Y: 27

stretch_navigation/rviz/octomap_mapper.rviz

@@ -0,0 +1,447 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Odometry1/Shape1
+        - /OccupancyGrid1
+      Splitter Ratio: 0.4206896424293518
+    Tree Height: 1109
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      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: false
+    - 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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - 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
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - 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: false
+      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): 4
+      Size (m): 0.004000000189989805
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      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: 10
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 237; 212; 0
+        Head Length: 0.03999999910593033
+        Head Radius: 0.05000000074505806
+        Shaft Length: 0.15000000596046448
+        Shaft Radius: 0.019999999552965164
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Class: octomap_rviz_plugin/OccupancyGrid
+      Enabled: true
+      Max. Height Display: inf
+      Max. Octree Depth: 16
+      Min. Height Display: -inf
+      Name: OccupancyGrid
+      Octomap Topic: /octomap_full
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 11.191774368286133
+      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: -5.315392971038818
+        Y: -3.4429707527160645
+        Z: -0.27000176906585693
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0197970867156982
+      Target Frame: <Fixed Frame>
+      Yaw: 3.8762691020965576
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1403
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000025f000004dffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004df000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000079b000004df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 2560
+  Y: 0

stretch_navigation/rviz/rtab_mapping.rviz

@@ -0,0 +1,396 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 787
+  - 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
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: MapCloud
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - Alpha: 1
+      Class: rtabmap_ros/MapGraph
+      Enabled: true
+      Global loop closure: 255; 0; 0
+      Landmark: 0; 128; 0
+      Local loop closure: 255; 255; 0
+      Merged neighbor: 255; 170; 0
+      Name: MapGraph
+      Neighbor: 0; 0; 255
+      Queue Size: 10
+      Topic: /rtabmap/mapGraph
+      Unreliable: false
+      User: 255; 0; 0
+      Value: true
+      Virtual: 255; 0; 255
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap_ros/MapCloud
+      Cloud decimation: 4
+      Cloud from scan: false
+      Cloud max depth (m): 4
+      Cloud min depth (m): 0
+      Cloud voxel size (m): 0.009999999776482582
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Enabled: true
+      Filter ceiling (m): 0
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0
+      Position Transformer: XYZ
+      Queue Size: 10
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /rtabmap/mapData
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Queue Size: 10
+      Topic: /move_base/global_costmap/footprint
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: rtabmap_ros/OrbitOriented
+      Distance: 17.96693229675293
+      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: -3.853421926498413
+        Y: 0.7817280888557434
+        Z: 0.4164792597293854
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7947970628738403
+      Target Frame: <Fixed Frame>
+      Yaw: 4.218579292297363
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000007fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003d000000810000005c00fffffffb0000000a0056006900650077007300000000c400000317000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000134fc0100000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000000007800000000000000000fb0000000800540069006d0065000000000000000780000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: true
+  Views:
+    collapsed: true
+  Width: 960
+  X: 0
+  Y: 27

stretch_navigation/rviz/rtab_navigation.rviz

@@ -0,0 +1,374 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Path1
+      Splitter Ratio: 0.5
+    Tree Height: 1082
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /rtabmap/grid_prob_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /rtabmap/mapPath
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 24.471881866455078
+      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: -2.2477076053619385
+        Y: -1.80465567111969
+        Z: -6.365629196166992
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 1.2835423946380615
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004c4000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000004c4000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 27

stretch_rtab/CMakeLists.txt

@@ -0,0 +1,202 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(stretch_rtab)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES stretch_rtab
+#  CATKIN_DEPENDS other_catkin_pkg
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/stretch_rtab.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/stretch_rtab_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# catkin_install_python(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_stretch_rtab.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

stretch_rtab/LICENSE.md

@@ -0,0 +1,11 @@
+The following license applies to the entire contents of this directory (the "Contents"), which contains software for use with the Stretch RE1 mobile manipulator, which is a robot produced and sold by Hello Robot Inc.
+
+Copyright 2020 Hello Robot Inc.
+
+The Contents are licensed under the Apache License, Version 2.0 (the "License"). You may not use the Contents except in compliance with the License. You may obtain a copy of the License at
+
+http://www.apache.org/licenses/LICENSE-2.0
+
+Unless required by applicable law or agreed to in writing, the Contents are distributed under the License are distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the License for the specific language governing permissions and limitations under the License.
+   
+For further information about the Contents including inquiries about dual licensing, please contact Hello Robot Inc.

stretch_rtab/README.md

@@ -0,0 +1,42 @@
+![](../images/HelloRobotLogoBar.png)
+
+## Overview
+
+*stretch_rtab* provides RTAB-MAPPiNG. This package utilizes rtab-map, move_base, and AMCL to map and drive the stretch RE1 around a space. Running this code will require the robot to be untethered.
+
+## Setup
+
+Use `rosdep` to install the required packages.
+
+```bash
+    cd ~/catkin_ws/src
+    git clone https://github.com/NateWright/stretch_ros.git
+    git clone https://github.com/pal-robotics/realsense_gazebo_plugin
+    cd ~/catkin_ws
+    rosdep install --from-paths src --ignore-src -r -y
+    catkin_make
+```
+
+## Running Demo
+
+### Gazebo
+
+```bash
+    roslaunch stretch_rtab gazebo.launch
+    roslaunch stretch_rtab start_rtab.launch sim:=true localization:=false move_base_config:=2d
+    roslaunch stretch_rtab rviz_rtab.launch mapping:=true
+```
+
+### Stretch RE1
+```bash
+    roslaunch stretch_rtab start_rtab.launch sim:=false localization:=false move_base_config:=2d
+    roslaunch stretch_rtab rviz_rtab.launch mapping:=true
+```
+## Code Status & Development Plans
+
+Move_base_config | Gazebo | Stretch RE1
+--- | --- | ---
+2d | okay | Good
+2d_unkown | Not Implemented | Not Implemented
+3d | Testing | Testing
+3d_unkown | Not Implemented | Not Implemented

stretch_rtab/config/2d/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.20
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.20
+  max_vel_x: 0.25
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 0.25
+  min_vel_theta: -0.25
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 3                   #was 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 32
+  goal_distance_bias: 24
+  occdist_scale: 0.1
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_rtab/config/2d/costmap_common_params.yaml

@@ -0,0 +1,26 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+# robot_radius: 0.55
+#             FL            BL              BR              FR
+footprint: [[0.05,0.175],[-0.30,0.175],[-0.30,-0.175],[0.05,-0.175]]
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.3
+  cost_scaling_factor: 2.5

stretch_rtab/config/2d/global_costmap_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+  plugins:
+  - {name: static_map, type: "costmap_2d::StaticLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 0.5
+  static_map: 
+    map_topic: /map
+    
+

stretch_rtab/config/2d/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_rtab/config/2d/local_costmap_params.yaml

@@ -0,0 +1,12 @@
+local_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.05

stretch_rtab/config/2d_unknown/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.35
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.8
+  max_vel_x: 1.0
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 80
+  goal_distance_bias: 60
+  occdist_scale: 1.4
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_rtab/config/2d_unknown/costmap_common_params.yaml

@@ -0,0 +1,24 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+robot_radius: 0.50
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.75
+  cost_scaling_factor: 2.5

stretch_rtab/config/2d_unknown/global_costmap_params.yaml

@@ -0,0 +1,13 @@
+global_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 2.0
+  publish_frequency: 0.0
+  static_map: false
+  rolling_window: true
+  width: 500.0
+  height: 500.0
+  resolution: 0.1

stretch_rtab/config/2d_unknown/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_rtab/config/2d_unknown/local_costmap_params.yaml

@@ -0,0 +1,12 @@
+local_costmap:
+  plugins:
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.06

stretch_rtab/config/3d/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.20
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.20
+  max_vel_x: 0.25
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 0.25
+  min_vel_theta: -0.25
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.3 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.3
+  min_vel_trans: 0
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 3                   #was 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 32
+  goal_distance_bias: 24
+  occdist_scale: 0.1
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_rtab/config/3d/costmap_common_params.yaml

@@ -0,0 +1,50 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+
+# robot_radius: 0.55
+#             FL            BL              BR              FR
+footprint: [[0.05,0.175],[-0.30,0.175],[-0.30,-0.175],[0.05,-0.175]]
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: voxel
+
+3d_obstacles:
+  enabled: true
+  max_obstacle_height:  1.5
+  origin_z: 0.0
+  z_resolution: 0.05
+  z_voxels: 30
+  unknown_threshold: 15
+  mark_threshold: 0
+  combination_method: 1
+  track_unknown_space: true
+  footprint_clearing_enabled: true
+  publish_voxel_map: true
+  observation_sources: cloud
+  cloud:
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    sensor_frame: base_link
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.05
+
+
+2d_obstacles:
+  enabled: true
+  observation_sources: laser
+  laser: 
+    data_type: LaserScan 
+    topic: /scan
+    marking: true
+    clearing: true 
+    expected_update_rate: 0.3
+    track_unknown_space: true
+
+
+inflation_layer:
+  enabled: true
+  inflation_radius: 0.3
+  cost_scaling_factor: 2.5

stretch_rtab/config/3d/global_costmap_params.yaml

@@ -0,0 +1,14 @@
+global_costmap:
+  plugins:
+  - {name: static_map_2d, type: "costmap_2d::StaticLayer"}
+  - {name: 3d_obstacles, type: "costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 1.0
+  publish_frequency: 0.5
+  rolling_window: true
+  static_map_2d: 
+    map_topic: /map
+    

stretch_rtab/config/3d/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_rtab/config/3d/local_costmap_params.yaml

@@ -0,0 +1,14 @@
+local_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 5.0
+  height: 5.0
+  resolution: 0.05

stretch_rtab/config/3d_unknown/base_local_planner_params.yaml

@@ -0,0 +1,79 @@
+base_local_planner: "dwa_local_planner/DWAPlannerROS"
+
+TrajectoryPlannerROS:
+  acc_lim_x: 0.35
+  acc_lim_y: 0.0
+  acc_lim_theta: 0.8
+  max_vel_x: 1.0
+  min_vel_x: 0.15
+  max_vel_y: 0.0
+  min_vel_y: 0.0
+  max_vel_theta: 1.5
+  min_vel_theta: -1.5
+  min_in_place_vel_theta: 0.3
+  holonomic_robot: false
+  # base vel/accel profile is in robot folders
+  # tolerances (defaults)
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.1
+  latch_xy_goal_tolerance: true
+  # forward simulation
+  sim_time: 1.0
+  sim_granularity: 0.025
+  angular_sim_granularity: 0.025
+  vx_samples: 3
+  vtheta_samples: 10
+  # scoring (defaults)
+  meter_scoring: true
+  path_distance_bias: 0.5
+  goal_distance_bias: 0.75
+  occdist_scale: 0.00625
+  heading_lookahead: 0.325
+  heading_scoring_timestep: 0.8
+  heading_scoring: true
+  dwa: false
+  # other
+  oscillation_reset_dist: 0.05
+  # debug
+  publish_cost_grid_pc: false
+
+DWAPlannerROS:
+  #vel params
+  max_vel_x: 0.6 #safe speed for fetch to travel at
+  min_vel_x: 0
+  max_vel_trans: 0.6
+  min_vel_trans: 0.1
+  max_vel_y: 0  #diff drive
+  min_vel_y: 0
+  max_vel_theta: 1.5
+  min_vel_theta: 0.4
+
+  #accel params
+  acc_lim_x: 0.7
+  acc_lim_y: 0  #diff drive
+  acc_lim_trans: 0.7
+  acc_lim_theta: 4
+
+  #forward sim params
+  sim_time: 1.1
+  vx_samples: 5
+  vy_samples: 0 #diff drive
+  vth_samples: 15
+
+  #scoring params
+  path_distance_bias: 80
+  goal_distance_bias: 60
+  occdist_scale: 1.4
+  forward_point_distance: 0.3
+
+  #higher goal tolerance
+  yaw_goal_tolerance: 0.1
+  xy_goal_tolerance: 0.2
+  latch_xy_goal_tolerance: true
+
+  #lower goal tolerance
+  #yaw_goal_tolerance: 0.05
+  #xy_goal_tolerance: 0.1
+  #latch_xy_goal_tolerance: false
+  
+  prune_plan: true

stretch_rtab/config/3d_unknown/costmap_common_params.yaml

@@ -0,0 +1,73 @@
+obstacle_range: 2.5
+raytrace_range: 2.5
+
+robot_radius: 0.50
+
+footprint_padding: 0.0
+
+transform_tolerance: 0.5
+map_type: costmap
+
+3d_obstacles:
+  enabled: true
+  voxel_decay: 15     #seconds if linear, e^n if exponential
+  decay_model: 0      #0=linear, 1=exponential, -1=persistent
+  voxel_size: 0.05   #meters
+  track_unknown_space: true   #default space is unknown
+  observation_persistence: 0.0  #seconds
+  unknown_threshold: 15     #voxel height
+  mark_threshold: 0      #voxel height
+  update_footprint_enabled: true
+  combination_method: 1      #1=max, 0=override
+
+  origin_z: 0.0    #meters
+  publish_voxel_map: true   # default off
+  transform_tolerance: 0.4    
+  mapping_mode: false  # default off, saves map not for navigation
+  map_save_duration: 60     #default 60s, how often to autosave
+  observation_sources: positive_obstacle negative_obstacle
+  positive_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: true
+    min_obstacle_height: 0.05                  #default 0, meters
+    #robot config defines max height here
+    min_z: -0.1
+    max_z: 6
+    vertical_fov_angle: 0.7      #default 0.7, radians
+    horizontal_fov_angle: 1.04   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+    
+  negative_obstacle:
+    enabled: true                #default true, can be toggled on/off with associated service call
+    data_type: PointCloud2
+    topic: /camera/depth/color/points
+    marking: true
+    clearing: false
+    min_obstacle_height: -5                   #default 0, meters
+    max_obstacle_height: -0.2
+    min_z: -6
+    max_z: 0.1
+    vertical_fov_angle: 0.785398      #default 0.7, radians
+    horizontal_fov_angle: 0.942478   #default 1.04, radians
+    decay_acceleration: 1       #default 0, 1/s^2. If laser scanner MUST be 0
+    model_type: 0                #default 0 (depth camera). Use 1 for 3D Lidar
+
+
+2d_obstacles:
+  observation_sources: laser
+  laser: {data_type: LaserScan, 
+          topic: /scan, 
+          marking: true, 
+          clearing: true, 
+          expected_update_rate: 0.3,
+          track_unknown_space: true
+          }
+
+
+inflation_layer:
+  inflation_radius: 0.75
+  cost_scaling_factor: 2.5

stretch_rtab/config/3d_unknown/global_costmap_params.yaml

@@ -0,0 +1,15 @@
+global_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 2.0
+  publish_frequency: 0.0
+  static_map: false
+  rolling_window: true
+  width: 500.0
+  height: 500.0
+  resolution: 0.1
+    

stretch_rtab/config/3d_unknown/global_planner_params.yaml

@@ -0,0 +1,11 @@
+base_global_planner: global_planner/GlobalPlanner
+GlobalPlanner:
+  allow_unknown: true
+  cost_factor: 0.5
+  lethal_cost: 253
+  neutral_cost: 66
+  orientation_mode: 0
+  orientation_window_size: 1
+  publish_potential: false
+  default_tolerance: 0.2
+  outline_map: false

stretch_rtab/config/3d_unknown/local_costmap_params.yaml

@@ -0,0 +1,13 @@
+local_costmap:
+  plugins:
+  - {name: 3d_obstacles, type: "nav2_costmap_2d::VoxelLayer"}
+  - {name: 2d_obstacles, type: "costmap_2d::ObstacleLayer"}
+  - {name: inflation_layer, type: "costmap_2d::InflationLayer"}
+  global_frame: odom
+  robot_base_frame: base_link
+  update_frequency: 10.0
+  publish_frequency: 5.0
+  rolling_window: true
+  width: 5.0
+  height: 5.0
+  resolution: 0.06

stretch_rtab/config/amcl.yaml

@@ -0,0 +1,23 @@
+amcl:
+  ros__parameters:
+    base_frame_id: "base_link"
+    global_frame_id: "map"
+    set_initial_pose: false
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: -1.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    resample_interval: 2
+    robot_model_type: "differential"
+    save_pose_rate: 0.5
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    use_map_topic: true
+    scan_topic: "/scan"
+    map_topic: "/map"

stretch_rtab/config/costmap_height.yaml

@@ -0,0 +1,3 @@
+3d_obstacles:
+  positive_obstacle:
+    max_obstacle_height: 1.5

stretch_rtab/config/stretch_gazebo.yaml

@@ -0,0 +1,6 @@
+cmd_vel_topic: "/stretch_diff_drive_controller/cmd_vel"
+odom_topic: "/stretch_diff_drive_controller/odom"
+scan_topic: "/scan"
+pointcloud_topic: "/camera/depth/image_raw"
+rgb_topic: "/camera/color/image_raw"
+rgb_camera_info: "/camera/color/camera_info"

stretch_rtab/launch/gazebo/gazebo.launch

@@ -0,0 +1,55 @@
+<launch>
+
+  <arg name="paused" default="false"/>
+  <arg name="use_sim_time" default="true"/>
+  <arg name="gui" default="true"/>
+  <arg name="headless" default="false"/>
+  <arg name="debug" default="false"/>
+  <arg name="rviz" default="false"/>
+  <arg name="model" default="$(find stretch_gazebo)/urdf/stretch_gazebo.urdf.xacro"/>
+
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="/usr/share/gazebo-11/worlds/willowgarage.world"/>
+    <arg name="debug" value="$(arg debug)" />
+    <arg name="gui" value="$(arg gui)" />
+    <arg name="paused" value="$(arg paused)"/>
+    <arg name="use_sim_time" value="$(arg use_sim_time)"/>
+    <arg name="headless" value="$(arg headless)"/>
+    <arg name="verbose" value="true"/>
+  </include>
+
+  <param name="robot_description" command="$(find xacro)/xacro $(arg model)" />
+
+  <!-- push robot_description to factory and spawn robot in gazebo -->
+  <node name="urdf_spawner" pkg="gazebo_ros" type="spawn_model"
+        args=" -urdf -model robot -param robot_description" respawn="false" output="screen" />
+
+  <node pkg="robot_state_publisher" type="robot_state_publisher"  name="robot_state_publisher">
+    <param name="publish_frequency" type="double" value="30.0" />
+  </node>
+
+  <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" 
+            ns="stretch_joint_state_controller" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/drive_config.yaml"
+            ns="stretch_diff_drive_controller" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/arm.yaml"/>
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/head.yaml" />
+
+  <rosparam command="load"
+            file="$(find stretch_gazebo)/config/gripper.yaml" />
+
+  <node name="stretch_controller_spawner" pkg="controller_manager" type="spawner"
+        args="stretch_joint_state_controller stretch_diff_drive_controller stretch_arm_controller stretch_head_controller stretch_gripper_controller"/>
+
+  <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
+
+</launch>

stretch_rtab/launch/gazebo/teleop_joy.launch

@@ -0,0 +1,12 @@
+<?xml version="1.0"?>
+<launch>
+    <node name="joy" pkg="joy" type="joy_node" output="screen">
+        <param name="dev" value="/dev/input/js0"/>
+        <param name="autorepeat_rate" value="20"/>
+        <param name="deadzone" value="0.05"/>
+    </node>
+
+    <node name="teleop_twist_joy" pkg="teleop_twist_joy" type="teleop_node">
+        <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel"/>
+    </node>
+</launch>

stretch_rtab/launch/move_base.launch

@@ -0,0 +1,29 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/move_base?distro=kinetic -->
+
+  <arg name="config"/>
+  
+  <arg name="cmd_vel_topic"/>
+  <arg name="odom_topic"/>
+
+  <!-- Use global costmap-->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <!-- Include robot specific parameters from the robot config folder -->
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_rtab)/config/costmap_height.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_rtab)/config/costmap_height.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/base_local_planner_params.yaml" command="load" />
+    <rosparam file="$(find stretch_rtab)/config/$(arg config)/global_planner_params.yaml" command="load" />
+    <param name="conservative_reset_dist" type="double" value="3.0" />
+    <param name="controller_frequency" type="double" value="7.0" /> <!-- 20 -->
+    <param name="planner_patience" value="10.0" />
+    <param name="max_planning_retries" value="20" />
+
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)" />
+    <remap from="odom" to="$(arg odom_topic)" />
+  </node>
+
+</launch>

stretch_rtab/launch/robot.launch

@@ -0,0 +1,36 @@
+<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>
+    <!-- -->
+
+    <!-- FRUSTUM FIELD OF VIEW VISUALIZATION -->
+    <node name="d435i_frustum_visualizer" pkg="stretch_core" type="d435i_frustum_visualizer" output="screen" />
+    <!-- -->
+
+    <!-- STRETCH DRIVER -->
+    <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+    <param name="/stretch_driver/mode" type="string" value="navigation" />
+    <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"/>
+    <!-- -->
+
+    <!-- IMU FILTER -->
+    <!-- <include file="$(find stretch_core)/launch/imu_filter.launch" /> -->
+    <!-- -->
+
+    <!-- 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" />
+    <!-- -->
+
+    <!-- ROBOT LOCALIZATION FILTER -->
+    <include file="$(find stretch_core)/launch/stretch_ekf.launch" />
+    <!-- -->
+</launch>

stretch_rtab/launch/rtab.launch

@@ -0,0 +1,57 @@
+<launch>
+  <arg name="localization"/>
+  <arg name="args"/>
+  <arg name="database_path"/>
+  <arg name="wait_for_transform"/> 
+  <arg name="pointcloud_topic"/>
+  <arg name="cmd_vel_topic"/>
+  <arg name="odom_topic"/>
+  <arg name="scan"/>
+  <arg name="rgb_topic"/>
+  <arg name="rgb_camera_info"/>
+  <!-- Mapping -->
+  <group ns="rtabmap">
+
+    <node name="rtabmap" pkg="rtabmap_ros" type="rtabmap" output="screen" args="$(arg args)">
+      <param name="database_path"       type="string" value="$(arg database_path)"/>
+      <param name="frame_id"            type="string" value="base_link"/>
+      <param name="odom_frame_id"       type="string" value="odom"/>
+      <param name="odom_tf_angular_variance" type="double" value="0.005"/>
+      <param name="odom_tf_linear_variance"  type="double" value="0.005"/>
+      <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
+      <param name="subscribe_depth"     type="bool"   value="true"/>
+      <param name="subscribe_scan"      type="bool"   value="true"/>
+    
+      <!-- inputs -->
+      <remap from="scan"            to="$(arg scan)"/>
+      <remap from="rgb/image"       to="$(arg rgb_topic)"/>
+      <remap from="depth/image"     to="$(arg pointcloud_topic)"/>
+      <remap from="rgb/camera_info" to="$(arg rgb_camera_info)"/>
+      
+      <!-- output -->
+      <remap from="grid_map" to="/map"/>
+    
+      <!-- RTAB-Map's parameters: do "rosrun rtabmap rtabmap (double-dash)params" to see the list of available parameters. -->
+      <param name="RGBD/ProximityBySpace"        type="string" value="true"/>   
+      <param name="RGBD/OptimizeFromGraphEnd"    type="string" value="false"/>  
+      <param name="Kp/MaxDepth"                  type="string" value="4.0"/>
+      <param name="Reg/Strategy"                 type="string" value="1"/>      
+      <param name="Icp/CoprrespondenceRatio"     type="string" value="0.3"/>
+      <param name="Vis/MinInliers"               type="string" value="6"/>      
+      <param name="Vis/InlierDistance"           type="string" value="0.1"/>    
+      <param name="RGBD/AngularUpdate"           type="string" value="0.1"/>    
+      <param name="RGBD/LinearUpdate"            type="string" value="0.1"/>    
+      <param name="Rtabmap/TimeThr"              type="string" value="700"/>
+      <param name="Mem/RehearsalSimilarity"      type="string" value="0.30"/>
+      <param name="Reg/Force3DoF"                type="string" value="true"/>  
+      <param name="Grid/FromDepth"               type="string" value="true"/>
+      <param name="RGBD/ProximityPathMaxNeighbors" type="string" value="10"/> 
+      
+    </node>
+    
+    <param     if="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="false"/>
+    <param unless="$(arg localization)" name="Mem/IncrementalMemory" type="string" value="true"/>
+    <param name="Mem/InitWMWithAllNodes" type="string" value="$(arg localization)"/>
+  </group>
+
+</launch>

stretch_rtab/launch/rviz_rtab.launch

@@ -0,0 +1,9 @@
+<launch>
+  <arg name="mapping" />
+  <group if="$(arg mapping)">
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find stretch_rtab)/rviz/rtab_mapping.rviz"/>
+  </group>
+  <group unless="$(arg mapping)">
+    <node pkg="rviz" type="rviz" name="rviz" args="-d $(find stretch_rtab)/rviz/rtab_navigation.rviz"/>
+  </group>
+</launch>

stretch_rtab/launch/start_rtab.launch

@@ -0,0 +1,65 @@
+<launch>
+  <arg name="sim" />
+  <arg name="localization" default="false" />
+
+  <arg name="database_path"     default="rtabmap.db"/>
+  <arg name="args"              default="-d"/>  <!--Possible arg -d to delete database on startup-->
+
+  <arg name="wait_for_transform"  default="0.2"/> 
+  <arg name="move_base_config"/>
+
+
+  <group if="$(arg sim)">
+    <arg name="pointcloud_topic" default="/realsense/depth/image_raw"/>
+    <arg name="cmd_vel_topic" value="/stretch_diff_drive_controller/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/realsense/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/realsense/color/camera_info"/>
+
+    <include file="$(find stretch_rtab)/launch/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+      <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    <include file="$(find stretch_rtab)/launch/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="args" value="$(arg args)"/>
+      <arg name="database_path"     default="$(arg database_path)"/>
+      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
+      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
+      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+      <arg name="odom_topic" default="$(arg odom_topic)"/>
+      <arg name="scan" default="$(arg scan_topic)"/>
+      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
+      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
+    </include>
+  </group>
+  <group unless="$(arg sim)">
+    <arg name="pointcloud_topic" value="/camera/aligned_depth_to_color/image_raw"/>
+    <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+    <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+    <arg name="scan_topic" default="/scan"/>
+    <arg name="rgb_topic" default="/camera/color/image_raw"/>
+    <arg name="rgb_camera_info" default="/camera/color/camera_info"/>
+
+    <include file="$(find stretch_rtab)/launch/robot.launch"/>
+    <include file="$(find stretch_rtab)/launch/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)"/>
+      <arg name="odom_topic" value="$(arg odom_topic)"/>
+    </include>
+    <include file="$(find stretch_rtab)/launch/rtab.launch">
+      <arg name="localization" value="$(arg localization)"/>
+      <arg name="args" value="$(arg args)"/>
+      <arg name="database_path"     default="$(arg database_path)"/>
+      <arg name="wait_for_transform"  default="$(arg wait_for_transform)"/> 
+      <arg name="pointcloud_topic" value="$(arg pointcloud_topic)"/>
+      <arg name="cmd_vel_topic" value="/stretch/cmd_vel"/>
+      <arg name="odom_topic" default="$(arg odom_topic)"/>
+      <arg name="scan" default="$(arg scan_topic)"/>
+      <arg name="rgb_topic" default="$(arg rgb_topic)"/>
+      <arg name="rgb_camera_info" default="$(arg rgb_camera_info)"/>
+    </include>
+  </group>
+</launch>

stretch_rtab/package.xml

@@ -0,0 +1,58 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>stretch_rtab</name>
+  <version>0.0.0</version>
+  <description>The stretch_rtab package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="csrobot@todo.todo">csrobot</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/stretch_rtab</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <exec_depend>move_base</exec_depend>
+  <exec_depend>rtabmap_ros</exec_depend>
+  <exec_depend>global_planner</exec_depend>
+  <exec_depend>dwa_local_planner</exec_depend>
+
+
+</package>

stretch_rtab/rviz/rtab_mapping.rviz

@@ -0,0 +1,398 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Map2
+      Splitter Ratio: 0.5
+    Tree Height: 1150
+  - 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
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: MapCloud
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - Alpha: 1
+      Class: rtabmap_ros/MapGraph
+      Enabled: true
+      Global loop closure: 255; 0; 0
+      Landmark: 0; 128; 0
+      Local loop closure: 255; 255; 0
+      Merged neighbor: 255; 170; 0
+      Name: MapGraph
+      Neighbor: 0; 0; 255
+      Queue Size: 10
+      Topic: /rtabmap/mapGraph
+      Unreliable: false
+      User: 255; 0; 0
+      Value: true
+      Virtual: 255; 0; 255
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap_ros/MapCloud
+      Cloud decimation: 4
+      Cloud from scan: false
+      Cloud max depth (m): 4
+      Cloud min depth (m): 0
+      Cloud voxel size (m): 0.009999999776482582
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Enabled: true
+      Filter ceiling (m): 0
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0
+      Position Transformer: XYZ
+      Queue Size: 10
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /rtabmap/mapData
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Queue Size: 10
+      Topic: /move_base/global_costmap/footprint
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: rtabmap_ros/OrbitOriented
+      Distance: 10.57433032989502
+      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: -3.853421926498413
+        Y: 0.7817280888557434
+        Z: 0.4164792597293854
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7997970581054688
+      Target Frame: <Fixed Frame>
+      Yaw: 2.288580894470215
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd00000004000000000000015600000508fc0200000007fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c00000508000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003d000000810000005c00fffffffb0000000a0056006900650077007300000000c400000317000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000134fc0100000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000000007800000000000000000fb0000000800540069006d00650000000000000007800000047700fffffffb0000000800540069006d00650100000000000004500000000000000000000008a40000050800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: true
+  Views:
+    collapsed: true
+  Width: 2560
+  X: 0
+  Y: 27

stretch_rtab/rviz/rtab_navigation.rviz

@@ -0,0 +1,374 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Path1
+      Splitter Ratio: 0.5
+    Tree Height: 1082
+  - 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
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      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:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        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_wrist_yaw:
+          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
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /rtabmap/grid_prob_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /rtabmap/mapPath
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    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: 24.471881866455078
+      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: -2.2477076053619385
+        Y: -1.80465567111969
+        Z: -6.365629196166992
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 1.2835423946380615
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004c4000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000004c4000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 27

vs.gitignore

@@ -0,0 +1,9 @@
+.vscode/*
+!.vscode/settings.json
+!.vscode/tasks.json
+!.vscode/launch.json
+!.vscode/extensions.json
+*.code-workspace
+
+# Local History for Visual Studio Code
+.history/