moveit · Ridhwanluthra · Jun 3, 2018 · Jun 3, 2018 · Jun 4, 2018 · Jun 4, 2018
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -18,6 +18,9 @@ find_package(catkin REQUIRED
     moveit_ros_planning_interface
     pluginlib
     geometric_shapes
+    pcl_ros
+    pcl_conversions
+    rosbag
 )
 
 find_package(Boost REQUIRED system filesystem date_time thread)
@@ -61,3 +64,5 @@ add_subdirectory(doc/move_group_python_interface)
 add_subdirectory(doc/state_display)
 add_subdirectory(doc/interactivity)
 add_subdirectory(doc/pick_place)
+add_subdirectory(doc/perception_pipeline)
+
diff --git a/doc/chomp_planner/chomp_planner_tutorial.rst b/doc/chomp_planner/chomp_planner_tutorial.rst
@@ -67,7 +67,7 @@ To run CHOMP in an evironment with obstacles, you can run the sample python scri
 
   :codedir:`collision_scene_example.py<collision_environments/scripts/collision_scene_example.py>`.
 
-This scripts creates a cluttered scene with four ostacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene.
+This scripts creates a cluttered scene with four obstacles or a simple scene with one obstacle depending on the argument given to the script. One can also change the position/size of the obstacles to change the scene.
 
 To run the CHOMP planner with obstacles, open two shells. In the first shell start RViz and wait for everything to finish loading: ::
 

diff --git a/doc/collision_environments/scripts/collision_scene_example.py b/doc/collision_environments/scripts/collision_scene_example.py
@@ -72,6 +72,7 @@ def add_box_object(self, name, dimensions, pose):
     if (len(sys.argv) != 2):
         print "Correct usage:: \n\"rosrun moveit_tutorials collision_scene_example.py cluttered\" OR \n\"rosrun moveit_tutorials collision_scene_example.py sparse\""
         sys.exit()
+
     if sys.argv[1] == "cluttered":
         load_scene.add_four_boxes();
     elif sys.argv[1] == "sparse":

diff --git a/doc/perception_pipeline/CMakeLists.txt b/doc/perception_pipeline/CMakeLists.txt
@@ -0,0 +1,5 @@
+add_executable(cylinder_segment src/cylinder_segment.cpp)
+target_link_libraries(cylinder_segment ${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})
diff --git a/doc/perception_pipeline/bags/perception_tutorial.bag b/doc/perception_pipeline/bags/perception_tutorial.bag
diff --git a/doc/perception_pipeline/cylinder_collision_object.png b/doc/perception_pipeline/cylinder_collision_object.png
diff --git a/doc/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch b/doc/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch
@@ -0,0 +1,7 @@
+<launch>
+  <include file="$(find moveit_tutorials)/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch" />
+
+  <!-- Run the detection and adding cylinder node -->
+  <node pkg="moveit_tutorials" type="cylinder_segment" name="point_cloud_preprocessor" />
+
+</launch>
diff --git a/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch b/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch
@@ -0,0 +1,11 @@
+<launch>
+  <include file="$(find panda_moveit_config)/launch/demo.launch" />
+
+  <!-- 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="tf" type="static_transform_publisher" name="to_temp_link" args="0 0.4 -0.6 0 0 0  temp_link panda_link0 10" />
+  <node pkg="tf" type="static_transform_publisher" name="to_panda_base" args="0 0 0 0 0.2 1.92 camera_rgb_optical_frame temp_link 10" />
+
+</launch>
diff --git a/doc/perception_pipeline/obstacle_avoidance.gif b/doc/perception_pipeline/obstacle_avoidance.gif
diff --git a/doc/perception_pipeline/perception_configuration_demo.png b/doc/perception_pipeline/perception_configuration_demo.png
diff --git a/...ion/perception_configuration_tutorial.rst → ...pipeline/perception_pipeline_tutorial.rst b/...ion/perception_configuration_tutorial.rst → ...pipeline/perception_pipeline_tutorial.rst
@@ -1,5 +1,18 @@
-Perception/Configuration
-=================================
+Perception Pipeline Tutorial
+============================
+
+MoveIt! allows for seamless integration of 3D sensors using `Octomap <http://octomap.github.io/>`_.
+Once properly configured, you should see something like this in rviz:
+
+.. image:: perception_configuration_demo.png
+   :width: 700px
+
+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
+-------------
 
 In this section, we will walk through configuring the 3D sensors on your robot with MoveIt!. The primary component in MoveIt! that deals with 3D perception is the Occupancy Map Updater. The updater uses a plugin architecture to process different types of input. The currently available plugins in MoveIt! are:
 
@@ -8,13 +21,14 @@ In this section, we will walk through configuring the 3D sensors on your robot w
 * The Depth Image Occupancy Map Updater: which can take as input Depth Images (``sensor_msgs/Image``)
 
 YAML Configuration file (Point Cloud)
--------------------------------------
++++++++++++++++++++++++++++++++++++++
 
-We will have to generate a YAML configuration file for configuring the 3D sensors. Please see `this example file <https://github.com/ros-planning/panda_moveit_config/blob/master/config/sensors_kinect_pointcloud.yaml>`_ for processing point clouds, located in the `panda_moveit_config repository for Kinetic <https://github.com/ros-planning/panda_moveit_config>`_: ::
+We will have to generate a YAML configuration file for configuring the 3D sensors. Please see `this example file <https://github.com/ros-planning/panda_moveit_config/blob/master/config/sensors_kinect_pointcloud.yaml>`_ for processing point clouds, located in the `panda_moveit_config repository for Kinetic <https://github.com/ros-planning/panda_moveit_config>`_.
+Save this file in the config folder in the robot's moveit_config package with name "sensors_kinect_pointcloud.yaml": ::
 
  sensors:
    - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
-     point_cloud_topic: /head_mount_kinect/depth_registered/points
+     point_cloud_topic: /camera/depth_registered/points
      max_range: 5.0
      point_subsample: 1
      padding_offset: 0.1
@@ -43,13 +57,14 @@ We will have to generate a YAML configuration file for configuring the 3D sensor
 
 
 YAML Configuration file (Depth Map)
------------------------------------
++++++++++++++++++++++++++++++++++++
 
-We will have to generate a YAML configuration file for configuring the 3D sensors. An `example file for processing depth images <https://github.com/ros-planning/panda_moveit_config/blob/master/config/sensors_kinect_depthmap.yaml>`_ can be found in the `panda_moveit_config repository <https://github.com/ros-planning/panda_moveit_config>`_ as well: ::
+We will have to generate a YAML configuration file for configuring the 3D sensors. An `example file for processing depth images <https://github.com/ros-planning/panda_moveit_config/blob/master/config/sensors_kinect_depthmap.yaml>`_ can be found in the `panda_moveit_config repository <https://github.com/ros-planning/panda_moveit_config>`_ as well.
+Save this file in the config folder in the robot's moveit_config package with name "sensors_kinect_depthmap.yaml": ::
 
  sensors:
    - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater
-     image_topic: /head_mount_kinect/depth_registered/image_raw
+     image_topic: /camera/depth_registered/image_raw
      queue_size: 5
      near_clipping_plane_distance: 0.3
      far_clipping_plane_distance: 5.0
@@ -84,14 +99,15 @@ We will have to generate a YAML configuration file for configuring the 3D sensor
 
 
 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.yaml" />
+ <rosparam command="load" file="$(find panda_moveit_config)/config/sensors_kinect_pointcloud.yaml" />
 
+If you are using depthmap change the name of the yaml file to ``sensors_kinect_depthmap.yaml``.
 Note that you will need to input the path to the right file you have created above.
 
 Octomap Configuration
@@ -106,3 +122,64 @@ MoveIt! uses an octree-based framework to represent the world around it. The *Oc
  * *octomap_frame*: specifies the coordinate frame in which this representation will be stored. If you are working with a mobile robot, this frame should be a fixed frame in the world.
  * *octomap_resolution*: specifies the resolution at which this representation is maintained (in meters).
  * *max_range*: specifies the maximum range value to be applied for any sensor input to this node.
+
+Obstacle Avoidance
+------------------
+
+If you set the initial and the final location of the robot in a way that there is no straight path between them, then the planner will automatically avoid the octomap and plan around it.
+
+.. image:: obstacle_avoidance.gif
+   :width: 700px
+
+Running the Interface
++++++++++++++++++++++
+Roslaunch the launch file to run the code directly from moveit_tutorials: ::
+
+ roslaunch moveit_tutorials obstacle_avoidance_demo.launch
+
+you should see something like the image shown at the beginning of this tutorial.
+
+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>`_
+
+Detecting and Adding Object as Collision Object
+-----------------------------------------------
+
+In this section, we will demonstrate an example of extracting a cylinder from a pointcloud, computing relevant values and adding it as a collision object to the planning scene. 
+We will be working with point clouds but it can be implemented similarly with depth maps.
+
+After running the code, you should be able to see something like this in rviz:
+
+.. image:: cylinder_collision_object.png
+   :width: 700px
+
+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
+
+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>`_
+
+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>`_
+
+.. |br| raw:: html
+
+   <br />
+
+.. |code_start| raw:: html
+
+   <code>
+
+.. |code_end| raw:: html
+
+   </code>
+
+.. tutorial-formatter:: ./src/cylinder_segment.cpp
diff --git a/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp b/doc/perception_pipeline/src/bag_publisher_maintain_time.cpp
@@ -0,0 +1,82 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+*  Copyright (c) 2018, Ridhwan Luthra.
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of Ridhwan Luthra nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/* Author: Ridhwan Luthra */
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <rosbag/bag.h>
+#include <rosbag/view.h>
+#include <sensor_msgs/PointCloud2.h>
+
+int main(int argc, char** argv)
+{
+  ros::init(argc, argv, "bag_publisher_maintain_time");
+  ros::NodeHandle nh;
+
+  ros::Publisher point_cloud_publisher = nh.advertise<sensor_msgs::PointCloud2>("/camera/depth_registered/points", 1);
+  ros::Rate loop_rate(0.1);
+
+  // Variable holding the rosbag containing point cloud data.
+  rosbag::Bag bagfile;
+  std::string path = ros::package::getPath("moveit_tutorials");
+  path += "/doc/perception_pipeline/bags/perception_tutorial.bag";
+  bagfile.open(path, rosbag::bagmode::Read);
+
+  std::vector<std::string> topics;
+  topics.push_back("/camera/depth_registered/points");
+
+  // Iterator for topics in bag.
+  rosbag::View bagtopics_iter(bagfile, rosbag::TopicQuery(topics));
+
+  for (auto const msg : bagtopics_iter)
+  {
+    sensor_msgs::PointCloud2::Ptr point_cloud_ptr = msg.instantiate<sensor_msgs::PointCloud2>();
+    if (point_cloud_ptr == NULL)
+    {
+      std::cout << "error" << std::endl;
+      break;
+    }
+
+    while (ros::ok())
+    {
+      point_cloud_ptr->header.stamp = ros::Time::now();
+      point_cloud_publisher.publish(*point_cloud_ptr);
+      ros::spinOnce();
+      loop_rate.sleep();
+    }
+  }
+  bagfile.close();
+  return 0;
+}