diff --git a/CMakeLists.txt b/CMakeLists.txt index f984409af..98e5943da 100644 --- 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 index 4247e4449..ccf4d9ced 100644 --- 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`. -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 index 4710f0013..213d4ce3b 100755 --- 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 new file mode 100644 index 000000000..90ce99605 --- /dev/null +++ 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 new file mode 100644 index 000000000..d52936a1c Binary files /dev/null and b/doc/perception_pipeline/bags/perception_tutorial.bag differ diff --git a/doc/perception_pipeline/cylinder_collision_object.png b/doc/perception_pipeline/cylinder_collision_object.png new file mode 100644 index 000000000..bd6845742 Binary files /dev/null and b/doc/perception_pipeline/cylinder_collision_object.png differ 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 new file mode 100644 index 000000000..7a326ee15 --- /dev/null +++ b/doc/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch @@ -0,0 +1,7 @@ + + + + + + + \ No newline at end of file diff --git a/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch b/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch new file mode 100644 index 000000000..273338e4f --- /dev/null +++ b/doc/perception_pipeline/launch/obstacle_avoidance_demo.launch @@ -0,0 +1,11 @@ + + + + + + + + + + + \ No newline at end of file diff --git a/doc/perception_pipeline/obstacle_avoidance.gif b/doc/perception_pipeline/obstacle_avoidance.gif new file mode 100644 index 000000000..69d1c9579 Binary files /dev/null and b/doc/perception_pipeline/obstacle_avoidance.gif differ diff --git a/doc/perception_pipeline/perception_configuration_demo.png b/doc/perception_pipeline/perception_configuration_demo.png new file mode 100644 index 000000000..fd5087331 Binary files /dev/null and b/doc/perception_pipeline/perception_configuration_demo.png differ diff --git a/doc/perception_configuration/perception_configuration_tutorial.rst b/doc/perception_pipeline/perception_pipeline_tutorial.rst similarity index 57% rename from doc/perception_configuration/perception_configuration_tutorial.rst rename to doc/perception_pipeline/perception_pipeline_tutorial.rst index 732f75593..4d0d2cf0a 100644 --- a/doc/perception_configuration/perception_configuration_tutorial.rst +++ b/doc/perception_pipeline/perception_pipeline_tutorial.rst @@ -1,5 +1,18 @@ -Perception/Configuration -================================= +Perception Pipeline Tutorial +============================ + +MoveIt! allows for seamless integration of 3D sensors using `Octomap `_. +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 `_ for processing point clouds, located in the `panda_moveit_config repository for Kinetic `_: :: +We will have to generate a YAML configuration file for configuring the 3D sensors. Please see `this example file `_ for processing point clouds, located in the `panda_moveit_config repository for Kinetic `_. +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 `_ can be found in the `panda_moveit_config repository `_ as well: :: +We will have to generate a YAML configuration file for configuring the 3D sensors. An `example file for processing depth images `_ can be found in the `panda_moveit_config repository `_ 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: :: - + +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 `_ + +Relevant Code ++++++++++++++ +The entire code can be seen :codedir:`here ` 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. `_ + +.. |br| raw:: html + +
+ +.. |code_start| raw:: html + + + +.. |code_end| raw:: html + + + +.. 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 new file mode 100644 index 000000000..151a03664 --- /dev/null +++ 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 +#include +#include +#include + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "bag_publisher_maintain_time"); + ros::NodeHandle nh; + + ros::Publisher point_cloud_publisher = nh.advertise("/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 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(); + 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; +} diff --git a/doc/perception_pipeline/src/cylinder_segment.cpp b/doc/perception_pipeline/src/cylinder_segment.cpp new file mode 100644 index 000000000..d44819f22 --- /dev/null +++ b/doc/perception_pipeline/src/cylinder_segment.cpp @@ -0,0 +1,368 @@ +/********************************************************************* +* 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 +#include +#include +#include +#include +#include +#include + +#include +#include + +class CylinderSegment +{ +public: + CylinderSegment() + { + ros::NodeHandle nh; + // Initialize subscriber to the raw point cloud + ros::Subscriber sub = nh.subscribe("/camera/depth_registered/points", 1, &CylinderSegment::cloudCB, this); + // Spin + ros::spin(); + } + + /** \brief Given the parameters of the cylinder add the cylinder to the planning scene. + @param cylinder_params - Pointer to the struct AddCylinderParams. */ + void addCylinder() + { + moveit::planning_interface::PlanningSceneInterface planning_scene_interface; + // BEGIN_SUB_TUTORIAL add_cylinder + // + // Adding Cylinder to Planning Scene + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // Define a collision object ROS message. + moveit_msgs::CollisionObject collision_object; + collision_object.header.frame_id = "camera_rgb_optical_frame"; + collision_object.id = "cylinder"; + + // Define a cylinder which will be added to the world. + shape_msgs::SolidPrimitive primitive; + primitive.type = primitive.CYLINDER; + primitive.dimensions.resize(2); + /* Setting height of cylinder. */ + primitive.dimensions[0] = cylinder_params->height; + /* Setting radius of cylinder. */ + primitive.dimensions[1] = cylinder_params->radius; + + // Define a pose for the cylinder (specified relative to frame_id). + geometry_msgs::Pose cylinder_pose; + /* Computing and setting quaternion from axis angle representation. */ + Eigen::Vector3d cylinder_z_direction(cylinder_params->direction_vec[0], cylinder_params->direction_vec[1], + cylinder_params->direction_vec[2]); + Eigen::Vector3d origin_z_direction(0., 0., 1.); + Eigen::Vector3d axis; + axis = origin_z_direction.cross(cylinder_z_direction); + axis.normalize(); + double angle = acos(cylinder_z_direction.dot(origin_z_direction)); + cylinder_pose.orientation.x = axis.x() * sin(angle / 2); + cylinder_pose.orientation.y = axis.y() * sin(angle / 2); + cylinder_pose.orientation.z = axis.z() * sin(angle / 2); + cylinder_pose.orientation.w = cos(angle / 2); + + // Setting the position of cylinder. + cylinder_pose.position.x = cylinder_params->center_pt[0]; + cylinder_pose.position.y = cylinder_params->center_pt[1]; + cylinder_pose.position.z = cylinder_params->center_pt[2]; + + // Add cylinder as collision object + collision_object.primitives.push_back(primitive); + collision_object.primitive_poses.push_back(cylinder_pose); + collision_object.operation = collision_object.ADD; + std::vector collision_objects; + collision_objects.push_back(collision_object); + planning_scene_interface.applyCollisionObjects(collision_objects); + // END_SUB_TUTORIAL + } + + /** \brief Given the pointcloud containing just the cylinder, compute its center point and its height and store in + cylinder_params. + @param cloud - Pointcloud containing just the cylinder. + @param cylinder_params - Pointer to the struct AddCylinderParams. */ + void extractLocationHeight(pcl::PointCloud::Ptr cloud) + { + double max_angle_y = 0.0; + double min_angle_y = std::numeric_limits::infinity(); + + double lowest_point[3]; + double highest_point[3]; + // BEGIN_SUB_TUTORIAL extract_location_height + // Consider a point inside the point cloud and imagine that point is formed on a XY plane where the perpendicular + // distance from the plane to the camera is Z. |br| + // The perpendicular drawn from the camera to the plane hits at center of the XY plane. |br| + // We have the x and y coordinate of the point which is formed on the XY plane. |br| + // X is the horizontal axis and Y is the vertical axis. |br| + // C is the center of the plane which is Z meter away from the center of camera and A is any point on the plane. + // |br| + // Now we know Z is the perpendicular distance from the point to the camera. |br| + // If you need to find the actual distance d from the point to the camera, you should calculate the hypotenuse- + // |code_start| hypot(point.z, point.x);\ |code_end| |br| + // angle the point made horizontally- |code_start| atan2(point.z,point.x);\ |code_end| |br| + // angle the point made Vertically- |code_start| atan2(point.z, point.y);\ |code_end| |br| + // Loop over the entire pointcloud. + for (auto const point : cloud->points) + { + /* Find the coordinates of the highest point */ + if (atan2(point.z, point.y) < min_angle_y) + { + min_angle_y = atan2(point.z, point.y); + lowest_point[0] = point.x; + lowest_point[1] = point.y; + lowest_point[2] = point.z; + } + /* Find the coordinates of the lowest point */ + else if (atan2(point.z, point.y) > max_angle_y) + { + max_angle_y = atan2(point.z, point.y); + highest_point[0] = point.x; + highest_point[1] = point.y; + highest_point[2] = point.z; + } + } + /* Store the center point of cylinder */ + cylinder_params->center_pt[0] = (highest_point[0] + lowest_point[0]) / 2; + cylinder_params->center_pt[1] = (highest_point[1] + lowest_point[1]) / 2; + cylinder_params->center_pt[2] = (highest_point[2] + lowest_point[2]) / 2; + /* Store the height of cylinder */ + cylinder_params->height = + sqrt(pow((lowest_point[0] - highest_point[0]), 2) + pow((lowest_point[1] - highest_point[1]), 2) + + pow((lowest_point[2] - highest_point[2]), 2)); + // END_SUB_TUTORIAL + } + + /** \brief Given a pointcloud extract the ROI defined by the user. + @param cloud - Pointcloud whose ROI needs to be extracted. */ + void passThroughFilter(pcl::PointCloud::Ptr cloud) + { + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName("z"); + // min and max values in z axis to keep + pass.setFilterLimits(0.3, 1.1); + pass.filter(*cloud); + } + + /** \brief Given the pointcloud and pointer cloud_normals compute the point normals and store in cloud_normals. + @param cloud - Pointcloud. + @param cloud_normals - The point normals once computer will be stored in this. */ + void computeNormals(pcl::PointCloud::Ptr cloud, pcl::PointCloud::Ptr cloud_normals) + { + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); + pcl::NormalEstimation ne; + ne.setSearchMethod(tree); + ne.setInputCloud(cloud); + // Set the number of k nearest neighbors to use for the feature estimation. + ne.setKSearch(50); + ne.compute(*cloud_normals); + } + + /** \brief Given the point normals and point indices, extract the normals for the indices. + @param cloud_normals - Point normals. + @param inliers_plane - Indices whose normals need to be extracted. */ + void extractNormals(pcl::PointCloud::Ptr cloud_normals, pcl::PointIndices::Ptr inliers_plane) + { + pcl::ExtractIndices extract_normals; + extract_normals.setNegative(true); + extract_normals.setInputCloud(cloud_normals); + extract_normals.setIndices(inliers_plane); + extract_normals.filter(*cloud_normals); + } + + /** \brief Given the pointcloud and indices of the plane, remove the plannar region from the pointcloud. + @param cloud - Pointcloud. + @param inliers_plane - Indices representing the plane. */ + void removePlaneSurface(pcl::PointCloud::Ptr cloud, pcl::PointIndices::Ptr inliers_plane) + { + // create a SAC segmenter without using normals + pcl::SACSegmentation segmentor; + segmentor.setOptimizeCoefficients(true); + segmentor.setModelType(pcl::SACMODEL_PLANE); + segmentor.setMethodType(pcl::SAC_RANSAC); + /* run at max 1000 iterations before giving up */ + segmentor.setMaxIterations(1000); + /* tolerance for variation from model */ + segmentor.setDistanceThreshold(0.01); + segmentor.setInputCloud(cloud); + /* Create the segmentation object for the planar model and set all the parameters */ + pcl::ModelCoefficients::Ptr coefficients_plane(new pcl::ModelCoefficients); + segmentor.segment(*inliers_plane, *coefficients_plane); + /* Extract the planar inliers from the input cloud */ + pcl::ExtractIndices extract_indices; + extract_indices.setInputCloud(cloud); + extract_indices.setIndices(inliers_plane); + /* Remove the planar inliers, extract the rest */ + extract_indices.setNegative(true); + extract_indices.filter(*cloud); + } + + /** \brief Given the pointcloud, pointer to pcl::ModelCoefficients and point normals extract the cylinder from the + pointcloud and store the cylinder parameters in coefficients_cylinder. + @param cloud - Pointcloud whose plane is removed. + @param coefficients_cylinder - Cylinder parameters used to define an infinite cylinder will be stored here. + @param cloud_normals - Point normals corresponding to the plane on which cylinder is kept */ + void extractCylinder(pcl::PointCloud::Ptr cloud, pcl::ModelCoefficients::Ptr coefficients_cylinder, + pcl::PointCloud::Ptr cloud_normals) + { + // Create the segmentation object for cylinder segmentation and set all the parameters + pcl::SACSegmentationFromNormals segmentor; + pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices); + segmentor.setOptimizeCoefficients(true); + segmentor.setModelType(pcl::SACMODEL_CYLINDER); + segmentor.setMethodType(pcl::SAC_RANSAC); + // Set the normal angular distance weight + segmentor.setNormalDistanceWeight(0.1); + // run at max 1000 iterations before giving up + segmentor.setMaxIterations(10000); + // tolerance for variation from model + segmentor.setDistanceThreshold(0.05); + // min max values of radius in meters to consider + segmentor.setRadiusLimits(0, 1); + segmentor.setInputCloud(cloud); + segmentor.setInputNormals(cloud_normals); + + // Obtain the cylinder inliers and coefficients + segmentor.segment(*inliers_cylinder, *coefficients_cylinder); + + // Extract the cylinder inliers from the input cloud + pcl::ExtractIndices extract; + extract.setInputCloud(cloud); + extract.setIndices(inliers_cylinder); + extract.setNegative(false); + extract.filter(*cloud); + } + + void cloudCB(const sensor_msgs::PointCloud2ConstPtr& input) + { + // BEGIN_SUB_TUTORIAL callback + // + // Perception Related + // ^^^^^^^^^^^^^^^^^^ + // First, convert from sensor_msgs to pcl::PointXYZRGB which is needed for most of the processing. + pcl::PointCloud::Ptr cloud(new pcl::PointCloud); + pcl::fromROSMsg(*input, *cloud); + // Using passthough filter to get region of interest. A passthrough filter just eliminates the point cloud values + // which do not lie in the user specified range. + passThroughFilter(cloud); + // Declare normals and call function to compute point normals. + pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud); + computeNormals(cloud, cloud_normals); + // inliers_plane will hold the indices of the point cloud that correspond to a plane. + pcl::PointIndices::Ptr inliers_plane(new pcl::PointIndices); + // Detect and eliminate the plane on which the cylinder is kept to ease the process of finding the cylinder. + removePlaneSurface(cloud, inliers_plane); + // We had calculated the point normals in a previous call to computeNormals, + // now we will be extracting the normals that correspond to the plane on which cylinder lies. + // It will be used to extract the cylinder. + extractNormals(cloud_normals, inliers_plane); + // ModelCoefficients will hold the parameters using which we can define a cylinder of infinite length. + // It has a public attribute |code_start| values\ |code_end| of type |code_start| std::vector< float >\ |code_end|\ + // . + // |br| + // |code_start| Values[0-2]\ |code_end| hold a point on the center line of the cylinder. |br| + // |code_start| Values[3-5]\ |code_end| hold direction vector of the z-axis. |br| + // |code_start| Values[6]\ |code_end| is the radius of the cylinder. + pcl::ModelCoefficients::Ptr coefficients_cylinder(new pcl::ModelCoefficients); + /* Extract the cylinder using SACSegmentation. */ + extractCylinder(cloud, coefficients_cylinder, cloud_normals); + // END_SUB_TUTORIAL + if (cloud->points.empty()) + { + ROS_ERROR_STREAM_NAMED("cylinder_segment", "Can't find the cylindrical component."); + return; + } + if (points_not_found) + { + // BEGIN_TUTORIAL + // CALL_SUB_TUTORIAL callback + // + // Storing Relevant Cylinder Values + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // The information that we have in |code_start| coefficients_cylinder\ |code_end| is not enough to define our + // cylinder. + // It does not have the actual location of the cylinder nor the actual height. |br| + // We define a struct to hold the parameters that are actually needed for defining a collision object completely. + // |br| + // CALL_SUB_TUTORIAL param_struct + /* Store the radius of the cylinder. */ + cylinder_params->radius = coefficients_cylinder->values[6]; + /* Store direction vector of z-axis of cylinder. */ + cylinder_params->direction_vec[0] = coefficients_cylinder->values[3]; + cylinder_params->direction_vec[1] = coefficients_cylinder->values[4]; + cylinder_params->direction_vec[2] = coefficients_cylinder->values[5]; + // + // Extracting Location and Height + // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + // Compute the center point of the cylinder using standard geometry + extractLocationHeight(cloud); + // CALL_SUB_TUTORIAL extract_location_height + // Use the parameters extracted to add the cylinder to the planning scene as a collision object. + addCylinder(); + // CALL_SUB_TUTORIAL add_cylinder + // END_TUTORIAL + points_not_found = false; + } + } + +private: + // BEGIN_SUB_TUTORIAL param_struct + // There are 4 fields and a total of 7 parameters used to define this. + struct AddCylinderParams + { + /* Radius of the cylinder. */ + double radius; + /* Direction vector towards the z-axis of the cylinder. */ + double direction_vec[3]; + /* Center point of the cylinder. */ + double center_pt[3]; + /* Height of the cylinder. */ + double height; + }; + // Declare a variable of type AddCylinderParams and store relevant values from ModelCoefficients. + AddCylinderParams* cylinder_params; + // END_SUB_TUTORIAL + + bool points_not_found = true; +}; + +int main(int argc, char** argv) +{ + // Initialize ROS + ros::init(argc, argv, "cylinder_segment"); + // Start the segmentor + CylinderSegment(); +} diff --git a/index.rst b/index.rst index a29a6cd29..d57feb08d 100644 --- a/index.rst +++ b/index.rst @@ -54,7 +54,7 @@ Before attempting to integrate a new robot with MoveIt!, check whether your robo doc/setup_assistant/setup_assistant_tutorial doc/urdf_srdf/urdf_srdf_tutorial doc/controller_configuration/controller_configuration_tutorial - doc/perception_configuration/perception_configuration_tutorial + doc/perception_pipeline/perception_pipeline_tutorial doc/ikfast/ikfast_tutorial doc/trac_ik/trac_ik_tutorial diff --git a/package.xml b/package.xml index 309bbd660..6b7ef79d5 100644 --- a/package.xml +++ b/package.xml @@ -22,6 +22,9 @@ interactive_markers geometric_shapes moveit_visual_tools + pcl_ros + pcl_conversions + rosbag pluginlib moveit_core @@ -31,5 +34,8 @@ interactive_markers moveit_visual_tools joy + pcl_ros + pcl_conversions + rosbag