From b861903fa6c15abc7a9a9b7a58b4f1a8499eefa4 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Wed, 14 Jun 2023 12:56:36 +0900 Subject: [PATCH] =?UTF-8?q?ros2=E3=83=96=E3=83=A9=E3=83=B3=E3=83=81?= =?UTF-8?q?=E3=81=AE=E6=9B=B4=E6=96=B0=E5=86=85=E5=AE=B9=E3=82=92=E5=8F=8D?= =?UTF-8?q?=E6=98=A0=20(#188)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 色認識サンプルの追加 (#183) * 色認識追加 * 二値化画像を配信 * 不要な処理を削除 * RVizに二値化画像を表示 * 距離のオフセット追加 * 変数名変更 * カーネルを変更 * 不要な空行を削除 * 画像取得時のheaderをtfに使用 * コメント、変数名修正 * Update README * コメント修正 * 変数名変更 * 動画URL追加 * バージョンタグを4.3.0に更新 (#184) --- crane_x7/package.xml | 2 +- crane_x7_control/package.xml | 2 +- crane_x7_examples/CMakeLists.txt | 3 + crane_x7_examples/README.md | 40 +++- .../launch/camera_example.launch.py | 4 +- crane_x7_examples/launch/camera_example.rviz | 30 ++- crane_x7_examples/launch/demo.launch.py | 1 + crane_x7_examples/package.xml | 3 +- crane_x7_examples/src/color_detection.cpp | 189 ++++++++++++++++++ .../src/point_cloud_detection.cpp | 6 +- crane_x7_gazebo/package.xml | 2 +- crane_x7_moveit_config/package.xml | 2 +- 12 files changed, 257 insertions(+), 27 deletions(-) create mode 100644 crane_x7_examples/src/color_detection.cpp diff --git a/crane_x7/package.xml b/crane_x7/package.xml index f8986eb9..90783e93 100644 --- a/crane_x7/package.xml +++ b/crane_x7/package.xml @@ -2,7 +2,7 @@ crane_x7 - 4.2.0 + 4.3.0 ROS 2 package suite of CRANE-X7 RT Corporation Apache License 2.0 diff --git a/crane_x7_control/package.xml b/crane_x7_control/package.xml index 35c98ac5..43578cd8 100644 --- a/crane_x7_control/package.xml +++ b/crane_x7_control/package.xml @@ -2,7 +2,7 @@ crane_x7_control - 4.2.0 + 4.3.0 The CRANE-X7 control package RT Corporation Apache License 2.0 diff --git a/crane_x7_examples/CMakeLists.txt b/crane_x7_examples/CMakeLists.txt index 1d93442a..77443be9 100644 --- a/crane_x7_examples/CMakeLists.txt +++ b/crane_x7_examples/CMakeLists.txt @@ -19,6 +19,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) +find_package(image_geometry REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) find_package(pcl_conversions REQUIRED) @@ -35,6 +36,7 @@ set(executable_list pick_and_place pick_and_place_tf aruco_detection + color_detection point_cloud_detection ) foreach(loop_var IN LISTS executable_list) @@ -42,6 +44,7 @@ foreach(loop_var IN LISTS executable_list) ament_target_dependencies(${loop_var} cv_bridge geometry_msgs + image_geometry moveit_ros_planning_interface OpenCV pcl_ros diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md index 302f5115..5355116b 100644 --- a/crane_x7_examples/README.md +++ b/crane_x7_examples/README.md @@ -27,8 +27,10 @@ - [Camera Examples](#camera-examples) - [aruco\_detection](#aruco_detection) - [Videos](#videos-5) - - [point\_cloud\_detection](#point_cloud_detection) + - [color\_detection](#color_detection) - [Videos](#videos-6) + - [point\_cloud\_detection](#point_cloud_detection) + - [Videos](#videos-7) ## 準備（実機を使う場合） @@ -229,6 +231,7 @@ CRANE-X7から20cm離れた位置にピッキング対象を設置します。 [「RealSense D435マウンタ搭載モデルを使用する場合」](#realsense-d435マウンタ搭載モデルを使用する場合)の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。 - [aruco\_detection](#aruco_detection) +- [color\_detection](#color_detection) - [point\_cloud\_detection](#point_cloud_detection) 実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -244,11 +247,11 @@ Arguments (pass arguments as ':='): ### aruco_detection -モノに取り付けたArUcoマーカをカメラで認識し、マーカ位置に合わせて掴むコード例です。 +モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。 マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けて使用します。 -認識されたマーカの位置姿勢はtfのフレームとして配信されます。 -tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。掴む対象は`target_0`に設定されています。マーカ認識には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 +検出されたマーカの位置姿勢はtfのフレームとして配信されます。 +tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。掴む対象は`target_0`に設定されています。マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 次のコマンドを実行します ```sh @@ -262,13 +265,34 @@ ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection --- +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +デフォルトでは青い物体の位置をtfのフレームとして配信します。 +tfの`frame_id`は`target_0`です。 +色の検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。 +検出した物体の距離は深度画像から取得します。 + +次のコマンドを実行します +```sh +ros2 launch crane_x7_examples camera_example.launch.py example:='color_detection' +``` + +#### Videos +[![crane_x7_color_detection_demo](http://img.youtube.com/vi/O8lqw7yemAI/hqdefault.jpg)](https://youtu.be/O8lqw7yemAI) + +[back to camera example list](#camera-examples) + +--- + ### point_cloud_detection -点群から物体位置を認識して掴むコード例です。 +点群から物体を検出して掴むコード例です。 -認識された物体位置はtfのフレームとして配信されます。 -tfの`frame_id`は認識された順に`target_0`、`target_1`、`target_2`…に設定されます。掴む対象は`target_0`に設定されています。 -物体認識には[Point Cloud Library](https://pointclouds.org/)を使用しています。 +検出された物体位置はtfのフレームとして配信されます。 +tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。掴む対象は`target_0`に設定されています。 +物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 次のコマンドを実行します ```sh diff --git a/crane_x7_examples/launch/camera_example.launch.py b/crane_x7_examples/launch/camera_example.launch.py index df43ad26..03a25fe1 100644 --- a/crane_x7_examples/launch/camera_example.launch.py +++ b/crane_x7_examples/launch/camera_example.launch.py @@ -57,9 +57,9 @@ def generate_launch_description(): kinematics_yaml = load_yaml('crane_x7_moveit_config', 'config/kinematics.yaml') declare_example_name = DeclareLaunchArgument( - 'example', default_value='aruco_detection', + 'example', default_value='color_detection', description=('Set an example executable name: ' - '[aruco_detection, point_cloud_detection]') + '[color_detection, aruco_detection, point_cloud_detection]') ) declare_use_sim_time = DeclareLaunchArgument( diff --git a/crane_x7_examples/launch/camera_example.rviz b/crane_x7_examples/launch/camera_example.rviz index 6af29af6..939c0d1d 100644 --- a/crane_x7_examples/launch/camera_example.rviz +++ b/crane_x7_examples/launch/camera_example.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 154 + Tree Height: 257 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -405,8 +405,6 @@ Visualization Manager: Value: false target_0: Value: true - target_1: - Value: true world: Value: false Marker Scale: 0.5 @@ -446,7 +444,8 @@ Visualization Manager: camera_link: camera_color_frame: camera_color_optical_frame: - {} + target_0: + {} camera_depth_frame: camera_depth_optical_frame: {} @@ -454,10 +453,6 @@ Visualization Manager: {} crane_x7_gripper_finger_b_link: {} - target_0: - {} - target_1: - {} Update Interval: 0 Value: true - Alpha: 1 @@ -509,6 +504,7 @@ Visualization Manager: Value: true Visibility: Grid: false + Image: true MotionPlanning: false PointCloud2: true TF: true @@ -548,6 +544,20 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: true + - Class: rviz_default_plugins/Image + Enabled: true + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /image_thresholded + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -622,11 +632,13 @@ Window Geometry: Height: 1016 Hide Left Dock: false Hide Right Dock: false + Image: + collapsed: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd000000040000000000000266000003a2fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000011b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015c0000018a0000016900fffffffb0000000c00430061006d00650072006101000002ec000000f10000002800ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003b7000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000266000003a2fc020000000bfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000182000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001c30000021a0000016900ffffff00000001000001b2000003a2fc0200000005fb0000000c00430061006d006500720061010000003b000001c60000002800fffffffb0000000a0049006d0061006700650100000207000001d60000002800fffffffb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d0065010000000000000450000000000000000000000314000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/crane_x7_examples/launch/demo.launch.py b/crane_x7_examples/launch/demo.launch.py index dbab28a3..dc5a7e63 100644 --- a/crane_x7_examples/launch/demo.launch.py +++ b/crane_x7_examples/launch/demo.launch.py @@ -104,6 +104,7 @@ def generate_launch_description(): launch_arguments={ 'device_type': 'd435', 'pointcloud.enable': 'true', + 'align_depth.enable': 'true', }.items() ) diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml index d2a85412..1b8eea2a 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -2,7 +2,7 @@ crane_x7_examples - 4.2.0 + 4.3.0 CRANE-X7 examples package RT Corporation Apache License 2.0 @@ -18,6 +18,7 @@ crane_x7_moveit_config cv_bridge geometry_msgs + image_geometry libopencv-dev moveit_ros_planning_interface pcl_ros diff --git a/crane_x7_examples/src/color_detection.cpp b/crane_x7_examples/src/color_detection.cpp new file mode 100644 index 00000000..72bc9e17 --- /dev/null +++ b/crane_x7_examples/src/color_detection.cpp @@ -0,0 +1,189 @@ +// Copyright 2023 RT Corporation +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Reference: +// https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/camera_info.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_ros/transform_broadcaster.h" +#include "opencv2/opencv.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include "cv_bridge/cv_bridge.h" +#include "image_geometry/pinhole_camera_model.h" +using std::placeholders::_1; + +class ImageSubscriber : public rclcpp::Node +{ +public: + ImageSubscriber() + : Node("color_detection") + { + image_subscription_ = this->create_subscription( + "/camera/color/image_raw", 10, std::bind(&ImageSubscriber::image_callback, this, _1)); + + depth_subscription_ = this->create_subscription( + "/camera/aligned_depth_to_color/image_raw", 10, + std::bind(&ImageSubscriber::depth_callback, this, _1)); + + camera_info_subscription_ = this->create_subscription( + "/camera/color/camera_info", 10, std::bind(&ImageSubscriber::camera_info_callback, this, _1)); + + image_thresholded_publisher_ = + this->create_publisher("image_thresholded", 10); + + tf_broadcaster_ = + std::make_unique(*this); + } + +private: + rclcpp::Subscription::SharedPtr image_subscription_; + rclcpp::Subscription::SharedPtr depth_subscription_; + rclcpp::Subscription::SharedPtr camera_info_subscription_; + rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + sensor_msgs::msg::CameraInfo::SharedPtr camera_info_; + sensor_msgs::msg::Image::SharedPtr depth_image_; + std::unique_ptr tf_broadcaster_; + + void image_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + // カメラのパラメータを取得してから処理を行う + if (camera_info_ && depth_image_) { + // 青い物体を検出するようにHSVの範囲を設定 + // 周囲の明るさ等の動作環境に合わせて調整 + const int LOW_H = 100, HIGH_H = 125; + const int LOW_S = 100, HIGH_S = 255; + const int LOW_V = 30, HIGH_V = 255; + + // ウェブカメラの画像を受け取る + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::cvtColor(cv_img->image, cv_img->image, cv::COLOR_RGB2HSV); + + // 画像処理用の変数を用意 + cv::Mat img_thresholded; + + // 画像の二値化 + cv::inRange( + cv_img->image, + cv::Scalar(LOW_H, LOW_S, LOW_V), + cv::Scalar(HIGH_H, HIGH_S, HIGH_V), + img_thresholded); + + // ノイズ除去の処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_OPEN, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 穴埋めの処理 + cv::morphologyEx( + img_thresholded, + img_thresholded, + cv::MORPH_CLOSE, + cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); + + // 画像の検出領域におけるモーメントを計算 + cv::Moments moment = moments(img_thresholded); + double d_m01 = moment.m01; + double d_m10 = moment.m10; + double d_area = moment.m00; + + // 検出した領域のピクセル数が10000より大きい場合 + if (d_area > 10000) { + // カメラモデル作成 + image_geometry::PinholeCameraModel camera_model; + + // カメラのパラメータを設定 + camera_model.fromCameraInfo(camera_info_); + + // 画像座標系における把持対象物の位置（2D） + const double pixel_x = d_m10 / d_area; + const double pixel_y = d_m01 / d_area; + const cv::Point2d point(pixel_x, pixel_y); + + // 補正後の画像座標系における把持対象物の位置を取得（2D） + const cv::Point2d rect_point = camera_model.rectifyPoint(point); + + // カメラ座標系から見た把持対象物の方向（Ray）を取得する + const cv::Point3d ray = camera_model.projectPixelTo3dRay(rect_point); + + // 把持対象物までの距離を取得 + // 把持対象物の表面より少し奥を掴むように設定 + const double DEPTH_OFFSET = 0.015; + const auto cv_depth = cv_bridge::toCvShare(depth_image_, depth_image_->encoding); + // カメラから把持対象物の表面までの距離 + const auto front_distance = cv_depth->image.at(point) / 1000.0; + const auto center_distance = front_distance + DEPTH_OFFSET; + + // 距離を取得できないか遠すぎる場合は把持しない + const double DEPTH_MAX = 0.5; + const double DEPTH_MIN = 0.2; + if (center_distance < DEPTH_MIN || center_distance > DEPTH_MAX) { + RCLCPP_INFO_STREAM(this->get_logger(), "Failed to get depth at" << point << "."); + return; + } + + // 把持対象物の位置を計算 + cv::Point3d object_position( + ray.x * center_distance, + ray.y * center_distance, + ray.z * center_distance); + + // 把持対象物の位置をTFに配信 + geometry_msgs::msg::TransformStamped t; + t.header = msg->header; + t.child_frame_id = "target_0"; + t.transform.translation.x = object_position.x; + t.transform.translation.y = object_position.y; + t.transform.translation.z = object_position.z; + tf_broadcaster_->sendTransform(t); + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); + } + } + } + + void camera_info_callback(const sensor_msgs::msg::CameraInfo::SharedPtr msg) + { + camera_info_ = msg; + } + + void depth_callback(const sensor_msgs::msg::Image::SharedPtr msg) + { + depth_image_ = msg; + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/crane_x7_examples/src/point_cloud_detection.cpp b/crane_x7_examples/src/point_cloud_detection.cpp index ac25be9d..8bb36f76 100644 --- a/crane_x7_examples/src/point_cloud_detection.cpp +++ b/crane_x7_examples/src/point_cloud_detection.cpp @@ -100,14 +100,14 @@ class PointCloudSubscriber : public rclcpp::Node auto cloud = std::make_shared>(); pcl::fromROSMsg(cloud_transformed, *cloud); - // 物体認識の前処理として点群の取得範囲の制限と間引きを行う - // フィルタリング後に点群がない場合は認識処理をスキップする + // 物体検出の前処理として点群の取得範囲の制限と間引きを行う + // フィルタリング後に点群がない場合は検出処理をスキップする if (preprocessing(cloud) == false) { return; } // 平面除去 - // 平面検知ができない場合は認識処理をスキップする + // 平面検知ができない場合は検出処理をスキップする // 物体がアームと別の高さの平面に置かれている場合など、 // Z軸方向のフィルタリングで不要な点群が除去できない場合に使用してみてください /* diff --git a/crane_x7_gazebo/package.xml b/crane_x7_gazebo/package.xml index a4e9133f..de03b7ef 100644 --- a/crane_x7_gazebo/package.xml +++ b/crane_x7_gazebo/package.xml @@ -2,7 +2,7 @@ crane_x7_gazebo - 4.2.0 + 4.3.0 The crane_x7_gazebo package RT Corporation Apache License 2.0 diff --git a/crane_x7_moveit_config/package.xml b/crane_x7_moveit_config/package.xml index 753c4e95..50edc60f 100644 --- a/crane_x7_moveit_config/package.xml +++ b/crane_x7_moveit_config/package.xml @@ -2,7 +2,7 @@ crane_x7_moveit_config - 4.2.0 + 4.3.0 CRANE-X7 move_group config package RT Corporation Apache License 2.0