Readme for speed benchmark (#1648)

* FCL Bullet benchmark readme
  * Benchmark script and launch file updated

* PR review fixups

moveit_ros/planning/launch/collision_checker_compare.launch

@@ -1,10 +1,16 @@
 <launch>
+  <arg name="visualization" default="false" doc="turns on and off visualization with rviz"/>
+
   <!-- send Panda urdf to parameter server -->
   <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" />
 
   <include file="$(find panda_moveit_config)/launch/planning_context.launch"/>
 
   <node name="compare_collision_checking_speed" pkg="moveit_ros_planning" type="moveit_compare_collision_checking_speed_fcl_bullet" respawn="false" output="screen">
     <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/>
+    <param name="visualization" value="$(arg visualization)" type="bool"/>
   </node>
+
+  <include if="$(arg visualization)" file="$(find panda_moveit_config)/launch/demo.launch"/>
+
 </launch>

moveit_ros/planning/planning_components_tools/README.md

@@ -0,0 +1,22 @@
+# Planning Component Tools
+
+## Compare collision checking speeds: FCL vs Bullet
+The launch file `collision_checker_compare.launch` starts a benchmark between FCL and Bullet as a collision detection library. The script `compare_collision_speed_checking_fcl_bullet.cpp` performs the actual speed tests.
+
+The collision scenarios can be visualized. By default, visualization is turned off in the launch file but can be activated through `visualization:=true` as an argument to the `roslaunch`. If activated, rviz is launched and visualizations like in the images below are created. For the speed test, either meshes or other available shape primitives like the box can be used.
+
+The clutter world objects are randomly placed such that they don't collide with the robot. The robot can then be moved to a colliding configuration. In the images, the red color of world objects indicates a collision with the robot.
+
+<p align="center">
+    <img src="images/collision_speed_100_meshes_no_collision.png" width="400">
+    <img src="images/collision_speed_100_meshes_collision.png" width="400">
+    <img src="images/collision_speed_100_box_collision.png" width="400">
+</p>
+
+The results of the speed test are printed to the console. Their unit is collision checks per second. A current overview over the results is given in the table below. Note that absolute values are not relevant because they are depending on the computing power of the local machine but the relative differences between Bullet and FCL should be reproducible.
+
+| Collision environment | Bullet | FCL |
+| :--------------- |------------:| ----:|
+| Robot self check, no collision |  220,000 | 110,000 |
+| World clutter 100 meshes, no collision | 37,000 | 38,000 |
+| World clutter 100 meshes, 4 in collision | 8,000 | 3,000 |

moveit_ros/planning/planning_components_tools/src/compare_collision_speed_checking_fcl_bullet.cpp

@@ -45,30 +45,37 @@
 #include <moveit/utils/robot_model_test_utils.h>
 
 static const std::string ROBOT_DESCRIPTION = "robot_description";
+
+/** \brief Factor to compute the maximum number of trials random clutter generation. */
 static const int MAX_SEARCH_FACTOR_CLUTTER = 3;
+
+/** \brief Factor to compute the maximum number of trials for random state generation. */
 static const int MAX_SEARCH_FACTOR_STATES = 30;
 
+/** \brief Defines a random robot state. */
 enum class RobotStateSelector
 {
   IN_COLLISION,
   NOT_IN_COLLISION,
   RANDOM,
 };
 
+/** \brief Enumerates the available collision detectors. */
 enum class CollisionDetector
 {
   FCL,
   BULLET,
 };
 
+/** \brief Enumerates the different types of collision objects. */
 enum class CollisionObjectType
 {
   MESH,
   BOX,
 };
 
 /** \brief Clutters the world of the planning scene with random objects in a certain area around the origin. All added
- *  objects are not in collision.
+ *  objects are not in collision with the robot.
 *
 *   \param planning_scene The planning scene
 *   \param num_objects The number of objects to be cluttered
@@ -188,8 +195,11 @@ void runCollisionDetection(unsigned int trials, const planning_scene::PlanningSc
   if (!only_self)
   {
     req.contacts = true;
-    req.max_contacts = 999;
-    req.max_contacts_per_pair = 99;
+    req.max_contacts = 99;
+    req.max_contacts_per_pair = 10;
+    // If distance is turned on it will slow down the collision checking a lot. Try reducing the
+    // number of contacts consequently.
+    // req.distance = true;
   }
 
   ros::WallTime start = ros::WallTime::now();
@@ -286,7 +296,7 @@ int main(int argc, char** argv)
 
   ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
 
-  unsigned int trials = 5000;
+  unsigned int trials = 10000;
 
   ros::AsyncSpinner spinner(1);
   spinner.start();
@@ -331,6 +341,7 @@ int main(int argc, char** argv)
     runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::BULLET, false);
     runCollisionDetection(trials, planning_scene, sampled_states, CollisionDetector::FCL, false);
 
+    // bring the robot into a position which collides with the world clutter
     double joint_2 = 1.5;
     current_state.setJointPositions("panda_joint2", &joint_2);
     current_state.update();
@@ -342,9 +353,15 @@ int main(int argc, char** argv)
     runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::BULLET, false);
     runCollisionDetection(trials, planning_scene, sampled_states_2, CollisionDetector::FCL, false);
 
-    moveit_msgs::PlanningScene planning_scene_msg;
-    planning_scene->getPlanningSceneMsg(planning_scene_msg);
-    planning_scene_diff_publisher.publish(planning_scene_msg);
+    bool visualize;
+    node_handle.getParam("/compare_collision_checking_speed/visualization", visualize);
+    if (visualize)
+    {
+      // publishes the planning scene to visualize in rviz if possible
+      moveit_msgs::PlanningScene planning_scene_msg;
+      planning_scene->getPlanningSceneMsg(planning_scene_msg);
+      planning_scene_diff_publisher.publish(planning_scene_msg);
+    }
   }
   else
   {