From b07111d72b97da81f9a067c76def62c13ee1e0e3 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Mon, 20 Feb 2023 09:54:32 +0900 Subject: [PATCH 1/9] =?UTF-8?q?=E7=82=B9=E7=BE=A4=E8=AA=8D=E8=AD=98?= =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E3=81=AE=E8=BF=BD=E5=8A=A0?= =?UTF-8?q?=20(#176)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 点群を0.5mの距離でフィルタリング * Voxel gridでダウンサンプリング * 点群の座標変換 * 点群の座標変換修正 * 点群の取得範囲変更 * KdTreeによるクラスタリング * 認識した物体位置をtfで配信 * スタイル修正 * ファイル名変更 * point_cloud_detectionをサンプル実行用launchに追加 * 不要な変数を削除 * Update README * プレース後の物体を再度把持してしまうためプレース位置修正 * 認識後の点群をRvizに表示 * 平面検出処理追加 * コメント修正 * READMEに動画URL追加 * tfの表示時間短縮 * タイムスタンプを一致させるためheaderをコピー * 点群の重心位置を物体位置として配信 * tfの取得時間の範囲を変更 * 物体位置の高さ制限を追加 * 平面検出をコメントアウト * newをmake_sharedに置き換え * 点群処理を複数の関数に分割 * Copyright表記修正 Co-authored-by: Shota Aoki * Update README * 変数の型修正 Co-authored-by: Shota Aoki * 変数の型修正 Co-authored-by: Shota Aoki * 変数名修正 Co-authored-by: Shota Aoki * 点群取得範囲修正 Co-authored-by: Shota Aoki * 平面検出の判定条件修正 Co-authored-by: Shota Aoki * for文を範囲for文に変更 Co-authored-by: Shota Aoki * for文を範囲for文に変更 Co-authored-by: Shota Aoki * 認識範囲に点群がない場合INFOを出力 --------- Co-authored-by: Shota Aoki --- crane_x7_examples/CMakeLists.txt | 5 + crane_x7_examples/README.md | 63 +++- .../launch/camera_example.launch.py | 2 +- crane_x7_examples/launch/camera_example.rviz | 68 ++++- crane_x7_examples/package.xml | 1 + crane_x7_examples/src/pick_and_place_tf.cpp | 25 +- .../src/point_cloud_detection.cpp | 278 ++++++++++++++++++ 7 files changed, 404 insertions(+), 38 deletions(-) create mode 100644 crane_x7_examples/src/point_cloud_detection.cpp diff --git a/crane_x7_examples/CMakeLists.txt b/crane_x7_examples/CMakeLists.txt index c26f01c9..1d93442a 100644 --- a/crane_x7_examples/CMakeLists.txt +++ b/crane_x7_examples/CMakeLists.txt @@ -21,6 +21,8 @@ find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(OpenCV REQUIRED COMPONENTS core) +find_package(pcl_conversions REQUIRED) +find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) @@ -33,6 +35,7 @@ set(executable_list pick_and_place pick_and_place_tf aruco_detection + point_cloud_detection ) foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) @@ -41,6 +44,8 @@ foreach(loop_var IN LISTS executable_list) geometry_msgs moveit_ros_planning_interface OpenCV + pcl_ros + pcl_conversions rclcpp tf2_geometry_msgs ) diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md index e00e7fdd..302f5115 100644 --- a/crane_x7_examples/README.md +++ b/crane_x7_examples/README.md @@ -24,9 +24,11 @@ - [Videos](#videos-3) - [pick\_and\_place](#pick_and_place) - [Videos](#videos-4) + - [Camera Examples](#camera-examples) - [aruco\_detection](#aruco_detection) - - [実行手順](#実行手順) - [Videos](#videos-5) + - [point\_cloud\_detection](#point_cloud_detection) + - [Videos](#videos-6) ## 準備（実機を使う場合） @@ -105,7 +107,7 @@ ros2 launch crane_x7_examples example.launch.py example:='gripper_control' use_s - [cartesian_path](#cartesian_path) - [pick_and_place](#pick_and_place) -実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 +実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 ```sh $ ros2 launch crane_x7_examples example.launch.py -s @@ -220,25 +222,60 @@ CRANE-X7から20cm離れた位置にピッキング対象を設置します。 [back to example list](#examples) ---- +## Camera Examples + +[RealSense D435マウンタ](https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)搭載モデルのカメラを使用したサンプルコードです。 + +[「RealSense D435マウンタ搭載モデルを使用する場合」](#realsense-d435マウンタ搭載モデルを使用する場合)の手順に従って`demo.launch`を実行している状態で各サンプルを実行できます。 + +- [aruco\_detection](#aruco_detection) +- [point\_cloud\_detection](#point_cloud_detection) + +実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで表示できます。 + +```sh +$ ros2 launch crane_x7_examples camera_example.launch.py -s +Arguments (pass arguments as ':='): + + 'example': + Set an example executable name: [aruco_detection, point_cloud_detection] + (default: 'aruco_detection') +``` ### aruco_detection -モノに取り付けたArUcoマーカをカメラで認識し、マーカ位置に合わせて掴むコード例です。[RealSense D435マウンタ](https://github.com/rt-net/crane_x7_Hardware/blob/master/3d_print_parts/v1.0/CRANE-X7_HandA_RealSenseD435マウンタ.stl)搭載モデルで実行することを想定しています。 +モノに取り付けた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)を使用しています。 -#### 実行手順 -1. [aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷し、一辺50mmの立方体に取り付けます -1. [「RealSense D435マウンタ搭載モデルを使用する場合」](#realsense-d435マウンタ搭載モデルを使用する場合)の手順でCRANE-X7を起動します -1. 次のコマンドを実行します - ```sh - ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection' - ``` -1. マーカのついた立方体をカメラに写る位置に置きます +次のコマンドを実行します +```sh +ros2 launch crane_x7_examples camera_example.launch.py example:='aruco_detection' +``` #### Videos [![crane_x7_aruco_detection_demo](http://img.youtube.com/vi/eWzmG_jbTmM/hqdefault.jpg)](https://youtu.be/eWzmG_jbTmM) -[back to example list](#examples) +[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/)を使用しています。 + +次のコマンドを実行します +```sh +ros2 launch crane_x7_examples camera_example.launch.py example:='point_cloud_detection' +``` + +#### Videos +[![crane_x7_point_cloud_detection_demo](http://img.youtube.com/vi/RgAjxH0CAuk/hqdefault.jpg)](https://youtu.be/RgAjxH0CAuk) + +[back to camera example list](#camera-examples) diff --git a/crane_x7_examples/launch/camera_example.launch.py b/crane_x7_examples/launch/camera_example.launch.py index d555d33b..df43ad26 100644 --- a/crane_x7_examples/launch/camera_example.launch.py +++ b/crane_x7_examples/launch/camera_example.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='aruco_detection', description=('Set an example executable name: ' - '[aruco_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 c94d6af8..6af29af6 100644 --- a/crane_x7_examples/launch/camera_example.rviz +++ b/crane_x7_examples/launch/camera_example.rviz @@ -1,14 +1,11 @@ Panels: - Class: rviz_common/Displays - Help Height: 78 + Help Height: 70 Name: Displays Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /PointCloud21 + Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 143 + Tree Height: 154 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -55,8 +52,6 @@ Visualization Manager: MoveIt_Planning_Time: 5 MoveIt_Use_Cartesian_Path: false MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Warehouse_Host: 127.0.0.1 - MoveIt_Warehouse_Port: 33829 MoveIt_Workspace: Center: X: 0 @@ -197,6 +192,7 @@ Visualization Manager: State Display Time: 0.05 s Trail Step Size: 1 Trajectory Topic: /display_planned_path + Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false @@ -352,7 +348,7 @@ Visualization Manager: Velocity_Scaling_Factor: 0.1 - Class: rviz_default_plugins/TF Enabled: true - Frame Timeout: 15 + Frame Timeout: 1 Frames: All Enabled: false base_link: @@ -407,7 +403,9 @@ Visualization Manager: Value: false crane_x7_wrist_link: Value: false - target: + target_0: + Value: true + target_1: Value: true world: Value: false @@ -448,8 +446,7 @@ Visualization Manager: camera_link: camera_color_frame: camera_color_optical_frame: - target: - {} + {} camera_depth_frame: camera_depth_optical_frame: {} @@ -457,6 +454,10 @@ Visualization Manager: {} crane_x7_gripper_finger_b_link: {} + target_0: + {} + target_1: + {} Update Interval: 0 Value: true - Alpha: 1 @@ -486,6 +487,7 @@ Visualization Manager: Topic: Depth: 5 Durability Policy: Volatile + Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable Value: /camera/depth/color/points @@ -494,6 +496,7 @@ Visualization Manager: Value: true - Class: rviz_default_plugins/Camera Enabled: true + Far Plane Distance: 100 Image Rendering: background and overlay Name: Camera Overlay Alpha: 0.5 @@ -507,10 +510,44 @@ Visualization Manager: Visibility: Grid: false MotionPlanning: false - PointCloud2: false + PointCloud2: true TF: true Value: true Zoom Factor: 1 + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: RGB8 + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /classified_points + Use Fixed Frame: true + Use rainbow: true + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -526,6 +563,9 @@ Visualization Manager: - Class: rviz_default_plugins/Measure Line color: 128; 128; 0 - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 Topic: Depth: 5 Durability Policy: Volatile @@ -586,7 +626,7 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000400000000000002660000039efc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000011a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015d000001880000017d00fffffffb0000000c00430061006d00650072006101000002eb000000f00000002800ffffff000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d0000039e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003b70000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000266000003a2fc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b0000011b000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000003f00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000015c0000018a0000016900fffffffb0000000c00430061006d00650072006101000002ec000000f10000002800ffffff000000010000010f000003a2fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000003a2000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000003b7000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml index 9c62083b..4b536129 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -20,6 +20,7 @@ geometry_msgs libopencv-dev moveit_ros_planning_interface + pcl_ros rclcpp realsense2_camera tf2_geometry_msgs diff --git a/crane_x7_examples/src/pick_and_place_tf.cpp b/crane_x7_examples/src/pick_and_place_tf.cpp index cf3d5f06..a875c23f 100644 --- a/crane_x7_examples/src/pick_and_place_tf.cpp +++ b/crane_x7_examples/src/pick_and_place_tf.cpp @@ -102,7 +102,7 @@ class PickAndPlaceTf : public rclcpp::Node private: void on_timer() { - // ID 0のマーカ位置姿勢を取得 + // target_0のtf位置姿勢を取得 geometry_msgs::msg::TransformStamped tf_msg; try { @@ -117,21 +117,26 @@ class PickAndPlaceTf : public rclcpp::Node } rclcpp::Time now = this->get_clock()->now(); - const std::chrono::nanoseconds FILTERING_TIME = 1s; + const std::chrono::nanoseconds FILTERING_TIME = 2s; const std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s; - const float DISTANCE_THRESHOLD = 0.01; + const double DISTANCE_THRESHOLD = 0.01; tf2::Stamped tf; tf2::convert(tf_msg, tf); const auto TF_ELAPSED_TIME = now.nanoseconds() - tf.stamp_.time_since_epoch().count(); const auto TF_STOP_TIME = now.nanoseconds() - tf_past_.stamp_.time_since_epoch().count(); + const double TARGET_Z_MIN_LIMIT = 0.04; - // 現在時刻から1秒以内に受け取ったtfを使用 + // 現在時刻から2秒以内に受け取ったtfを使用 if (TF_ELAPSED_TIME < FILTERING_TIME.count()) { double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); // 把持対象の位置が停止していることを判定 if (tf_diff < DISTANCE_THRESHOLD) { // 把持対象が3秒以上停止している場合ピッキング動作開始 if (TF_STOP_TIME > STOP_TIME_THRESHOLD.count()) { + // 把持対象が低すぎる場合は把持位置を調整 + if (tf.getOrigin().z() < TARGET_Z_MIN_LIMIT) { + tf.getOrigin().setZ(TARGET_Z_MIN_LIMIT); + } picking(tf.getOrigin()); } } else { @@ -165,31 +170,31 @@ class PickAndPlaceTf : public rclcpp::Node control_gripper(GRIPPER_DEFAULT); // 掴む準備をする - control_arm(target_position.x(), target_position.y(), 0.2, -180, 0, 90); + control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90); // ハンドを開く control_gripper(GRIPPER_OPEN); // 掴みに行く - control_arm(target_position.x(), target_position.y(), 0.13, -180, 0, 90); + control_arm(target_position.x(), target_position.y(), target_position.z() + 0.07, -180, 0, 90); // ハンドを閉じる control_gripper(GRIPPER_CLOSE); // 持ち上げる - control_arm(target_position.x(), target_position.y(), 0.2, -180, 0, 90); + control_arm(target_position.x(), target_position.y(), target_position.z() + 0.12, -180, 0, 90); // 移動する - control_arm(0.2, 0.2, 0.2, -180, 0, 90); + control_arm(0.1, 0.2, 0.2, -180, 0, 90); // 下ろす - control_arm(0.2, 0.2, 0.13, -180, 0, 90); + control_arm(0.1, 0.2, 0.13, -180, 0, 90); // ハンドを開く control_gripper(GRIPPER_OPEN); // 少しだけハンドを持ち上げる - control_arm(0.2, 0.2, 0.2, -180, 0, 90); + control_arm(0.1, 0.2, 0.2, -180, 0, 90); // 初期姿勢に戻る // control_arm(0.15, 0.0, 0.3, -180, 0, 90); diff --git a/crane_x7_examples/src/point_cloud_detection.cpp b/crane_x7_examples/src/point_cloud_detection.cpp new file mode 100644 index 00000000..ac25be9d --- /dev/null +++ b/crane_x7_examples/src/point_cloud_detection.cpp @@ -0,0 +1,278 @@ +// 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://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Broadcaster-Cpp.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/passthrough.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/voxel_grid.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/planar_segmentation.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/extract_indices.html +// https://pcl.readthedocs.io/projects/tutorials/en/master/cluster_extraction.html +// + +#include +#include +#include +#include + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "pcl/common/centroid.h" +#include "pcl/common/common.h" +#include "pcl/filters/extract_indices.h" +#include "pcl/filters/passthrough.h" +#include "pcl/filters/voxel_grid.h" +#include "pcl/io/pcd_io.h" +#include "pcl/kdtree/kdtree.h" +#include "pcl/point_cloud.h" +#include "pcl/point_types.h" +#include "pcl/segmentation/extract_clusters.h" +#include "pcl/segmentation/sac_segmentation.h" +#include "pcl_conversions/pcl_conversions.h" +#include "pcl_ros/transforms.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/LinearMath/Matrix3x3.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" + +class PointCloudSubscriber : public rclcpp::Node +{ +public: + PointCloudSubscriber() + : Node("point_cloud_detection") + { + point_cloud_subscription_ = this->create_subscription( + "/camera/depth/color/points", + 10, + std::bind(&PointCloudSubscriber::point_cloud_callback, this, std::placeholders::_1)); + + publisher_ = this->create_publisher("/classified_points", 10); + + tf_broadcaster_ = + std::make_unique(*this); + + tf_buffer_ = + std::make_unique(this->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + } + +private: + rclcpp::Subscription::SharedPtr point_cloud_subscription_; + rclcpp::Publisher::SharedPtr publisher_; + std::unique_ptr tf_broadcaster_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + + void point_cloud_callback(const sensor_msgs::msg::PointCloud2::SharedPtr msg) + { + // カメラ座標系における点群をロボット座標系に変換 + geometry_msgs::msg::TransformStamped tf_msg; + + try { + tf_msg = tf_buffer_->lookupTransform( + "base_link", msg->header.frame_id, + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform base_link to camera_depth_optical_frame: %s", + ex.what()); + return; + } + + sensor_msgs::msg::PointCloud2 cloud_transformed; + pcl_ros::transformPointCloud("base_link", tf_msg, *msg, cloud_transformed); + + // ROSメッセージの点群フォーマットからPCLのフォーマットに変換 + auto cloud = std::make_shared>(); + pcl::fromROSMsg(cloud_transformed, *cloud); + + // 物体認識の前処理として点群の取得範囲の制限と間引きを行う + // フィルタリング後に点群がない場合は認識処理をスキップする + if (preprocessing(cloud) == false) { + return; + } + + // 平面除去 + // 平面検知ができない場合は認識処理をスキップする + // 物体がアームと別の高さの平面に置かれている場合など、 + // Z軸方向のフィルタリングで不要な点群が除去できない場合に使用してみてください + /* + if (plane_extraction(cloud) == false) { + return; + } + */ + + // KdTreeを用いて点群を物体ごとに分類(クラスタリング)する + auto cluster_indices = clustering(cloud); + + // クラスタごとに色分けし、クラスタ位置をtfで配信する + auto cloud_output = std::make_shared>(); + broadcast_cluster_position(cloud, cloud_output, cluster_indices, cloud_transformed.header); + + // クラスタリングした点群を配信する + sensor_msgs::msg::PointCloud2 sensor_msg; + cloud_output->header = cloud->header; + pcl::toROSMsg(*cloud_output, sensor_msg); + publisher_->publish(sensor_msg); + } + + bool preprocessing(std::shared_ptr> & cloud) + { + // X軸方向0.05~0.5m以外の点群を削除 + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName("x"); + pass.setFilterLimits(0.05, 0.5); + pass.filter(*cloud); + + // Z軸方向0.03~0.5m以外の点群を削除 + // 物体が乗っている平面の点群を削除します + pass.setInputCloud(cloud); + pass.setFilterFieldName("z"); + pass.setFilterLimits(0.03, 0.5); + pass.filter(*cloud); + + // Voxel gridで点群を間引く(ダウンサンプリング) + pcl::VoxelGrid sor; + sor.setInputCloud(cloud); + sor.setLeafSize(0.01f, 0.01f, 0.01f); + sor.filter(*cloud); + + // フィルタリング後に点群がない場合はfalseを返す + if (cloud->size() <= 0) { + RCLCPP_INFO(this->get_logger(), "No point cloud in the detection area."); + return false; + } else { + return true; + } + } + + bool plane_extraction(std::shared_ptr> & cloud) + { + auto coefficients = std::make_shared(); + auto inliers = std::make_shared(); + pcl::SACSegmentation seg; + seg.setOptimizeCoefficients(true); + seg.setModelType(pcl::SACMODEL_PLANE); + seg.setMethodType(pcl::SAC_RANSAC); + seg.setDistanceThreshold(0.01); + seg.setInputCloud(cloud); + seg.segment(*inliers, *coefficients); + + // 平面が検出できなかった場合 + if (inliers->indices.size() <= 0) { + RCLCPP_INFO(this->get_logger(), "Could not estimate a planar model for the given dataset."); + return false; + } + + // 検出した平面を削除 + pcl::ExtractIndices extract; + extract.setInputCloud(cloud); + extract.setIndices(inliers); + extract.setNegative(true); + extract.filter(*cloud); + return true; + } + + std::vector clustering( + std::shared_ptr> & cloud) + { + auto tree = std::make_shared>(); + tree->setInputCloud(cloud); + + std::vector cluster_indices; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(0.02); + ec.setMinClusterSize(10); + ec.setMaxClusterSize(250); + ec.setSearchMethod(tree); + ec.setInputCloud(cloud); + ec.extract(cluster_indices); + return cluster_indices; + } + + void broadcast_cluster_position( + std::shared_ptr> & cloud_input, + std::shared_ptr> & cloud_output, + std::vector & cluster_indices, + std_msgs::msg::Header & tf_header) + { + int cluster_i = 0; + enum COLOR_RGB + { + RED = 0, + GREEN, + BLUE, + COLOR_MAX + }; + const int CLUSTER_MAX = 10; + const int CLUSTER_COLOR[CLUSTER_MAX][COLOR_MAX] = { + {230, 0, 18}, {243, 152, 18}, {255, 251, 0}, + {143, 195, 31}, {0, 153, 68}, {0, 158, 150}, + {0, 160, 233}, {0, 104, 183}, {29, 32, 136}, + {146, 7, 131} + }; + + for (const auto & point_indices : cluster_indices) { + auto cloud_cluster = std::make_shared>(); + // 点群の色を変更 + for (const auto & point_i : point_indices.indices) { + cloud_input->points[point_i].r = CLUSTER_COLOR[cluster_i][RED]; + cloud_input->points[point_i].g = CLUSTER_COLOR[cluster_i][GREEN]; + cloud_input->points[point_i].b = CLUSTER_COLOR[cluster_i][BLUE]; + cloud_cluster->points.push_back(cloud_input->points[point_i]); + } + // 点群数の入力 + cloud_cluster->width = cloud_cluster->points.size(); + cloud_cluster->height = 1; + // 無効なpointがないのでis_denseはtrue + cloud_cluster->is_dense = true; + // 配信用の点群に追加 + *cloud_output += *cloud_cluster; + + // tfの配信 + // 点群の重心位置を物体位置として配信する + Eigen::Vector4f cluster_centroid; + pcl::compute3DCentroid(*cloud_cluster, cluster_centroid); + geometry_msgs::msg::TransformStamped t; + t.header = tf_header; + t.child_frame_id = "target_" + std::to_string(cluster_i); + t.transform.translation.x = cluster_centroid.x(); + t.transform.translation.y = cluster_centroid.y(); + t.transform.translation.z = cluster_centroid.z(); + t.transform.rotation.x = 0.0; + t.transform.rotation.y = 0.0; + t.transform.rotation.z = 0.0; + t.transform.rotation.w = 1.0; + tf_broadcaster_->sendTransform(t); + + // 設定した最大クラスタ数を超えたら処理を終える + cluster_i++; + if (cluster_i >= CLUSTER_MAX) { + break; + } + } + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From b5dc45bd64c081f55b7fec0fe87125314c45a171 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Mon, 20 Feb 2023 15:34:06 +0900 Subject: [PATCH 2/9] =?UTF-8?q?=E3=83=90=E3=83=BC=E3=82=B8=E3=83=A7?= =?UTF-8?q?=E3=83=B3=E3=82=BF=E3=82=B0=E3=82=924.2.0=E3=81=AB=E6=9B=B4?= =?UTF-8?q?=E6=96=B0=20(#177)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit v4.2.0に更新 --- crane_x7/package.xml | 2 +- crane_x7_control/package.xml | 2 +- crane_x7_examples/package.xml | 2 +- crane_x7_gazebo/package.xml | 2 +- crane_x7_moveit_config/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/crane_x7/package.xml b/crane_x7/package.xml index bef41c87..f8986eb9 100644 --- a/crane_x7/package.xml +++ b/crane_x7/package.xml @@ -2,7 +2,7 @@ crane_x7 - 4.1.0 + 4.2.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 b1b69704..35c98ac5 100644 --- a/crane_x7_control/package.xml +++ b/crane_x7_control/package.xml @@ -2,7 +2,7 @@ crane_x7_control - 4.1.0 + 4.2.0 The CRANE-X7 control package RT Corporation Apache License 2.0 diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml index 4b536129..d2a85412 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -2,7 +2,7 @@ crane_x7_examples - 4.1.0 + 4.2.0 CRANE-X7 examples package RT Corporation Apache License 2.0 diff --git a/crane_x7_gazebo/package.xml b/crane_x7_gazebo/package.xml index 7ef86122..a4e9133f 100644 --- a/crane_x7_gazebo/package.xml +++ b/crane_x7_gazebo/package.xml @@ -2,7 +2,7 @@ crane_x7_gazebo - 4.1.0 + 4.2.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 6505e106..753c4e95 100644 --- a/crane_x7_moveit_config/package.xml +++ b/crane_x7_moveit_config/package.xml @@ -2,7 +2,7 @@ crane_x7_moveit_config - 4.1.0 + 4.2.0 CRANE-X7 move_group config package RT Corporation Apache License 2.0 From 8f040478ba98c3809d4f4667d59c5b06a5875479 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Fri, 26 May 2023 19:20:02 +0900 Subject: [PATCH 3/9] =?UTF-8?q?=E8=89=B2=E8=AA=8D=E8=AD=98=E3=82=B5?= =?UTF-8?q?=E3=83=B3=E3=83=97=E3=83=AB=E3=81=AE=E8=BF=BD=E5=8A=A0=20(#183)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * 色認識追加 * 二値化画像を配信 * 不要な処理を削除 * RVizに二値化画像を表示 * 距離のオフセット追加 * 変数名変更 * カーネルを変更 * 不要な空行を削除 * 画像取得時のheaderをtfに使用 * コメント、変数名修正 * Update README * コメント修正 * 変数名変更 --- crane_x7_examples/CMakeLists.txt | 3 + crane_x7_examples/README.md | 38 +++- .../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 | 1 + crane_x7_examples/src/color_detection.cpp | 189 ++++++++++++++++++ .../src/point_cloud_detection.cpp | 6 +- 8 files changed, 250 insertions(+), 22 deletions(-) create mode 100644 crane_x7_examples/src/color_detection.cpp 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..69fcae39 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,32 @@ 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 +[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..e0dd60a7 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -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軸方向のフィルタリングで不要な点群が除去できない場合に使用してみてください /* From 01f16d67c166c40037777e0c64cdc470d8e0834f Mon Sep 17 00:00:00 2001 From: Kuwagata Date: Mon, 5 Jun 2023 13:47:54 +0900 Subject: [PATCH 4/9] =?UTF-8?q?=E5=8B=95=E7=94=BBURL=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_x7_examples/README.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/crane_x7_examples/README.md b/crane_x7_examples/README.md index 69fcae39..5355116b 100644 --- a/crane_x7_examples/README.md +++ b/crane_x7_examples/README.md @@ -280,6 +280,8 @@ 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) --- From bcba8f9750bc3075602590c4f75496732af6181e Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Tue, 6 Jun 2023 16:25:19 +0900 Subject: [PATCH 5/9] =?UTF-8?q?=E3=83=90=E3=83=BC=E3=82=B8=E3=83=A7?= =?UTF-8?q?=E3=83=B3=E3=82=BF=E3=82=B0=E3=82=924.3.0=E3=81=AB=E6=9B=B4?= =?UTF-8?q?=E6=96=B0=20(#184)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_x7/package.xml | 2 +- crane_x7_control/package.xml | 2 +- crane_x7_examples/package.xml | 2 +- crane_x7_gazebo/package.xml | 2 +- crane_x7_moveit_config/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) 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/package.xml b/crane_x7_examples/package.xml index e0dd60a7..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 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 From cfa26ef1c4c7bc17a2e51ceeaec0fdd072a04bd8 Mon Sep 17 00:00:00 2001 From: Shota Aoki Date: Thu, 24 Aug 2023 17:51:17 +0900 Subject: [PATCH 6/9] Remove gz ros2 controll installation. (#189) Remove gz ros2 controll installation --- .ci.rosinstall | 6 +----- README.en.md | 1 - README.md | 1 - 3 files changed, 1 insertion(+), 7 deletions(-) diff --git a/.ci.rosinstall b/.ci.rosinstall index dbb5477f..d5d7a9ed 100644 --- a/.ci.rosinstall +++ b/.ci.rosinstall @@ -1,8 +1,4 @@ - git: uri: https://github.com/rt-net/crane_x7_description.git local-name: crane_x7_description - version: ros2 -- git: - uri: https://github.com/ros-controls/gz_ros2_control.git - local-name: gz_ros2_control - version: humble + version: ros2 \ No newline at end of file diff --git a/README.en.md b/README.en.md index 87bc3af2..68e5b3e6 100644 --- a/README.en.md +++ b/README.en.md @@ -55,7 +55,6 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git # Install dependencies -$ git clone -b humble https://github.com/ros-controls/gz_ros2_control.git $ rosdep install -r -y -i --from-paths . # Build & Install diff --git a/README.md b/README.md index 49d38764..8eb5fc2e 100644 --- a/README.md +++ b/README.md @@ -57,7 +57,6 @@ $ git clone -b ros2 https://github.com/rt-net/crane_x7_ros.git $ git clone -b ros2 https://github.com/rt-net/crane_x7_description.git # Install dependencies -$ git clone -b humble https://github.com/ros-controls/gz_ros2_control.git $ rosdep install -r -y -i --from-paths . # Build & Install From 2f074eb2b1e49b740a3ed68831736bf7c053fca1 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Thu, 12 Oct 2023 20:52:39 +0900 Subject: [PATCH 7/9] =?UTF-8?q?Gazebo=E3=81=AEclock=E3=82=92topic=E3=81=B8?= =?UTF-8?q?=E9=85=8D=E4=BF=A1=20(#192)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ros_gz_bridge追加 --- crane_x7_gazebo/launch/crane_x7_with_table.launch.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py index 59801bf5..f68e0570 100644 --- a/crane_x7_gazebo/launch/crane_x7_with_table.launch.py +++ b/crane_x7_gazebo/launch/crane_x7_with_table.launch.py @@ -82,6 +82,13 @@ def generate_launch_description(): output='screen', ) + bridge = Node( + package='ros_gz_bridge', + executable='parameter_bridge', + arguments=['/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock'], + output='screen' + ) + return LaunchDescription([ SetParameter(name='use_sim_time', value=True), ign_gazebo, @@ -89,5 +96,6 @@ def generate_launch_description(): move_group, spawn_joint_state_controller, spawn_arm_controller, - spawn_gripper_controller + spawn_gripper_controller, + bridge ]) From deaa39a9c821b975b450597d62471c5d8f054c93 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Tue, 17 Oct 2023 10:24:50 +0900 Subject: [PATCH 8/9] =?UTF-8?q?goal=5Ftolerance=E3=81=AE=E8=A8=AD=E5=AE=9A?= =?UTF-8?q?=E3=81=A8=E6=8A=8A=E6=8C=81=E8=A7=92=E3=81=AE=E8=AA=BF=E6=95=B4?= =?UTF-8?q?=20(#193)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * goal_tolerance調整 * グリッパ角調整 --- crane_x7_control/config/crane_x7_controllers.yaml | 1 + crane_x7_examples/src/pick_and_place_tf.cpp | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/crane_x7_control/config/crane_x7_controllers.yaml b/crane_x7_control/config/crane_x7_controllers.yaml index 43e74bd2..7bdfa64c 100644 --- a/crane_x7_control/config/crane_x7_controllers.yaml +++ b/crane_x7_control/config/crane_x7_controllers.yaml @@ -29,6 +29,7 @@ crane_x7_arm_controller: crane_x7_gripper_controller: ros__parameters: joint: crane_x7_gripper_finger_a_joint + goal_tolerance: 0.1 command_interfaces: - position diff --git a/crane_x7_examples/src/pick_and_place_tf.cpp b/crane_x7_examples/src/pick_and_place_tf.cpp index a875c23f..82c0fe83 100644 --- a/crane_x7_examples/src/pick_and_place_tf.cpp +++ b/crane_x7_examples/src/pick_and_place_tf.cpp @@ -163,7 +163,7 @@ class PickAndPlaceTf : public rclcpp::Node { const double GRIPPER_DEFAULT = 0.0; const double GRIPPER_OPEN = angles::from_degrees(60.0); - const double GRIPPER_CLOSE = angles::from_degrees(15.0); + const double GRIPPER_CLOSE = angles::from_degrees(20.0); // 何かを掴んでいた時のためにハンドを開閉 control_gripper(GRIPPER_OPEN); From 8b7a382756f46bba363e459b5a93d43cb134e7d7 Mon Sep 17 00:00:00 2001 From: Kuwamai Date: Mon, 18 Dec 2023 11:54:20 +0900 Subject: [PATCH 9/9] Prepare for release 4.4.0 (#194) 4.4.0 --- crane_x7/package.xml | 2 +- crane_x7_control/package.xml | 2 +- crane_x7_examples/package.xml | 2 +- crane_x7_gazebo/package.xml | 2 +- crane_x7_moveit_config/package.xml | 2 +- 5 files changed, 5 insertions(+), 5 deletions(-) diff --git a/crane_x7/package.xml b/crane_x7/package.xml index 90783e93..c5f54c82 100644 --- a/crane_x7/package.xml +++ b/crane_x7/package.xml @@ -2,7 +2,7 @@ crane_x7 - 4.3.0 + 4.4.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 43578cd8..7c5173cc 100644 --- a/crane_x7_control/package.xml +++ b/crane_x7_control/package.xml @@ -2,7 +2,7 @@ crane_x7_control - 4.3.0 + 4.4.0 The CRANE-X7 control package RT Corporation Apache License 2.0 diff --git a/crane_x7_examples/package.xml b/crane_x7_examples/package.xml index 1b8eea2a..f5157997 100644 --- a/crane_x7_examples/package.xml +++ b/crane_x7_examples/package.xml @@ -2,7 +2,7 @@ crane_x7_examples - 4.3.0 + 4.4.0 CRANE-X7 examples package RT Corporation Apache License 2.0 diff --git a/crane_x7_gazebo/package.xml b/crane_x7_gazebo/package.xml index de03b7ef..878b6c56 100644 --- a/crane_x7_gazebo/package.xml +++ b/crane_x7_gazebo/package.xml @@ -2,7 +2,7 @@ crane_x7_gazebo - 4.3.0 + 4.4.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 50edc60f..372c33f5 100644 --- a/crane_x7_moveit_config/package.xml +++ b/crane_x7_moveit_config/package.xml @@ -2,7 +2,7 @@ crane_x7_moveit_config - 4.3.0 + 4.4.0 CRANE-X7 move_group config package RT Corporation Apache License 2.0