diff --git a/doc/mesh_filter/Filtered_Depth.png b/doc/mesh_filter/Filtered_Depth.png
index 8f600f38b..d5e327dc7 100644
Binary files a/doc/mesh_filter/Filtered_Depth.png and b/doc/mesh_filter/Filtered_Depth.png differ
diff --git a/doc/mesh_filter/MeshFilter.gif b/doc/mesh_filter/MeshFilter.gif
index 0e2a8aec4..f390f8561 100644
Binary files a/doc/mesh_filter/MeshFilter.gif and b/doc/mesh_filter/MeshFilter.gif differ
diff --git a/doc/mesh_filter/Model_Depth.png b/doc/mesh_filter/Model_Depth.png
index 91ce74160..ae3d7ec13 100644
Binary files a/doc/mesh_filter/Model_Depth.png and b/doc/mesh_filter/Model_Depth.png differ
diff --git a/doc/mesh_filter/launch/mesh_filter.launch b/doc/mesh_filter/launch/mesh_filter.launch
deleted file mode 100644
index a0ca6a2f1..000000000
--- a/doc/mesh_filter/launch/mesh_filter.launch
+++ /dev/null
@@ -1,12 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-
-  <include file="$(dirname)/ur5_gazebo.launch"/>
-
-  <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
-  <node pkg="nodelet" type="nodelet" name="mesh_filter" args="load mesh_filter/DepthSelfFiltering standalone_nodelet" output="screen">
-    <remap to="/camera/depth/image_raw" from="/depth"/>
-    <remap to="/camera/color/camera_info" from="/camera_info"/>
-  </node>
-
-</launch>
diff --git a/doc/mesh_filter/launch/start_mesh_filter.launch b/doc/mesh_filter/launch/start_mesh_filter.launch
new file mode 100644
index 000000000..e64c0661c
--- /dev/null
+++ b/doc/mesh_filter/launch/start_mesh_filter.launch
@@ -0,0 +1,15 @@
+<?xml version="1.0"?>
+<launch>
+  <!-- Start Mesh Filter -->
+  <node pkg="nodelet" type="nodelet" name="standalone_nodelet" args="manager" output="screen"/>
+  <node pkg="nodelet" type="nodelet" name="mesh_filter" args="load mesh_filter/DepthSelfFiltering standalone_nodelet" output="screen">
+    <remap to="/camera/depth/image_raw" from="/depth"/>
+    <remap to="/camera/depth/camera_info" from="/camera_info"/>
+    <param name="near_clipping_plane_distance" value="0.05"/>
+    <param name="far_clipping_plane_distance" value="1.5"/>
+    <param name="shadow_threshold" value="0.1"/>
+    <param name="padding_scale" value="1.0"/>
+    <param name="padding_offset" value="0.001"/>
+  </node>
+
+</launch>
diff --git a/doc/mesh_filter/launch/start_panda.launch b/doc/mesh_filter/launch/start_panda.launch
new file mode 100644
index 000000000..85db747bb
--- /dev/null
+++ b/doc/mesh_filter/launch/start_panda.launch
@@ -0,0 +1,65 @@
+<?xml version="1.0"?>
+<launch>
+  <arg name="arm_id" default="panda"/>
+  <arg name="robot" default="panda"/>
+  <arg name="paused"      default="false" doc="Should the simulation directly be stopped at 0s?" />
+  <arg name="initial_joint_positions"
+       doc="Initial joint configuration of the panda. Specify as a list of name/value pairs in form of '-J [name-of-joint] [value-in-rad]'. Default is a 90 degree bend in the elbow"
+       default="-J panda_joint1 0
+                -J panda_joint2 -0.785398163
+                -J panda_joint3 0
+                -J panda_joint4 -2.35619449
+                -J panda_joint5 0
+                -J panda_joint6 1.57079632679
+                -J panda_joint7 0.785398163397
+                -J panda_finger_joint1 0.001
+                -J panda_finger_joint2 0.001"
+       />
+  <arg name="controller"  default="effort_joint_trajectory_controller"/>
+
+  <!-- Start Gazebo -->
+  <include file="$(find gazebo_ros)/launch/empty_world.launch">
+    <arg name="world_name" value="worlds/empty.world"/>
+    <!-- Always start in paused mode, and only unpause when spawning the model -->
+    <arg name="paused" value="true"/>
+    <arg name="gui" value="false"/>
+    <arg name="use_sim_time" value="true"/>
+  </include>
+
+  <!-- Load robot description -->
+  <param name="robot_description" command="$(find xacro)/xacro '$(find moveit_tutorials)/doc/mesh_filter/urdf/panda_sensor.xacro'"/>
+
+  <!-- Load Controllers -->
+  <rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
+  <rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" />
+
+  <!-- Spawn Robot -->
+  <arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
+  <node name="panda_model_spawner"
+        pkg="gazebo_ros"
+        type="spawn_model"
+        args="-param robot_description -urdf -model panda $(arg unpause)
+              $(arg initial_joint_positions)
+              "/>
+
+  <!-- spawns the controller after the robot was put into its initial joint pose -->
+  <node pkg="controller_manager"
+        type="spawner"
+        name="panda_controller_spawner"
+        respawn="false" output="screen"
+        args="--wait-for initialized franka_state_controller $(arg controller)"
+  />
+
+  <!-- Robot state publisher -->
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher"/>
+  <node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
+    <rosparam param="source_list">[franka_state_controller/joint_states] </rosparam>
+    <param name="rate" value="30"/>
+  </node>
+
+
+  <!-- Start rviz -->
+  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
+    args="-d $(find moveit_tutorials)/doc/mesh_filter/rviz/mesh_filter.rviz" output="screen">
+  </node>
+</launch>
diff --git a/doc/mesh_filter/launch/ur5_gazebo.launch b/doc/mesh_filter/launch/ur5_gazebo.launch
deleted file mode 100644
index e7a5c260a..000000000
--- a/doc/mesh_filter/launch/ur5_gazebo.launch
+++ /dev/null
@@ -1,29 +0,0 @@
-<?xml version="1.0"?>
-<launch>
-  <arg name="limited" default="false"  doc="If true, limits joint range [-PI, PI] on all joints." />
-  <arg name="paused" default="false" doc="Starts gazebo in paused mode" />
-  <arg name="gui" default="true" doc="Starts gazebo gui" />
-
-  <!-- startup simulated world -->
-  <include file="$(find gazebo_ros)/launch/empty_world.launch">
-    <arg name="gui" value="$(arg gui)"/>
-  </include>
-
-  <param name="robot_description" command="$(find xacro)/xacro '$(find moveit_tutorials)/doc/mesh_filter/urdf/ur5_sensor.urdf.xacro' "/>>
-
-  <!-- push robot_description to factory and spawn robot in gazebo -->
-  <node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
-
-  <include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
-
-  <!-- start this controller -->
-  <rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
-  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" args="spawn arm_controller" respawn="false" output="screen"/>
-
-  <!-- load other controllers -->
-  <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false" output="screen" args="load joint_group_position_controller" />
-
-  <node name="$(anon rviz)" pkg="rviz" type="rviz" respawn="false"
-    args="-d $(find moveit_tutorials)/doc/mesh_filter/rviz/mesh_filter.rviz" output="screen">
-  </node>
-</launch>
diff --git a/doc/mesh_filter/mesh_filter_tutorial.rst b/doc/mesh_filter/mesh_filter_tutorial.rst
index e5de7858f..3629add9b 100644
--- a/doc/mesh_filter/mesh_filter_tutorial.rst
+++ b/doc/mesh_filter/mesh_filter_tutorial.rst
@@ -1,10 +1,10 @@
 Mesh Filter with UR5 and Kinect
 ===============================
 
-MoveIt's mesh filter functionality removes your robot's geometry from a point cloud! If your robot's arm is in your depth sensor's view, the points associated with the arm are subtracted from the point cloud.
+MoveIt's mesh filter functionality removes your robot's geometry from a depth image! If your robot's arm is in your depth sensor's view, the pixels associated with the arm are subtracted from the depth image.
 
-This is accomplished by giving the original point cloud, the robot's transforms (``\tf``) and the robot's URDF as inputs.
-The filter then publishes a modified point cloud which does not contain the points that overlaps with the current robot state.
+This is accomplished by giving the original depth image, the robot's transforms (``\tf``) and the robot's URDF as inputs.
+The filter then publishes a modified depth image which does not contain the pixels that overlaps with the current robot state.
 
 .. image:: MeshFilter.gif
 
@@ -14,98 +14,31 @@ Getting Started
 * Follow the instructions for :moveit_website:`installing MoveIt<install>`
   first if you have not already done that.
 
-* Clone the `Universal Robot package <https://github.com/ros-industrial/universal_robot>`_ to your workspace for Melodic. Remember to rebuild your workspace after cloning.
-
+* Install the franka_panda package for ROS `sudo apt-get install ros-$ROS_DISTRO-franka-ros`
 * Install `Gazebo <http://classic.gazebosim.org/tutorials?tut=install_ubuntu&cat=install>`_. You need to install ros-${ROS_DISTRO}-gazebo-plugins too.
 
 
 Running the Demo
 -----------------
 
-Roslaunch the launch file to run the code directly from moveit_tutorials: ::
+Roslaunch the launch filis a depth image masked to remove pixels that overlaps with the robot state.e to run the code directly from moveit_tutorials: ::
+
+ roslaunch moveit_tutorials start_panda.launch
 
- roslaunch moveit_tutorials mesh_filter.launch
+ roslaunch moveit_tutorials start_mesh_filter.launch
 
-The above command opens a UR5 arm with Kinect sensor on Gazebo and Rviz
+The above command opens a Panda arm with Kinect sensor in Gazebo and Rviz
 
-Topic ``/filtered/depth`` produces the modified point cloud with points subtracted that overlaps with the robot state.
+Topic ``/filtered/depth`` is a depth image masked to remove pixels that overlap with the robot state.
 
 .. image:: Filtered_Depth.png
 
-Topic ``/model/depth`` gives the points that overlap with the current robot state
+Topic ``/model/depth`` is a depth image masked to show only pixels that overlap with the robot state.
 
 .. image:: Model_Depth.png
 
 Check out the mesh filter code `here <https://github.com/moveit/moveit/blob/master/moveit_ros/perception/mesh_filter/src/depth_self_filter_nodelet.cpp>`_
 
-
-How to add sensor to arm in simulation
---------------------------------------
-
-Include sensor plugin in a ``.gazebo`` file. In this tutorial, a kinect sensor plugin is added to ``kinect_camera.gazebo`` ::
-
-    <gazebo reference="camera_depth_frame">
-        <sensor name="kinect_camera" type="depth">
-            <update_rate>20</update_rate>
-            <camera>
-                <horizontal_fov>1.047198</horizontal_fov>
-                <image>
-                    <width>640</width>
-                    <height>480</height>
-                    <format>B8G8R8</format>
-                </image>
-                <clip>
-                    <near>0.05</near>
-                    <far>3</far>
-                </clip>
-            </camera>
-            <plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
-            <baseline>0.1</baseline>
-            <alwaysOn>true</alwaysOn>
-            <updateRate>10</updateRate>
-            <cameraName>camera_ir</cameraName>
-            <imageTopicName>/camera/color/image_raw</imageTopicName>
-            <cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
-            <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
-            <depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
-            <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
-            <frameName>camera_depth_optical_frame</frameName>
-            <distortion_k1>0.0</distortion_k1>
-            <distortion_k2>0.0</distortion_k2>
-            <distortion_k3>0.0</distortion_k3>
-            <distortion_t1>0.0</distortion_t1>
-            <distortion_t2>0.0</distortion_t2>
-            <pointCloudCutoff>0.16</pointCloudCutoff>
-            <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
-            </plugin>
-        </sensor>
-    </gazebo>
-
-
-Attach sensor to `base urdf <https://github.com/ros-industrial/universal_robot/blob/melodic-devel/ur_description/urdf/ur5.xacro>`_ of UR5 using links and joints as shown in ``ur5_sensor.urdf.xacro`` ::
-
-    <!-- ur5 -->
-    <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
-
-    <!-- Attach UR5 to table -->
-    <joint name="table_joint" type="fixed">
-        <parent link="table"/>
-        <child link="base_link"/>
-    </joint>
-
-    <!-- Attach Kinect to table -->
-    <joint type="fixed" name="table_camera_joint">
-        <origin xyz="0.15 -1 0.1" rpy="0 0 1.57"/>
-        <child link="camera_rgb_frame"/>
-        <parent link="table"/>
-        <axis xyz="0 0 0" rpy="0 0 0"/>
-        <limit effort="10000" velocity="1000"/>
-        <dynamics damping="1.0" friction="1.0"/>
-    </joint>
-
-
-
-
 References
 ----------
 `Understanding ROS Nodelets <https://medium.com/@waleedmansoor/understanding-ros-nodelets-c43a11c8169e>`_
diff --git a/doc/mesh_filter/rviz/mesh_filter.rviz b/doc/mesh_filter/rviz/mesh_filter.rviz
index cf76c1004..69d4b971e 100644
--- a/doc/mesh_filter/rviz/mesh_filter.rviz
+++ b/doc/mesh_filter/rviz/mesh_filter.rviz
@@ -6,9 +6,10 @@ Panels:
       Expanded:
         - /Global Options1
         - /Status1
+        - /DepthCloud1
         - /DepthCloud1/Auto Size1
       Splitter Ratio: 0.5
-    Tree Height: 755
+    Tree Height: 731
   - Class: rviz/Selection
     Name: Selection
   - Class: rviz/Tool Properties
@@ -24,7 +25,6 @@ Panels:
     Name: Views
     Splitter Ratio: 0.5
   - Class: rviz/Time
-    Experimental: false
     Name: Time
     SyncMode: 0
     SyncSource: DepthCloud
@@ -53,6 +53,23 @@ Visualization Manager:
       Plane Cell Count: 10
       Reference Frame: <Fixed Frame>
       Value: true
+    - Class: rviz/TF
+      Enabled: false
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
     - Alpha: 1
       Class: rviz/RobotModel
       Collision Enabled: false
@@ -63,84 +80,130 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
-        base:
+        camera_depth_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        base_link:
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        camera_depth_frame:
+        camera_rgb_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_depth_optical_frame:
+        camera_rgb_optical_frame:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_link:
+        panda_hand:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        camera_rgb_frame:
+        panda_hand_sc:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        camera_rgb_optical_frame:
+        panda_hand_tcp:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_leftfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        ee_link:
+          Value: true
+        panda_link0:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        forearm_link:
+        panda_link0_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link1:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        shoulder_link:
+        panda_link1_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link2:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        table:
+        panda_link2_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link3:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        tool0:
+        panda_link3_sc:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        upper_arm_link:
+        panda_link4:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        world:
+        panda_link4_sc:
           Alpha: 1
           Show Axes: false
           Show Trail: false
-        wrist_1_link:
+        panda_link5:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        wrist_2_link:
+        panda_link5_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link6:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
-        wrist_3_link:
+        panda_link6_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
           Alpha: 1
           Show Axes: false
           Show Trail: false
           Value: true
+        world:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
@@ -164,14 +227,12 @@ Visualization Manager:
       Color Transformer: RGB8
       Color Transport Hint: raw
       Decay Time: 0
-      Depth Map Topic: /camera/depth/image_raw
+      Depth Map Topic: /model/depth
       Depth Map Transport Hint: raw
       Enabled: true
       Invert Rainbow: false
       Max Color: 255; 255; 255
-      Max Intensity: 4096
       Min Color: 0; 0; 0
-      Min Intensity: 0
       Name: DepthCloud
       Occlusion Compensation:
         Occlusion Time-Out: 30
@@ -185,55 +246,35 @@ 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
-      Max Intensity: 4096
-      Min Color: 0; 0; 0
-      Min Intensity: 0
-      Name: PointCloud2
-      Position Transformer: XYZ
-      Queue Size: 10
-      Selectable: true
-      Size (Pixels): 3
-      Size (m): 0.009999999776482582
-      Style: Flat Squares
-      Topic: /camera/depth/points
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /model/depth
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Model Depth
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
       Unreliable: false
-      Use Fixed Frame: true
-      Use rainbow: true
-      Value: false
-    - Class: rviz/TF
-      Enabled: false
-      Frame Timeout: 15
-      Frames:
-        All Enabled: true
-      Marker Scale: 1
-      Name: TF
-      Show Arrows: true
-      Show Axes: true
-      Show Names: true
-      Tree:
-        {}
-      Update Interval: 0
-      Value: false
+      Value: true
+    - Class: rviz/Image
+      Enabled: true
+      Image Topic: /filtered/depth
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Filtered Depth
+      Normalize Range: true
+      Queue Size: 2
+      Transport Hint: raw
+      Unreliable: false
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
     Default Light: true
-    Fixed Frame: base_link
+    Fixed Frame: panda_link0
     Frame Rate: 30
   Name: root
   Tools:
@@ -257,33 +298,37 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.395120620727539
+      Distance: 2.25454044342041
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
+      Field of View: 0.7853981852531433
       Focal Point:
-        X: 0
-        Y: 0
-        Z: 0
+        X: -0.03617127612233162
+        Y: 0.1707955151796341
+        Z: 0.2093578726053238
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.4803980886936188
+      Pitch: 0.4653978943824768
       Target Frame: <Fixed Frame>
-      Value: Orbit (rviz)
-      Yaw: 5.158586025238037
+      Yaw: 5.458542346954346
     Saved: ~
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1052
+  Filtered Depth:
+    collapsed: false
+  Height: 1376
   Hide Left Dock: false
-  Hide Right Dock: false
-  QMainWindow State: 000000ff00000000fd0000000400000000000002890000037efc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000037e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006100000002c6000000f50000000000000000000000010000010f0000037efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000037e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d0065010000000000000780000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003dc0000037e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Hide Right Dock: true
+  Model Depth:
+    collapsed: false
+  QMainWindow State: 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
   Selection:
     collapsed: false
   Time:
@@ -291,7 +336,7 @@ Window Geometry:
   Tool Properties:
     collapsed: false
   Views:
-    collapsed: false
-  Width: 1920
-  X: 0
-  Y: 0
+    collapsed: true
+  Width: 2488
+  X: 72
+  Y: 27
diff --git a/doc/mesh_filter/urdf/kinect_camera.gazebo b/doc/mesh_filter/urdf/kinect_camera.gazebo
deleted file mode 100644
index 1e484d6ca..000000000
--- a/doc/mesh_filter/urdf/kinect_camera.gazebo
+++ /dev/null
@@ -1,39 +0,0 @@
-<?xml version="1.0"?>
-<robot>
-<gazebo reference="camera_depth_frame">
-  <sensor name="kinect_camera" type="depth">
-    <update_rate>20</update_rate>
-    <camera>
-      <horizontal_fov>1.047198</horizontal_fov>
-      <image>
-        <width>640</width>
-        <height>480</height>
-        <format>B8G8R8</format>
-      </image>
-      <clip>
-        <near>0.05</near>
-        <far>3</far>
-      </clip>
-    </camera>
-    <plugin name="kinect_controller" filename="libgazebo_ros_openni_kinect.so">
-      <baseline>0.1</baseline>
-      <alwaysOn>true</alwaysOn>
-      <updateRate>10</updateRate>
-      <cameraName>camera_ir</cameraName>
-      <imageTopicName>/camera/color/image_raw</imageTopicName>
-      <cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
-      <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
-      <depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
-      <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
-      <frameName>camera_depth_optical_frame</frameName>
-      <distortion_k1>0.0</distortion_k1>
-      <distortion_k2>0.0</distortion_k2>
-      <distortion_k3>0.0</distortion_k3>
-      <distortion_t1>0.0</distortion_t1>
-      <distortion_t2>0.0</distortion_t2>
-      <pointCloudCutoff>0.16</pointCloudCutoff>
-      <pointCloudCutoffMax>10.0</pointCloudCutoffMax>
-    </plugin>
-  </sensor>
-</gazebo>
-</robot>
diff --git a/doc/mesh_filter/urdf/ur5_sensor.urdf.xacro b/doc/mesh_filter/urdf/panda_sensor.xacro
similarity index 50%
rename from doc/mesh_filter/urdf/ur5_sensor.urdf.xacro
rename to doc/mesh_filter/urdf/panda_sensor.xacro
index 2891e24a2..f0ded5c95 100644
--- a/doc/mesh_filter/urdf/ur5_sensor.urdf.xacro
+++ b/doc/mesh_filter/urdf/panda_sensor.xacro
@@ -1,64 +1,28 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://wiki.ros.org/xacro"
-       name="ur5" >
-
-  <xacro:arg name="transmission_hw_interface" default="hardware_interface/PositionJointInterface"/>
-  <xacro:arg name="resource_dir" default=""/>
-
-  <!-- common stuff -->
-  <xacro:include filename="$(find ur_description)/urdf/common.gazebo.xacro" />
-
-  <!-- Add sensor -->
-  <include filename="$(find moveit_tutorials)/doc/mesh_filter/urdf/kinect_camera.gazebo" />
-
-  <!-- ur5 -->
-  <xacro:include filename="$(find ur_description)/urdf/ur5.urdf.xacro" />
-
-  <xacro:ur5_robot prefix="" joint_limited="false"
-    transmission_hw_interface="$(arg transmission_hw_interface)"/>
-
-  <link name="world" />
-
-  <link name="table">
-    <visual>
-      <origin rpy="0 0 0" xyz="0 -0.45 -0.03"/>
-      <geometry>
-        <box size="0.6 1.2 0.05"/>
-      </geometry>
-      <material name="grey"/>
-    </visual>
-    <surface>
-      <friction>
-        <ode>
-          <mu>5</mu>
-          <mu2>5</mu2>
-        </ode>
-      </friction>
-    </surface>
-    <collision name='camera_collision'>
-      <origin rpy="0 0 0" xyz="0 -0.45 -0.03"/>
-      <geometry>
-        <box size="0.6 1.2 0.05"/>
-      </geometry>
-    </collision>
-  </link>
-
-  <joint name="world_table_joint" type="fixed">
-    <parent link="world"/>
-    <child link="table"/>
-  </joint>
-
-  <!-- Attach UR5 to table -->
-  <joint name="table_joint" type="fixed">
-    <parent link="table"/>
-    <child link="base_link"/>
-  </joint>
-
-  <!-- Attach Kinect to table -->
-  <joint type="fixed" name="table_camera_joint">
-    <origin xyz="0.15 -1 0.1" rpy="0 0 1.57"/>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
+
+  <!-- Include Franka Panda -->
+  <xacro:arg name="hand" default="true"/>
+  <xacro:arg name="gazebo" default="true"/>
+  <xacro:arg name="arm_id" default="panda"/>
+  <xacro:arg name="x" default="0"/>
+  <xacro:arg name="y" default="0"/>
+  <xacro:arg name="z" default="0"/>
+  <xacro:arg name="roll" default="0"/>
+  <xacro:arg name="pitch" default="0"/>
+  <xacro:arg name="yaw" default="0"/>
+  <xacro:include filename="$(find franka_description)/robots/panda/panda.urdf.xacro"/>
+
+  <!-- Define the grey material -->
+  <material name="grey">
+    <color rgba="0.5 0.5 0.5 1.0"/>
+  </material>
+
+<!-- Attach Kinect to world -->
+  <joint type="fixed" name="world_camera_joint">
+    <origin xyz="0.15 -1 0.5" rpy="0 0 1.57"/>
     <child link="camera_rgb_frame"/>
-    <parent link="table"/>
+    <parent link="world"/>
     <axis xyz="0 0 0" rpy="0 0 0"/>
     <limit effort="10000" velocity="1000"/>
     <dynamics damping="1.0" friction="1.0"/>
@@ -144,4 +108,57 @@
     </inertial>
   </link>
 
+  <!-- GAZEBO PLUGIN -->
+  <!-- Left/Depth camera -->
+  <gazebo reference="camera_link">
+    <material> Gazebo/DarkGrey </material>
+  </gazebo>
+
+  <gazebo reference="camera_depth_frame">
+    <sensor type="depth" name="kinect_camera">
+      <always_on>true</always_on>
+      <update_rate>15.0</update_rate>
+      <visualize>false</visualize>
+      <camera name="kinect_camera">
+        <horizontal_fov>1.7633</horizontal_fov>
+        <image>
+          <height>720</height>
+          <width>1280</width>
+          <format>R8G8B8</format>
+        </image>
+        <clip>
+          <near>0.1</near>
+          <far>300</far>
+        </clip>
+        <noise>
+          <type>gaussian</type>
+          <mean>0.0</mean>
+          <stddev>0.007</stddev>
+        </noise>
+      </camera>
+
+      <plugin name="camera_controller" filename="libgazebo_ros_openni_kinect.so">
+        <cameraName>camera_ir</cameraName>
+        <imageTopicName>left/image_rect_color</imageTopicName>
+        <imageTopicName>/camera/color/image_raw</imageTopicName>
+        <cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
+        <depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
+        <depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
+        <pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
+        <frameName>camera_depth_optical_frame</frameName>
+        <pointCloudCutoff>0.1</pointCloudCutoff>
+        <distortionK1>0</distortionK1>
+        <distortionK2>0</distortionK2>
+        <distortionK3>0</distortionK3>
+        <distortionT1>0</distortionT1>
+        <distortionT2>0</distortionT2>
+        <CxPrime>0</CxPrime>
+        <Cx>0</Cx>
+        <Cy>0</Cy>
+        <focalLength>0</focalLength>
+        <hackBaseline>0</hackBaseline>
+      </plugin>
+    </sensor>
+  </gazebo>
 </robot>
+