diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 1da0016..d4ed8da 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -24,6 +24,8 @@ find_package(control_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) +find_package(pcl_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rclcpp_action REQUIRED) @@ -115,6 +117,8 @@ set(executable_list waist_control pick_and_place_right_arm_waist pick_and_place_left_arm + pick_and_place_tf + point_cloud_detection ) foreach(loop_var IN LISTS executable_list) add_executable(${loop_var} src/${loop_var}.cpp) @@ -126,6 +130,8 @@ foreach(loop_var IN LISTS executable_list) image_geometry moveit_ros_planning_interface OpenCV + pcl_ros + pcl_conversions rclcpp tf2_geometry_msgs control_msgs diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 58eb169..cec0bd2 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -19,6 +19,7 @@ - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - [head\_camera\_tracking](#head_camera_tracking) - [chest\_camera\_tracking](#chest_camera_tracking) + - [point\_cloud\_detection](#point_cloud_detection) ## 準備（実機を使う場合） @@ -93,6 +94,7 @@ ros2 launch sciurus17_examples example.launch.py example:='gripper_control' use_ - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) - [head\_camera\_tracking](#head_camera_tracking) - [chest\_camera\_tracking](#chest_camera_tracking) +- [point\_cloud\_detection](#point_cloud_detection) 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -203,3 +205,21 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py [back to example list](#examples) --- + +### point_cloud_detection + +点群から物体を検出して掴むコード例です。 + +検出された物体位置はtfのフレームとして配信されます。 +tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。 +掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。 +物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 + +次のコマンドを実行します +```sh +ros2 launch sciurus17_examples camera_example.launch.py example:='point_cloud_detection' +``` + +[back to example list](#examples) + +--- diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py new file mode 100644 index 0000000..29021d4 --- /dev/null +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -0,0 +1,89 @@ +# Copyright 2024 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. + +import os + +from ament_index_python.packages import get_package_share_directory +from sciurus17_description.robot_description_loader import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import SetParameter +from launch_ros.actions import Node +import yaml + + +def load_file(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return file.read() + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, 'r') as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + + +def generate_launch_description(): + description_loader = RobotDescriptionLoader() + + robot_description_semantic_config = load_file( + 'sciurus17_moveit_config', 'config/sciurus17.srdf') + robot_description_semantic = { + 'robot_description_semantic': robot_description_semantic_config} + + kinematics_yaml = load_yaml('sciurus17_moveit_config', 'config/kinematics.yaml') + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='point_cloud_detection', + description=('Set an example executable name: ' + '[point_cloud_detection]') + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', default_value='false', + description=('Set true when using the gazebo simulator.') + ) + + picking_node = Node(name="pick_and_place_tf", + package='sciurus17_examples', + executable='pick_and_place_tf', + output='screen', + parameters=[{'robot_description': description_loader.load()}, + robot_description_semantic, + kinematics_yaml]) + + detection_node = Node(name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples', + executable=LaunchConfiguration('example'), + output='screen') + + return LaunchDescription([ + declare_use_sim_time, + SetParameter(name='use_sim_time', value=LaunchConfiguration('use_sim_time')), + picking_node, + declare_example_name, + detection_node + ]) diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index 50c7f53..92e4201 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -24,6 +24,7 @@ image_geometry libopencv-dev moveit_ros_planning_interface + pcl_ros rclcpp rclcpp_components tf2_geometry_msgs diff --git a/sciurus17_examples/src/pick_and_place_tf.cpp b/sciurus17_examples/src/pick_and_place_tf.cpp new file mode 100644 index 0000000..96d3fe9 --- /dev/null +++ b/sciurus17_examples/src/pick_and_place_tf.cpp @@ -0,0 +1,347 @@ +// Copyright 2024 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://github.com/ros-planning/moveit2_tutorials/blob +// /a547cf49ff7d1fe16a93dfe020c6027bcb035b51/doc/move_group_interface +// /src/move_group_interface_tutorial.cpp +// https://docs.ros.org/en/humble/Tutorials/Intermediate/Tf2/Writing-A-Tf2-Listener-Cpp.html + +#include +#include +#include +#include + +#include "angles/angles.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "moveit/move_group_interface/move_group_interface.h" +#include "pose_presets.hpp" +#include "rclcpp/rclcpp.hpp" +#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp" +#include "tf2/convert.h" +#include "tf2/exceptions.h" +#include "tf2_ros/transform_listener.h" +#include "tf2_ros/buffer.h" +using namespace std::chrono_literals; +using MoveGroupInterface = moveit::planning_interface::MoveGroupInterface; + +class PickAndPlaceTf : public rclcpp::Node +{ +public: + PickAndPlaceTf( + rclcpp::Node::SharedPtr move_group_neck_node, + rclcpp::Node::SharedPtr move_group_l_arm_node, + rclcpp::Node::SharedPtr move_group_l_gripper_node, + rclcpp::Node::SharedPtr move_group_r_arm_node, + rclcpp::Node::SharedPtr move_group_r_gripper_node) + : Node("pick_and_place_tf_node") + { + using namespace std::placeholders; + move_group_neck_ = + std::make_shared(move_group_neck_node, "neck_group"); + move_group_neck_->setMaxVelocityScalingFactor(0.1); + move_group_neck_->setMaxAccelerationScalingFactor(0.1); + + move_group_l_arm_ = + std::make_shared(move_group_l_arm_node, "l_arm_waist_group"); + move_group_l_arm_->setMaxVelocityScalingFactor(0.1); + move_group_l_arm_->setMaxAccelerationScalingFactor(0.1); + + move_group_l_gripper_ = + std::make_shared(move_group_l_gripper_node, "l_gripper_group"); + move_group_l_gripper_->setMaxVelocityScalingFactor(1.0); + move_group_l_gripper_->setMaxAccelerationScalingFactor(1.0); + + move_group_r_arm_ = + std::make_shared(move_group_r_arm_node, "r_arm_waist_group"); + move_group_r_arm_->setMaxVelocityScalingFactor(0.1); + move_group_r_arm_->setMaxAccelerationScalingFactor(0.1); + + move_group_r_gripper_ = + std::make_shared(move_group_r_gripper_node, "r_gripper_group"); + move_group_r_gripper_->setMaxVelocityScalingFactor(1.0); + move_group_r_gripper_->setMaxAccelerationScalingFactor(1.0); + + // 姿勢を初期化 + init_body(); + + // 可動範囲を制限する + moveit_msgs::msg::Constraints constraints; + constraints.name = "arm_constraints"; + + // 腰軸の可動範囲を制限する + moveit_msgs::msg::JointConstraint joint_constraint; + joint_constraint.joint_name = "waist_yaw_joint"; + joint_constraint.position = 0.0; + joint_constraint.tolerance_above = angles::from_degrees(45); + joint_constraint.tolerance_below = angles::from_degrees(45); + joint_constraint.weight = 1.0; + constraints.joint_constraints.push_back(joint_constraint); + + move_group_l_arm_->setPathConstraints(constraints); + move_group_r_arm_->setPathConstraints(constraints); + + tf_buffer_ = + std::make_unique(this->get_clock(), 2s); + tf_listener_ = + std::make_shared(*tf_buffer_); + + timer_ = this->create_wall_timer( + 500ms, std::bind(&PickAndPlaceTf::on_timer, this)); + } + +private: + enum class ArmSide + { + LEFT, + RIGHT + }; + + void on_timer() + { + // target_0のtf位置姿勢を取得 + geometry_msgs::msg::TransformStamped tf_msg; + + try { + tf_msg = tf_buffer_->lookupTransform( + "base_link", "target_0", + tf2::TimePointZero); + } catch (const tf2::TransformException & ex) { + RCLCPP_INFO( + this->get_logger(), "Could not transform base_link to target: %s", + ex.what()); + return; + } + + rclcpp::Time now = this->get_clock()->now(); + constexpr std::chrono::nanoseconds FILTERING_TIME = 2s; + constexpr std::chrono::nanoseconds STOP_TIME_THRESHOLD = 3s; + constexpr 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(); + constexpr double TARGET_Z_MIN_LIMIT = 0.04; + constexpr double TARGET_X_MIN_LIMIT = 0.13; + constexpr double TARGET_X_MAX_LIMIT = 0.3; + + // 掴む物体位置を制限する + if (tf.getOrigin().z() < TARGET_Z_MIN_LIMIT) { + return; + } + if (tf.getOrigin().x() < TARGET_X_MIN_LIMIT || tf.getOrigin().x() > TARGET_X_MAX_LIMIT) { + return; + } + + // 検出されてから2秒以上経過した物体は掴まない + if (TF_ELAPSED_TIME > FILTERING_TIME.count()) { + return; + } + + // 動いている物体は掴まない + double tf_diff = (tf_past_.getOrigin() - tf.getOrigin()).length(); + if (tf_diff > DISTANCE_THRESHOLD) { + tf_past_ = tf; + return; + } + + // 物体が3秒以上停止している場合ピッキング動作開始 + if (TF_STOP_TIME < STOP_TIME_THRESHOLD.count()) { + return; + } + + picking(tf.getOrigin()); + } + + void init_body() + { + const double INITIAL_YAW_ANGLE = angles::from_degrees(0.0); + const double INITIAL_PITCH_ANGLE = angles::from_degrees(-80.0); + + move_group_l_arm_->setNamedTarget("l_arm_waist_init_pose"); + move_group_l_arm_->move(); + move_group_r_arm_->setNamedTarget("r_arm_waist_init_pose"); + move_group_r_arm_->move(); + + std::vector joint_values; + joint_values.push_back(INITIAL_YAW_ANGLE); + joint_values.push_back(INITIAL_PITCH_ANGLE); + move_group_neck_->setJointValueTarget(joint_values); + move_group_neck_->move(); + } + + void picking(tf2::Vector3 target_position) + { + // グリッパ開閉角度 + constexpr double GRIPPER_CLOSE = 0.0; + const double GRIPPER_OPEN = angles::from_degrees(50.0); + const double GRIPPER_GRASP = angles::from_degrees(20.0); + + // 物体を置く位置 + constexpr double PLACE_POSITION_X = 0.35; + constexpr double PLACE_POSITION_Y = 0.0; + constexpr double PLACE_POSITION_Z = 0.05; + + // 物体位置のオフセット + constexpr double APPROACH_OFFSET_Z = 0.12; + constexpr double GRASP_OFFSET_Z = 0.08; + + // 物体位置に応じて左右の腕を切り替え + ArmSide current_arm; + if (target_position.y() > 0) { + current_arm = ArmSide::LEFT; + } else { + current_arm = ArmSide::RIGHT; + } + + // 何かを掴んでいた時のためにハンドを開閉 + control_gripper(current_arm, GRIPPER_OPEN); + control_gripper(current_arm, GRIPPER_CLOSE); + + // 掴む準備をする + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + APPROACH_OFFSET_Z); + + // ハンドを開く + control_gripper(current_arm, GRIPPER_OPEN); + + // 掴みに行く + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + GRASP_OFFSET_Z); + + // ハンドを閉じる + control_gripper(current_arm, GRIPPER_GRASP); + + // 持ち上げる + control_arm( + current_arm, + target_position.x(), target_position.y(), target_position.z() + APPROACH_OFFSET_Z); + + // 移動する + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + APPROACH_OFFSET_Z); + + // 下ろす + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + GRASP_OFFSET_Z); + + // ハンドを開く + control_gripper(current_arm, GRIPPER_OPEN); + + // 少しだけハンドを持ち上げる + control_arm( + current_arm, + PLACE_POSITION_X, PLACE_POSITION_Y, PLACE_POSITION_Z + APPROACH_OFFSET_Z); + + // 初期姿勢に戻る + init_arm(current_arm); + + // ハンドを閉じる + control_gripper(current_arm, GRIPPER_CLOSE); + } + + // グリッパ制御 + void control_gripper(const ArmSide current_arm, const double angle) + { + auto joint_values = move_group_l_gripper_->getCurrentJointValues(); + + if (current_arm == ArmSide::LEFT) { + joint_values[0] = -angle; + move_group_l_gripper_->setJointValueTarget(joint_values); + move_group_l_gripper_->move(); + } + if (current_arm == ArmSide::RIGHT) { + joint_values[0] = angle; + move_group_r_gripper_->setJointValueTarget(joint_values); + move_group_r_gripper_->move(); + } + } + + // アーム制御 + void control_arm( + const ArmSide current_arm, const double x, const double y, const double z) + { + if (current_arm == ArmSide::LEFT) { + move_group_l_arm_->setPoseTarget( + pose_presets::left_arm_downward(x, y, z)); + move_group_l_arm_->move(); + } + if (current_arm == ArmSide::RIGHT) { + move_group_r_arm_->setPoseTarget( + pose_presets::right_arm_downward(x, y, z)); + move_group_r_arm_->move(); + } + } + + void init_arm(const ArmSide current_arm) + { + if (current_arm == ArmSide::LEFT) { + move_group_l_arm_->setNamedTarget("l_arm_waist_init_pose"); + move_group_l_arm_->move(); + } + if (current_arm == ArmSide::RIGHT) { + move_group_r_arm_->setNamedTarget("r_arm_waist_init_pose"); + move_group_r_arm_->move(); + } + } + + std::shared_ptr move_group_neck_; + std::shared_ptr move_group_l_arm_; + std::shared_ptr move_group_l_gripper_; + std::shared_ptr move_group_r_arm_; + std::shared_ptr move_group_r_gripper_; + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_{nullptr}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; + tf2::Stamped tf_past_; +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + auto move_group_neck_node = + rclcpp::Node::make_shared("move_group_neck_node", node_options); + auto move_group_l_arm_node = + rclcpp::Node::make_shared("move_group_l_arm_node", node_options); + auto move_group_l_gripper_node = + rclcpp::Node::make_shared("move_group_l_gripper_node", node_options); + auto move_group_r_arm_node = + rclcpp::Node::make_shared("move_group_r_arm_node", node_options); + auto move_group_r_gripper_node = + rclcpp::Node::make_shared("move_group_r_gripper_node", node_options); + + rclcpp::executors::MultiThreadedExecutor exec; + auto pick_and_place_tf_node = std::make_shared( + move_group_neck_node, + move_group_l_arm_node, + move_group_l_gripper_node, + move_group_r_arm_node, + move_group_r_gripper_node); + exec.add_node(pick_and_place_tf_node); + exec.add_node(move_group_neck_node); + exec.add_node(move_group_l_arm_node); + exec.add_node(move_group_l_gripper_node); + exec.add_node(move_group_r_arm_node); + exec.add_node(move_group_r_gripper_node); + exec.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/sciurus17_examples/src/point_cloud_detection.cpp b/sciurus17_examples/src/point_cloud_detection.cpp new file mode 100644 index 0000000..879d6d6 --- /dev/null +++ b/sciurus17_examples/src/point_cloud_detection.cpp @@ -0,0 +1,277 @@ +// Copyright 2024 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_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( + "/head_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.08~0.5m以外の点群を削除 + pcl::PassThrough pass; + pass.setInputCloud(cloud); + pass.setFilterFieldName("x"); + pass.setFilterLimits(0.08, 0.5); + pass.filter(*cloud); + + // Z軸方向0.03~0.2m以外の点群を削除 + // 物体が乗っている平面の点群を削除します + pass.setInputCloud(cloud); + pass.setFilterFieldName("z"); + pass.setFilterLimits(0.03, 0.2); + 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->empty()) { + 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.empty()) { + 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 + }; + constexpr int CLUSTER_MAX = 10; + constexpr 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>(); + cloud_cluster->points.reserve(cloud_input->points.size()); + // 点群の色を変更 + 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; +} diff --git a/sciurus17_moveit_config/config/moveit.rviz b/sciurus17_moveit_config/config/moveit.rviz index 7e52a93..daa9d41 100644 --- a/sciurus17_moveit_config/config/moveit.rviz +++ b/sciurus17_moveit_config/config/moveit.rviz @@ -5,7 +5,7 @@ Panels: Property Tree Widget: Expanded: ~ Splitter Ratio: 0.5 - Tree Height: 148 + Tree Height: 242 - Class: rviz_common/Help Name: Help - Class: rviz_common/Views @@ -444,6 +444,185 @@ Visualization Manager: Reliability Policy: Reliable Value: /chest_camera/image_raw Value: true + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background and overlay + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /head_camera/color/image_raw + Value: true + Visibility: + ClassifiedPointCloud: true + Grid: false + Image: false + MotionPlanning: true + PointCloud2: false + 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: ClassifiedPointCloud + 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 + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 1 + Frames: + All Enabled: false + base_link: + Value: true + body_link: + Value: false + chest_camera_link: + Value: false + dummy_mimic_fix_l: + Value: false + dummy_mimic_fix_r: + Value: false + head_camera_color_frame: + Value: false + head_camera_color_optical_frame: + Value: false + head_camera_depth_frame: + Value: false + head_camera_depth_optical_frame: + Value: false + head_camera_link: + Value: false + l_gripperA_link: + Value: false + l_gripperB_link: + Value: false + l_link1: + Value: false + l_link2: + Value: false + l_link3: + Value: false + l_link4: + Value: false + l_link5: + Value: false + l_link5_armarker: + Value: false + l_link6: + Value: false + l_link7: + Value: false + neck_pitch_link: + Value: false + neck_yaw_link: + Value: false + r_gripperA_link: + Value: false + r_gripperB_link: + Value: false + r_link1: + Value: false + r_link2: + Value: false + r_link3: + Value: false + r_link4: + Value: false + r_link5: + Value: false + r_link5_armarker: + Value: false + r_link6: + Value: false + r_link7: + Value: false + world: + Value: false + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + world: + base_link: + body_link: + chest_camera_link: + {} + l_link1: + l_link2: + l_link3: + l_link4: + l_link5: + l_link5_armarker: + {} + l_link6: + l_link7: + l_gripperA_link: + {} + l_gripperB_link: + {} + neck_yaw_link: + neck_pitch_link: + head_camera_link: + head_camera_color_frame: + head_camera_color_optical_frame: + {} + head_camera_depth_frame: + head_camera_depth_optical_frame: + {} + r_link1: + r_link2: + r_link3: + r_link4: + r_link5: + r_link5_armarker: + {} + r_link6: + r_link7: + r_gripperA_link: + {} + r_gripperB_link: + {} + dummy_mimic_fix_l: + {} + dummy_mimic_fix_r: + {} + Update Interval: 0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -483,22 +662,24 @@ Visualization Manager: Yaw: 0 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 975 Help: collapsed: false Hide Left Dock: false - Hide Right Dock: true + Hide Right Dock: false Image: collapsed: false MotionPlanning: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd00000002000000000000024200000375fc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d000000d1000000c900fffffffb0000000a0049006d0061006700650100000114000000d40000002800fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001ee000001c40000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00ffffff00000001000001c500000375fc0200000001fb0000000a00560069006500770073000000003d00000375000000a400ffffff000002680000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000200000000000001f300000375fc0200000004fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d0000012f000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000172000002400000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00ffffff000000010000016600000375fc0200000003fb0000000c00430061006d006500720061010000003d000001a80000002800fffffffb0000000a0049006d00610067006501000001eb000001c70000002800fffffffb0000000a00560069006500770073000000003d00000375000000a400ffffff000002900000037500000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: - collapsed: true - Width: 1200 - X: 156 - Y: 29 + collapsed: false + Width: 1525 + X: 171 + Y: 38 diff --git a/sciurus17_vision/launch/head_camera.launch.py b/sciurus17_vision/launch/head_camera.launch.py index f70cd87..a7cc0c8 100644 --- a/sciurus17_vision/launch/head_camera.launch.py +++ b/sciurus17_vision/launch/head_camera.launch.py @@ -27,8 +27,8 @@ def generate_launch_description(): 'camera_namespace': '', 'camera_name': 'head_camera', 'device_type': 'd415', - 'rgb_camera.profile': '640x360x30', - 'depth_module.profile': '640x360x30', + 'rgb_camera.color_profile': '640x360x30', + 'depth_module.depth_profile': '640x360x30', 'pointcloud.enable': 'true', 'align_depth.enable': 'true', }.items()