From 54d5e3c81679406f93e2d4a56ed2c9ebf542721f Mon Sep 17 00:00:00 2001 From: "Isaac I.Y. Saito" <130s@2000.jukuin.keio.ac.jp> Date: Wed, 28 Jun 2023 13:51:27 -0400 Subject: [PATCH] Improve: perception: debug comments --- .../perception_pipeline_tutorial.rst | 73 +++++++++++-------- ...and_add_cylinder_collision_object_demo.cpp | 8 +- 2 files changed, 49 insertions(+), 32 deletions(-) diff --git a/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst b/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst index e1768cd4ce..a25417df69 100644 --- a/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst +++ b/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst @@ -11,7 +11,14 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. -Particularly, use ``rolling`` for this tutorial as some dependencies may not be available in ``humble`` or earlier (explained in `moveit2_tutorials!700 `_). +* NOTE-1: Particularly, use ``rolling`` for this tutorial as some dependencies may not be available in ``humble`` or earlier (explained in `moveit2_tutorials!700 `_). +* NOTE-2: As of June 2023, the tutorial code is based on unmerged software changes in the upstream repo. `moveit_resources!181 `_. Before it's merged, you need to have its source and build by commands e.g. :: + + cd ~/ws_moveit/src + git clone https://github.com/130s/moveit_resources.git -b feature-panda-moveit-perception + cd ~/ws_moveit + sudo apt update && rosdep install -r --from-paths . --ignore-src --rosdistro $ROS_DISTRO -y + colcon build --mixin release Configuration ------------- @@ -27,20 +34,22 @@ To use the Occupancy Map Updater, it is only necessary to set the appropriate pa YAML Configuration file (Point Cloud) +++++++++++++++++++++++++++++++++++++ -We will have to generate a YAML configuration file for configuring the 3D sensors. Please see :panda_codedir:`this example file ` for processing point clouds. +We will have to generate a YAML configuration file for configuring the 3D sensors. Please see :panda_codedir:`this example file ` for processing point clouds. -Save this file in the config folder in the robot's moveit_config package with name "sensors_kinect_pointcloud.yaml": :: +Save this file in the config folder in the robot's moveit_config package with name "sensors_3d.yaml": :: - sensors: - - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /camera/depth_registered/points - max_range: 5.0 - point_subsample: 1 - padding_offset: 0.1 - padding_scale: 1.0 - max_update_rate: 1.0 - filtered_cloud_topic: filtered_cloud - ns: kinect + sensors: + - kinect_pointcloud + kinect_pointcloud: + filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + ns: kinect + padding_offset: 0.1 + padding_scale: 0.5 + point_cloud_topic: /camera/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater **The general parameters are:** @@ -66,21 +75,25 @@ Save this file in the config folder in the robot's moveit_config package with na YAML Configuration file (Depth Map) +++++++++++++++++++++++++++++++++++ -We will have to generate a YAML configuration file for configuring the 3D sensors. An :panda_codedir:`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: /camera/depth_registered/image_raw - queue_size: 5 - near_clipping_plane_distance: 0.3 - far_clipping_plane_distance: 5.0 - shadow_threshold: 0.2 - padding_scale: 4.0 - padding_offset: 0.03 - max_update_rate: 1.0 - filtered_cloud_topic: filtered_cloud - ns: kinect +We will have to generate a YAML configuration file for configuring the 3D sensors. An :panda_codedir:`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_3d.yaml": :: + + sensors: + - kinect_depthimage + kinect_depthimage: + far_clipping_plane_distance: 5.0 + filtered_cloud_topic: filtered_cloud + image_topic: /camera/depth_registered/image_raw + max_update_rate: 1.0 + near_clipping_plane_distance: 0.3 + ns: kinect + padding_offset: 0.03 + padding_scale: 4.0 + queue_size: 5 + sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater + shadow_threshold: 0.2 + skip_vertical_pixels: 4 + skip_horizontal_pixels: 6 **The general parameters are:** @@ -114,9 +127,9 @@ Add the YAML file to the launch script ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ You will now need to create a *sensor_manager.launch* file in the "launch" directory of your panda_moveit_config directory (e.g. `on github `_) with this sensor information. 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``. +If you are using depthmap change the name of the yaml file to ``sensors_3d.yaml``. Note that you will need to input the path to the right file you have created above. Octomap Configuration diff --git a/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp b/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp index 7ccfc8e762..26262a5ab7 100644 --- a/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp +++ b/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp @@ -303,15 +303,19 @@ class CylinderSegment : public rclcpp::Node pcl::fromROSMsg(*input, *cloud); // Use a passthrough filter to get the region of interest. // The filter removes points outside the specified range. + RCLCPP_DEBUG(this->get_logger(), "Before passThroughFilter"); passThroughFilter(cloud); // Compute point normals for later sample consensus step. pcl::PointCloud::Ptr cloud_normals(new pcl::PointCloud); + RCLCPP_DEBUG(this->get_logger(), "Before computeNormals"); 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 remove points on the (planar) surface on which the cylinder is resting. + RCLCPP_DEBUG(this->get_logger(), "Before removePlaneSurface"); removePlaneSurface(cloud, inliers_plane); // Remove surface points from normals as well + RCLCPP_DEBUG(this->get_logger(), "Before extractNormals"); 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\ |code_end|\ . @@ -325,11 +329,11 @@ class CylinderSegment : public rclcpp::Node // END_SUB_TUTORIAL if (cloud->points.empty() || coefficients_cylinder->values.size() != 7) { - RCLCPP_ERROR(this->get_logger(), "detect_and_add_cylinder_collision_object_demo, can't find the cylindrical component."); + RCLCPP_ERROR(this->get_logger(), "Can't find the cylindrical component. Returning the callback."); return; } - RCLCPP_INFO(this->get_logger(), "Detected Cylinder - Adding CollisionObject to PlanningScene"); + RCLCPP_INFO(this->get_logger(), "Detected a cylinder - Adding CollisionObject to PlanningScene"); // BEGIN_TUTORIAL // CALL_SUB_TUTORIAL callback