Improve perception tutorial (#633)

Author: v4hn

doc/perception_pipeline/CMakeLists.txt

@@ -1,13 +1,14 @@
-add_executable(cylinder_segment src/cylinder_segment.cpp)
-target_link_libraries(cylinder_segment ${catkin_LIBRARIES})
+add_executable(detect_and_add_cylinder_collision_object_demo src/detect_and_add_cylinder_collision_object_demo.cpp)
+target_link_libraries(detect_and_add_cylinder_collision_object_demo ${catkin_LIBRARIES})
 
 add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp)
 target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES})
 
 install(
 TARGETS
     bag_publisher_maintain_time
-    cylinder_segment
+    detect_and_add_cylinder_collision_object_demo
 DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION})
 
 install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION})
+install(DIRECTORY bags DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/doc/perception_pipeline/)

doc/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch

@@ -1,11 +1,16 @@
 <launch>
   <include file="$(find panda_moveit_config)/launch/demo.launch" />
+  <!--
+  In this tutorial we wait much longer for the robot transforms than we usually would,
+  because the user's machine might be quite slow.
+  -->
+  <param name="robot_description_planning/shape_transform_cache_lookup_wait_time" value="0.5" />
 
   <!-- Play the rosbag that contains the pointcloud data -->
   <node pkg="moveit_tutorials" type="bag_publisher_maintain_time" name="point_clouds" />
 
   <!-- If needed, broadcast static tf for robot root -->
   <node pkg="tf2_ros" type="static_transform_publisher" name="to_panda" args="0 0 0 0 0 0  world panda_link0" />
-  <node pkg="tf2_ros" type="static_transform_publisher" name="to_camera" args="0.115 0.427 0.570 0 0.2 1.92 camera_rgb_optical_frame world" />
+  <node pkg="tf2_ros" type="static_transform_publisher" name="to_camera" args="0.00 -.40 0.60 0.19 0.07 -1.91 world camera_rgb_optical_frame" />
 
 </launch>

doc/perception_pipeline/launch/obstacle_avoidance_demo.launch

@@ -9,6 +9,7 @@ Once properly configured, you should see something like this in RViz:
 
 Getting Started
 ---------------
+
 If you haven't already done so, make sure you've completed the steps in `Getting Started <../getting_started/getting_started.html>`_.
 
 Configuration
@@ -106,6 +107,7 @@ Update the launch file
 
 Add the YAML file to the launch script
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
 You will now need to update the *sensor_manager.launch* file in the "launch" directory of your panda_moveit_config directory with this sensor information (this file is auto-generated by the Setup Assistant but is empty). You will need to add the following line into that file to configure the set of sensor sources for MoveIt to use: ::
 
  <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
@@ -115,6 +117,7 @@ Note that you will need to input the path to the right file you have created abo
 
 Octomap Configuration
 ^^^^^^^^^^^^^^^^^^^^^
+
 You will also need to configure the `Octomap <http://octomap.github.io/>`_ by adding the following lines into the *sensor_manager.launch*: ::
 
  <param name="octomap_frame" type="string" value="odom_combined" />
@@ -136,7 +139,8 @@ If you set the initial and the final location of the robot in a way that there i
 
 Running the Interface
 +++++++++++++++++++++
-Roslaunch the launch file to run the code directly from moveit_tutorials: ::
+
+Launch the prepared launch file in moveit_tutorials to see the planning scene integrating sample point cloud data into an octomap: ::
 
  roslaunch moveit_tutorials obstacle_avoidance_demo.launch
 
@@ -145,7 +149,7 @@ If not, you may have run into a `known OpenGL rendering issue <http://wiki.ros.o
 
  export LIBGL_ALWAYS_SOFTWARE=1
 
-You can test obstacle avoidance for yourself by setting the goal state manually and then planning and executing. To learn how to do that look at `MoveIt Quickstart in RViz <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_
+You can test obstacle avoidance with the generated octomap for yourself by setting the goal state manually and then planning and executing. To learn how to do that look at `MoveIt Quickstart in RViz <../quickstart_in_rviz/quickstart_in_rviz_tutorial.html>`_
 
 Detecting and Adding Object as Collision Object
 -----------------------------------------------
@@ -160,20 +164,14 @@ After running the code, you should be able to see something like this in RViz:
 
 Running the Code
 ++++++++++++++++
-Roslaunch the launch file to run the code directly from moveit_tutorials: ::
 
- roslaunch moveit_tutorials detect_and_add_cylinder_collision_object_demo.launch
+Keep the launch file from above running and run the code directly from moveit_tutorials: ::
 
-KNOWN ISSUE - You may see the following error when running the demo ::
-
-  ros.moveit_ros_planning.planning_scene_monitor: Transform error: Lookup would require extrapolation into the future.  Requested time 1527473962.793050157 but the latest data is at time 1527473962.776993978, when looking up transform from frame [panda_link2] to frame [camera_rgb_optical_frame]
-  ros.moveit_ros_perception: Transform cache was not updated. Self-filtering may fail.
-
-We are working on fixing it, it should not break the working of the demo.
-You can follow its status in the `issue tracker <https://github.com/ros-planning/moveit_tutorials/issues/192>`_
+ rosrun moveit_tutorials detect_and_add_cylinder_collision_object_demo
 
 Relevant Code
 +++++++++++++
+
 The entire code can be seen :codedir:`here<perception_pipeline>` in the moveit_tutorials GitHub project.
 
 The details regarding the implementation of each of the perception pipeline function have been omitted in this tutorial as they are well documented `here <http://wiki.ros.org/pcl/Tutorials>`_.
@@ -190,4 +188,4 @@ The details regarding the implementation of each of the perception pipeline func
 
    </code>
 
-.. tutorial-formatter:: ./src/cylinder_segment.cpp
+.. tutorial-formatter:: ./src/detect_and_add_cylinder_collision_object_demo.cpp

doc/perception_pipeline/perception_pipeline_tutorial.rst

@@ -44,9 +44,11 @@ int main(int argc, char** argv)
 {
   ros::init(argc, argv, "bag_publisher_maintain_time");
   ros::NodeHandle nh;
+  ros::AsyncSpinner spinner(1);
+  spinner.start();
 
-  ros::Publisher point_cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1);
-  ros::Rate loop_rate(0.1);
+  ros::Publisher point_cloud_publisher =
+      nh.advertise<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 2, true);
 
   // Variable holding the rosbag containing point cloud data.
   rosbag::Bag bagfile;
@@ -58,25 +60,27 @@ int main(int argc, char** argv)
   topics.push_back("/camera/depth_registered/points");
 
   // Iterator for topics in bag.
-  rosbag::View bagtopics_iter(bagfile, rosbag::TopicQuery(topics));
+  rosbag::View bag(bagfile, rosbag::TopicQuery(topics));
 
-  for (auto const msg : bagtopics_iter)
+  sensor_msgs::PointCloud2::Ptr point_cloud_ptr = bag.begin()->instantiate<sensor_msgs::PointCloud2>();
+  if (!point_cloud_ptr)
   {
-    sensor_msgs::PointCloud2::Ptr point_cloud_ptr = msg.instantiate<sensor_msgs::PointCloud2>();
-    if (point_cloud_ptr == nullptr)
-    {
-      std::cout << "error" << std::endl;
-      break;
-    }
+    ROS_FATAL("invalid message in rosbag");
+    return 1;
+  }
+
+  // Give a bit of time to move_group to connect & cache transforms
+  // works around sporadic tf extrapolation errors
+  ros::Duration(1.0).sleep();
 
-    while (ros::ok())
-    {
-      point_cloud_ptr->header.stamp = ros::Time::now();
-      point_cloud_publisher.publish(*point_cloud_ptr);
-      ros::spinOnce();
-      loop_rate.sleep();
-    }
+  ros::Rate loop_rate(0.2);
+  while (ros::ok())
+  {
+    point_cloud_ptr->header.stamp = ros::Time::now();
+    point_cloud_publisher.publish(*point_cloud_ptr);
+    loop_rate.sleep();
   }
+
   bagfile.close();
   return 0;
 }