Dev/fixing mesh filter tutorial

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>

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>

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>`_