diff --git a/CMakeLists.txt b/CMakeLists.txt index 31ef0b667e..3df0ee02e0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -58,6 +58,7 @@ add_subdirectory(doc/examples/motion_planning_pipeline) add_subdirectory(doc/examples/motion_planning_python_api) add_subdirectory(doc/examples/move_group_interface) add_subdirectory(doc/examples/moveit_cpp) +add_subdirectory(doc/examples/perception_pipeline) add_subdirectory(doc/examples/planning_scene) add_subdirectory(doc/examples/planning_scene_ros_api) add_subdirectory(doc/examples/realtime_servo) diff --git a/conf.py b/conf.py index 770bd7c73e..c39b207349 100644 --- a/conf.py +++ b/conf.py @@ -217,14 +217,16 @@ "https://github.com/" + html_context["github_user"] + "/panda_moveit_config/blob/" - + "melodic-devel" + + "rolling-devel" + "/%s", "", ), "moveit_resources_codedir": ( "https://github.com/" - + html_context["github_user"] - + "/moveit_resources/tree/ros2" +# + html_context["github_user"] + + "130s" +# + "/moveit_resources/tree/ros2" + + "/moveit_resources/tree/feature-panda-moveit-perception" + "/%s", "", ), # TODO(dlu): use ros_distro when noetic-devel branch is available diff --git a/doc/examples/perception_pipeline/CMakeLists.txt b/doc/examples/perception_pipeline/CMakeLists.txt index fad25d2a80..52dbcb5a2e 100644 --- a/doc/examples/perception_pipeline/CMakeLists.txt +++ b/doc/examples/perception_pipeline/CMakeLists.txt @@ -1,13 +1,23 @@ -add_executable(cylinder_segment src/cylinder_segment.cpp) -target_link_libraries(cylinder_segment ${catkin_LIBRARIES}) +find_package(PCL REQUIRED) +set(SUBTUTORIAL_DEPENDS + geometry_msgs + pcl_conversions + pcl_ros + sensor_msgs +) +foreach(dep IN ITEMS ${SUBTUTORIAL_DEPENDS}) + find_package(${dep} REQUIRED) +endforeach() -add_executable(bag_publisher_maintain_time src/bag_publisher_maintain_time.cpp) -target_link_libraries(bag_publisher_maintain_time ${catkin_LIBRARIES} ${Boost_LIBRARIES}) +include_directories(include ${PCL_INCLUDE_DIRS}) -install( -TARGETS - bag_publisher_maintain_time - cylinder_segment -DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) +set(APP_CYLINDER detect_and_add_cylinder_collision_object_demo) +add_executable(${APP_CYLINDER} src/detect_and_add_cylinder_collision_object_demo.cpp) +ament_target_dependencies(${APP_CYLINDER} + ${SUBTUTORIAL_DEPENDS} + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +install(TARGETS ${APP_CYLINDER} RUNTIME DESTINATION lib/${PROJECT_NAME}) -install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) +install(DIRECTORY bags launch + DESTINATION share/${PROJECT_NAME} +) diff --git a/doc/examples/perception_pipeline/bags/perception_tutorial.bag b/doc/examples/perception_pipeline/bags/perception_tutorial.bag deleted file mode 100644 index d52936a1cc..0000000000 Binary files a/doc/examples/perception_pipeline/bags/perception_tutorial.bag and /dev/null differ diff --git a/doc/examples/perception_pipeline/bags/perception_tutorial.bag/metadata.yaml b/doc/examples/perception_pipeline/bags/perception_tutorial.bag/metadata.yaml new file mode 100644 index 0000000000..7d259b116e --- /dev/null +++ b/doc/examples/perception_pipeline/bags/perception_tutorial.bag/metadata.yaml @@ -0,0 +1,27 @@ +rosbag2_bagfile_information: + compression_format: '' + compression_mode: '' + custom_data: {} + duration: + nanoseconds: 0 + files: + - duration: + nanoseconds: 0 + message_count: 1 + path: perception_tutorial.db3 + starting_time: + nanoseconds_since_epoch: 1530016169214859695 + message_count: 1 + relative_file_paths: + - perception_tutorial.db3 + starting_time: + nanoseconds_since_epoch: 1530016169214859695 + storage_identifier: sqlite3 + topics_with_message_count: + - message_count: 1 + topic_metadata: + name: /camera/depth_registered/points + offered_qos_profiles: '' + serialization_format: cdr + type: sensor_msgs/msg/PointCloud2 + version: 6 diff --git a/doc/examples/perception_pipeline/bags/perception_tutorial.bag/perception_tutorial.db3 b/doc/examples/perception_pipeline/bags/perception_tutorial.bag/perception_tutorial.db3 new file mode 100644 index 0000000000..c419b88115 Binary files /dev/null and b/doc/examples/perception_pipeline/bags/perception_tutorial.bag/perception_tutorial.db3 differ diff --git a/doc/examples/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch b/doc/examples/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch index f04fd647fc..272b93e5c8 100644 --- a/doc/examples/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch +++ b/doc/examples/perception_pipeline/launch/detect_and_add_cylinder_collision_object_demo.launch @@ -1,7 +1,7 @@ - + - + diff --git a/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch b/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch deleted file mode 100644 index 567038136c..0000000000 --- a/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch +++ /dev/null @@ -1,11 +0,0 @@ - - - - - - - - - - - diff --git a/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.xml b/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.xml new file mode 100644 index 0000000000..b30756bd12 --- /dev/null +++ b/doc/examples/perception_pipeline/launch/obstacle_avoidance_demo.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst b/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst index 6fa5d61ead..5e63f6a587 100644 --- a/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst +++ b/doc/examples/perception_pipeline/perception_pipeline_tutorial.rst @@ -1,13 +1,8 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - Perception Pipeline Tutorial ============================ MoveIt allows for seamless integration of 3D sensors using `Octomap `_. -Once properly configured, you should see something like this in rviz: +Once properly configured, you should see something like this in RViz: .. image:: perception_configuration_demo.png :width: 700px @@ -16,6 +11,15 @@ Getting Started --------------- If you haven't already done so, make sure you've completed the steps in :doc:`Getting Started `. +* 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 ------------- @@ -30,19 +34,36 @@ 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. - -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: /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 +We will have to generate a YAML configuration file for configuring the 3D sensors. Please see :moveit_resources_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_3d.yaml": :: + + 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 + 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:** @@ -57,30 +78,36 @@ Save this file in the config folder in the robot's moveit_config package with na * *point_subsample*: Choose one of every *point_subsample* points. -* *padding_offset*: The size of the padding (in cm). +* *padding_scale*: Should always be >= 1.0. Scale up collision shapes in the scene before excluding them from the octomap. -* *padding_scale*: The scale of the padding. +* *padding_offset*: Absolute padding (in m) around scaled collision shapes when excluding them from the octomap. * *filtered_cloud_topic*: The topic on which the filtered cloud will be published (mainly for debugging). The filtering cloud is the resultant cloud after self-filtering has been performed. +* *ns*: An optional namespace for the advertised topics. Required for multiple sensors of the same type. 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 +We will have to generate a YAML configuration file for configuring the 3D sensors. An :moveit_resources_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:** @@ -99,33 +126,55 @@ Save this file in the config folder in the robot's moveit_config package with na * *shadow_threshold*: The minimum brightness of the shadow map below an entity for its dynamic shadow to be visible -* *padding_offset*: The size of the padding (in cm). +* *padding_scale*: Should always be >= 1.0. Scale up collision shapes in the scene before excluding them from the octomap. -* *padding_scale*: The scale of the padding. +* *padding_offset*: Absolute padding (in m) around scaled collision shapes when excluding them from the octomap. * *filtered_cloud_topic*: The topic on which the filtered cloud will be published (mainly for debugging). The filtering cloud is the resultant cloud after self-filtering has been performed. +* *ns*: An optional namespace for the advertised topics. Required for multiple sensors of the same type. 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: :: - - +You will now need to create a *sensor_manager.launch* file in the "launch" directory of your ``panda_moveit_config`` directory with this sensor information, which is already done for you for convenience under :moveit_resources_codedir:`moveit_resources/panda_moveit_config `. + +You will need ``sensors_3d.yaml`` to be read by your application. :moveit_resources_codedir:`In panda_moveit_config/launch/demo.launch.py ` the file is already read like the following (It's convoluted, you should use ``demo.launch.py`` from ``moveit_resources`` repo unless you know what this piece of code does.): :: + + from moveit_configs_utils import MoveItConfigsBuilder + : + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description( + file_path="config/panda.urdf.xacro", + mappings={ + "ros2_control_hardware_type": LaunchConfiguration( + "ros2_control_hardware_type" + ) + }, + ) + .robot_description_semantic(file_path="config/panda.srdf") + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"]) + .sensors_3d("config/sensors_3d.yaml") + .to_moveit_configs() + ) -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 ^^^^^^^^^^^^^^^^^^^^^ -You will also need to configure the `Octomap `_ by adding the following lines into the *sensor_manager.launch*: :: - - - - - +You will also need to configure the `Octomap `_. :moveit_resources_codedir:`In the pre-made example (panda_moveit_config/launch/demo.launch.py) ` the following does the job: :: + + def _octomap_launch_params(params: ParameterBuilder): + params.parameter("octomap_frame", "camera_rgb_optical_frame") + params.parameter("octomap_resolution", 0.05) + params.parameter("max_range", 5.0) + return params.to_dict() + MoveIt uses an octree-based framework to represent the world around it. The *Octomap* parameters above are configuration parameters for this representation: * *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). @@ -139,18 +188,29 @@ If you set the initial and the final location of the robot in a way that there i .. image:: obstacle_avoidance.gif :width: 700px +Before running the software ++++++++++++++++++++++++++++ +This tutorial uses ``moveit2_tutorials`` that depends on ``moveit_task_constructor``, whose installer has not yet been available in ros2 yet (progress tracked in `moveit_task_constructor#400 `_) so you need to get it via source code. Move into your colcon workspace and pull the MoveIt Task Constructor source: :: + + cd ~/ws_moveit/src + git clone git@github.com:ros-planning/moveit_task_constructor.git -b ros2 + cd ~/ws_moveit + colcon build --mixin release + source ~/ws_moveit/install/setup.bash + Running the Interface +++++++++++++++++++++ -Roslaunch the launch file to run the code directly from moveit_tutorials: :: - roslaunch moveit_tutorials obstacle_avoidance_demo.launch +Launch the prepared launch file in moveit_tutorials to see the planning scene integrating sample point cloud data into an octomap: :: + + ros2 launch moveit_tutorials obstacle_avoidance_demo.launch You should see something like the image shown at the beginning of this tutorial. If not, you may have run into a `known OpenGL rendering issue `_. To work around the issue, you can force CPU-based rendering with this command: 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 :doc:`MoveIt Quickstart in RViz ` +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 `_ Detecting and Adding Object as Collision Object ----------------------------------------------- @@ -158,30 +218,23 @@ 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: +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. +Keep the launch file from above running and run the code directly from moveit_tutorials: :: -We are working on fixing it, it should not break the working of the demo. -You can follow its status in the `issue tracker `_ + ros2 run moveit2_tutorials detect_and_add_cylinder_collision_object_demo Relevant Code +++++++++++++ -The entire code can be seen :codedir:`here` in the moveit_tutorials GitHub project. +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 `_. +The details regarding the implementation of each of the perception pipeline function have been omitted in this tutorial as they are well documented on `ros1 wiki `_. .. |br| raw:: html @@ -195,4 +248,4 @@ The details regarding the implementation of each of the perception pipeline func -.. tutorial-formatter:: ./src/cylinder_segment.cpp +.. tutorial-formatter:: ./src/detect_and_add_cylinder_collision_object_demo.cpp diff --git a/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp b/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp deleted file mode 100644 index c813478dd3..0000000000 --- a/doc/examples/perception_pipeline/src/bag_publisher_maintain_time.cpp +++ /dev/null @@ -1,82 +0,0 @@ -/********************************************************************* - * 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 == nullptr) - { - 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/examples/perception_pipeline/src/cylinder_segment.cpp b/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp similarity index 66% rename from doc/examples/perception_pipeline/src/cylinder_segment.cpp rename to doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp index 99a9576d3a..26262a5ab7 100644 --- a/doc/examples/perception_pipeline/src/cylinder_segment.cpp +++ b/doc/examples/perception_pipeline/src/detect_and_add_cylinder_collision_object_demo.cpp @@ -34,57 +34,55 @@ /* Author: Ridhwan Luthra */ -#include -#include +#include +#include +#include +#include #include #include #include #include #include +#include +#include "sensor_msgs/point_cloud2_iterator.hpp" +#include "std_msgs/msg/string.hpp" -#include -#include - -class CylinderSegment +class CylinderSegment : public rclcpp::Node { public: - CylinderSegment() + CylinderSegment() : rclcpp::Node("detect_and_add_cylinder_collision_object_demo") { - 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(); + cloud_subscriber_ = this->create_subscription( + "/camera/depth_registered/points", 10, std::bind(&CylinderSegment::cloudCB, this, std::placeholders::_1)); } - /** \brief Given the parameters of the cylinder add the cylinder to the planning scene. - @param cylinder_params - Pointer to the struct AddCylinderParams. */ + /** \brief Given the parameters of the cylinder add the cylinder to the planning scene. */ 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; + moveit_msgs::msg::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; + shape_msgs::msg::SolidPrimitive primitive; primitive.type = primitive.CYLINDER; primitive.dimensions.resize(2); /* Setting height of cylinder. */ - primitive.dimensions[0] = cylinder_params->height; + primitive.dimensions[0] = cylinder_params_.height; /* Setting radius of cylinder. */ - primitive.dimensions[1] = cylinder_params->radius; + primitive.dimensions[1] = cylinder_params_.radius; // Define a pose for the cylinder (specified relative to frame_id). - geometry_msgs::Pose cylinder_pose; + geometry_msgs::msg::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 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); @@ -96,23 +94,22 @@ class CylinderSegment 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]; + 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; - planning_scene_interface.applyCollisionObject(collision_object); + planning_scene_interface_.applyCollisionObject(collision_object); // 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(const pcl::PointCloud::Ptr& cloud) + /** \brief Given the pointcloud containing just the cylinder, + compute its center point and its height and store in cylinder_params. + @param cloud - point cloud containing just the cylinder. */ + void extractLocationHeight(const pcl::PointCloud::Ptr& cloud) { double max_angle_y = -std::numeric_limits::infinity(); double min_angle_y = std::numeric_limits::infinity(); @@ -154,21 +151,24 @@ class CylinderSegment } } /* 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; + 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 = + 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(const pcl::PointCloud::Ptr& cloud) + * <- @TODO 20230607 @130s doesn't see user input anywhere in this .cpp file? + * + * @param cloud - Pointcloud whose ROI needs to be extracted. + **/ + void passThroughFilter(const pcl::PointCloud::Ptr& cloud) { - pcl::PassThrough pass; + pcl::PassThrough pass; pass.setInputCloud(cloud); pass.setFilterFieldName("z"); // min and max values in z axis to keep @@ -179,11 +179,11 @@ class CylinderSegment /** \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(const pcl::PointCloud::Ptr& cloud, + void computeNormals(const pcl::PointCloud::Ptr& cloud, const pcl::PointCloud::Ptr& cloud_normals) { - pcl::search::KdTree::Ptr tree(new pcl::search::KdTree()); - pcl::NormalEstimation ne; + 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. @@ -204,14 +204,13 @@ class CylinderSegment extract_normals.filter(*cloud_normals); } - /** \brief Given the pointcloud and indices of the plane, remove the plannar region from the pointcloud. + /** \brief Given the pointcloud and indices of the plane, remove the planar region from the pointcloud. @param cloud - Pointcloud. @param inliers_plane - Indices representing the plane. */ - void removePlaneSurface(const pcl::PointCloud::Ptr& cloud, - const pcl::PointIndices::Ptr& inliers_plane) + void removePlaneSurface(const pcl::PointCloud::Ptr& cloud, const pcl::PointIndices::Ptr& inliers_plane) { // create a SAC segmenter without using normals - pcl::SACSegmentation segmentor; + pcl::SACSegmentation segmentor; segmentor.setOptimizeCoefficients(true); segmentor.setModelType(pcl::SACMODEL_PLANE); segmentor.setMethodType(pcl::SAC_RANSAC); @@ -224,7 +223,7 @@ class CylinderSegment 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; + pcl::ExtractIndices extract_indices; extract_indices.setInputCloud(cloud); extract_indices.setIndices(inliers_plane); /* Remove the planar inliers, extract the rest */ @@ -237,12 +236,12 @@ class CylinderSegment @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(const pcl::PointCloud::Ptr& cloud, + void extractCylinder(const pcl::PointCloud::Ptr& cloud, const pcl::ModelCoefficients::Ptr& coefficients_cylinder, const pcl::PointCloud::Ptr& cloud_normals) { // Create the segmentation object for cylinder segmentation and set all the parameters - pcl::SACSegmentationFromNormals segmentor; + pcl::SACSegmentationFromNormals segmentor; pcl::PointIndices::Ptr inliers_cylinder(new pcl::PointIndices); segmentor.setOptimizeCoefficients(true); segmentor.setModelType(pcl::SACMODEL_CYLINDER); @@ -250,11 +249,11 @@ class CylinderSegment // Set the normal angular distance weight segmentor.setNormalDistanceWeight(0.1); // run at max 1000 iterations before giving up - segmentor.setMaxIterations(10000); + segmentor.setMaxIterations(1000); // tolerance for variation from model - segmentor.setDistanceThreshold(0.05); + segmentor.setDistanceThreshold(0.008); // min max values of radius in meters to consider - segmentor.setRadiusLimits(0, 1); + segmentor.setRadiusLimits(0.01, 0.1); segmentor.setInputCloud(cloud); segmentor.setInputNormals(cloud_normals); @@ -262,109 +261,113 @@ class CylinderSegment segmentor.segment(*inliers_cylinder, *coefficients_cylinder); // Extract the cylinder inliers from the input cloud - pcl::ExtractIndices extract; + pcl::ExtractIndices extract; extract.setInputCloud(cloud); extract.setIndices(inliers_cylinder); extract.setNegative(false); extract.filter(*cloud); } - void cloudCB(const sensor_msgs::PointCloud2ConstPtr& input) +private: + rclcpp::Subscription::SharedPtr cloud_subscriber_; + moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; + + // 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 along 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 + + void cloudCB(const sensor_msgs::msg::PointCloud2::SharedPtr 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); + // + // This section uses a standard PCL-based processing pipeline to estimate a cylinder's pose in the point cloud. + // + // First, we convert from sensor_msgs to pcl::PointXYZ 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. + // 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); - // Declare normals and call function to compute point normals. + // 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 eliminate the plane on which the cylinder is kept to ease the process of finding the cylinder. + // 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); - // 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. + // 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|\ . // |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. + // |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()) + if (cloud->points.empty() || coefficients_cylinder->values.size() != 7) { - ROS_ERROR_STREAM_NAMED("cylinder_segment", "Can't find the cylindrical component."); + RCLCPP_ERROR(this->get_logger(), "Can't find the cylindrical component. Returning the callback."); 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 + RCLCPP_INFO(this->get_logger(), "Detected a cylinder - Adding CollisionObject to PlanningScene"); - bool points_not_found = true; + // 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 + } }; int main(int argc, char** argv) { // Initialize ROS - ros::init(argc, argv, "cylinder_segment"); - // Start the segmentor - CylinderSegment(); + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); } diff --git a/package.xml b/package.xml index 9f4fca44ab..437b8e2fc5 100644 --- a/package.xml +++ b/package.xml @@ -19,6 +19,7 @@ ament_cmake control_msgs + geometry_msgs interactive_markers moveit_core moveit_ros_perception @@ -27,11 +28,12 @@ moveit_hybrid_planning moveit_visual_tools moveit_msgs - - + pcl_conversions + pcl_ros pluginlib rviz_visual_tools + sensor_msgs tf2_eigen tf2_geometry_msgs tf2_ros @@ -43,7 +45,6 @@ moveit_common moveit_ros_planning - controller_manager gripper_controllers