diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 4c60ea06..0d14b67b 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -22,11 +22,14 @@ find_package(cv_bridge REQUIRED) find_package(geometry_msgs REQUIRED) find_package(control_msgs REQUIRED) find_package(image_geometry REQUIRED) +find_package(image_transport REQUIRED) +find_package(message_filters 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_ros REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) @@ -34,7 +37,7 @@ find_package(rclcpp_components REQUIRED) include_directories(include) set(components_list - color_detection + color_detection_2d neck_jt_control object_tracker waist_jt_control @@ -51,18 +54,21 @@ foreach(loop_var IN LISTS components_list) cv_bridge geometry_msgs image_geometry + image_transport + message_filters OpenCV rclcpp + tf2_ros control_msgs) endforeach() -rclcpp_components_register_nodes(color_detection "sciurus17_examples::ColorDetection") +rclcpp_components_register_nodes(color_detection_2d "sciurus17_examples::ColorDetection2D") rclcpp_components_register_nodes(neck_jt_control "sciurus17_examples::NeckJtControl") rclcpp_components_register_nodes(object_tracker "sciurus17_examples::ObjectTracker") rclcpp_components_register_nodes(waist_jt_control "sciurus17_examples::WaistJtControl") install(TARGETS - color_detection + color_detection_2d neck_jt_control object_tracker waist_jt_control @@ -121,6 +127,7 @@ set(executable_list pick_and_place_right_arm_waist pick_and_place_left_arm pick_and_place_tf + color_detection point_cloud_detection ) foreach(loop_var IN LISTS executable_list) @@ -131,11 +138,14 @@ foreach(loop_var IN LISTS executable_list) cv_bridge geometry_msgs image_geometry + image_transport + message_filters moveit_ros_planning_interface OpenCV pcl_ros pcl_conversions rclcpp + tf2_ros tf2_geometry_msgs control_msgs ) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 78c9f3ee..b07b7d4d 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) + - [color\_detection](#color_detection) - [point\_cloud\_detection](#point_cloud_detection) ## Setup @@ -289,12 +290,31 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py --- +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +- デフォルトでは青い物体の位置をtfのフレームとして配信します。 +- 検出した物体のフレーム名は `target_0` です。 +- 色の検出にはOpenCVを使用しています。 +- 検出した物体の距離は深度画像から取得します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples camera_example.launch.py example:='color_detection' +``` + +[Back to example list](#examples) + +--- + ### point_cloud_detection 点群から物体を検出して掴むコード例です。 - 検出された物体位置はtfのフレームとして配信されます。 -- tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。 +- 検出した物体のフレーム名は `target_0`、`target_1`、`target_2`…です。 - 掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。 - 物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 diff --git a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp similarity index 81% rename from sciurus17_examples/include/sciurus17_examples/color_detection.hpp rename to sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp index 8d4dfc63..a669fc07 100644 --- a/sciurus17_examples/include/sciurus17_examples/color_detection.hpp +++ b/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp @@ -12,8 +12,8 @@ // See the License for the specific language governing permissions and // limitations under the License. -#ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_ -#define SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_ +#ifndef SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_ +#define SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_ #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -23,10 +23,10 @@ namespace sciurus17_examples { -class ColorDetection : public rclcpp::Node +class ColorDetection2D : public rclcpp::Node { public: - explicit ColorDetection(const rclcpp::NodeOptions & options); + explicit ColorDetection2D(const rclcpp::NodeOptions & options); private: rclcpp::Subscription::SharedPtr image_subscription_; @@ -39,4 +39,4 @@ class ColorDetection : public rclcpp::Node } // namespace sciurus17_examples -#endif // SCIURUS17_EXAMPLES__COLOR_DETECTION_HPP_ +#endif // SCIURUS17_EXAMPLES__COLOR_DETECTION_2D_HPP_ diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 68cb1a79..23422cb6 100644 --- a/sciurus17_examples/launch/camera_example.launch.py +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -25,7 +25,10 @@ def generate_launch_description(): declare_example_name = DeclareLaunchArgument( 'example', default_value='point_cloud_detection', - description=('Set an example executable name: [point_cloud_detection]'), + description=( + 'Set an example executable name: ' + '[color_detection, point_cloud_detection]' + ), ) declare_use_sim_time = DeclareLaunchArgument( diff --git a/sciurus17_examples/launch/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py index 06f6ab6e..54e40d62 100644 --- a/sciurus17_examples/launch/chest_camera_tracking.launch.py +++ b/sciurus17_examples/launch/chest_camera_tracking.launch.py @@ -35,10 +35,10 @@ def generate_launch_description(): output='screen', composable_node_descriptions=[ ComposableNode( - name='color_detection', + name='color_detection_2d', namespace='chest_camera_tracking', package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection', + plugin='sciurus17_examples::ColorDetection2D', remappings=[('/image_raw', '/chest_camera/image_raw')], extra_arguments=[{'use_intra_process_comms': True}], ), diff --git a/sciurus17_examples/launch/example.launch.py b/sciurus17_examples/launch/example.launch.py index fd237fdc..aaa4c6c4 100644 --- a/sciurus17_examples/launch/example.launch.py +++ b/sciurus17_examples/launch/example.launch.py @@ -28,10 +28,10 @@ def generate_launch_description(): description=( 'Set an example executable name: ' '[gripper_control, pose_groupstate, joint_values, neck_control, waist_control,' - 'pick_and_place_right_arm_waist, pick_and_place_left_arm]' + 'pick_and_place_right_arm_waist, pick_and_place_left_arm, head_camera_tracking, ' + 'chest_camera_tracking]' ), ) - declare_use_sim_time = DeclareLaunchArgument( 'use_sim_time', default_value='false', diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index bf7bca29..f66675d1 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -35,10 +35,10 @@ def generate_launch_description(): output='screen', composable_node_descriptions=[ ComposableNode( - name='color_detection', + name='color_detection_2d', namespace='head_camera_tracking', package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection', + plugin='sciurus17_examples::ColorDetection2D', remappings=[('/image_raw', '/head_camera/color/image_raw')], extra_arguments=[{'use_intra_process_comms': True}], ), diff --git a/sciurus17_examples/package.xml b/sciurus17_examples/package.xml index e75ec812..81689c21 100644 --- a/sciurus17_examples/package.xml +++ b/sciurus17_examples/package.xml @@ -22,11 +22,14 @@ cv_bridge geometry_msgs image_geometry + image_transport + message_filters libopencv-dev moveit_ros_planning_interface pcl_ros rclcpp rclcpp_components + tf2_ros tf2_geometry_msgs ament_lint_auto diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 71da591c..791e1797 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -14,135 +14,198 @@ // Reference: // https://www.opencv-srf.com/2010/09/object-detection-using-color-seperation.html -// https://docs.opencv.org/4.5.4/d0/d49/tutorial_moments.html -#include "sciurus17_examples/color_detection.hpp" +#include +#include +#include +#include #include "rclcpp/rclcpp.hpp" -#include "geometry_msgs/msg/point_stamped.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.hpp" +#include "tf2/LinearMath/Matrix3x3.hpp" +#include "tf2_ros/transform_broadcaster.h" #include "opencv2/opencv.hpp" +#include "opencv2/imgproc/imgproc.hpp" #include "cv_bridge/cv_bridge.hpp" +#include "image_geometry/pinhole_camera_model.hpp" +#include "image_transport/image_transport.hpp" +#include "image_transport/subscriber_filter.hpp" +#include "message_filters/subscriber.h" +#include "message_filters/synchronizer.h" +#include "message_filters/sync_policies/exact_time.h" + using std::placeholders::_1; +using std::placeholders::_2; +using std::placeholders::_3; -namespace sciurus17_examples +class ImageSubscriber : public rclcpp::Node { +public: + ImageSubscriber() + : Node("color_detection") + { + color_sub_.subscribe(this, "/head_camera/color/image_raw", "raw"); + depth_sub_.subscribe(this, "/head_camera/aligned_depth_to_color/image_raw", "raw"); + info_sub_.subscribe(this, "/head_camera/color/camera_info"); + + sync_ = std::make_unique>( + ExactPolicy(10), color_sub_, depth_sub_, info_sub_); + sync_->registerCallback(std::bind(&ImageSubscriber::sync_callback, this, _1, _2, _3)); + + image_thresholded_publisher_ = + this->create_publisher("image_thresholded", 10); + + tf_broadcaster_ = + std::make_unique(*this); + } -ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) -: Node("color_detection", options) -{ - image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); +private: + using ExactPolicy = message_filters::sync_policies::ExactTime< + sensor_msgs::msg::Image, + sensor_msgs::msg::Image, + sensor_msgs::msg::CameraInfo>; + image_transport::SubscriberFilter color_sub_; + image_transport::SubscriberFilter depth_sub_; + message_filters::Subscriber info_sub_; + std::unique_ptr> sync_; + rclcpp::Publisher::SharedPtr image_thresholded_publisher_; + std::unique_ptr tf_broadcaster_; + + void sync_callback( + const sensor_msgs::msg::Image::ConstSharedPtr & color_msg, + const sensor_msgs::msg::Image::ConstSharedPtr & depth_msg, + const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg) + { + // 青い物体を検出するように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_color = cv_bridge::toCvCopy(color_msg, color_msg->encoding); + + // 画像をRGBからHSVに変換 + cv::cvtColor(cv_color->image, cv_color->image, cv::COLOR_RGB2HSV); + // 画像処理用の変数を用意 + cv::Mat img_thresholded; + + // 画像の二値化 + cv::inRange( + cv_color->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 = cv::moments(img_thresholded); + double d_m01 = moment.m01; + double d_m10 = moment.m10; + double d_area = moment.m00; + + // 検出した領域のピクセル数が10000より大きい場合に把持位置を配信 + if (d_area <= 10000) { + return; + } - image_annotated_publisher_ = - this->create_publisher("image_annotated", 10); + // カメラモデル作成 + image_geometry::PinholeCameraModel camera_model; + + // カメラのパラメータを設定 + camera_model.fromCameraInfo(*info_msg); + + // 画像座標系における把持対象物の位置（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_msg, depth_msg->encoding); + + // カメラから把持対象物の表面までの距離 + double front_distance = 0.0; + // 深度画像アクセス用に小数座標を整数インデックスに変換 + const cv::Point point_int(static_cast(point.x), static_cast(point.y)); + + // エンコーディングの違いによる深度値の取得方法の違いに対応 + if (depth_msg->encoding == "16UC1") { + // RealSenseの深度画像フォーマット + front_distance = cv_depth->image.at(point_int) / 1000.0; + } else if (depth_msg->encoding == "32FC1") { + // Gazeboの深度画像フォーマット + front_distance = cv_depth->image.at(point_int); + } else { + RCLCPP_WARN( + this->get_logger(), + "Unsupported depth encoding: %s", + depth_msg->encoding.c_str()); + return; + } - object_point_publisher_ = - this->create_publisher("target_position", 10); -} + const auto center_distance = front_distance + DEPTH_OFFSET; -void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) -{ - // オレンジ色の物体を検出するようにHSVの範囲を設定 - const int LOW_H = 5, HIGH_H = 20; - const int LOW_S = 120, HIGH_S = 255; - const int LOW_V = 120, HIGH_V = 255; - - // 画像全体の10%以上の大きさで映った物体を検出 - const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; - - auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); - - // 画像をRGBからHSVに変換 - cv::Mat img_hsv; - cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); - - // 画像の二値化 - cv::Mat img_thresholded; - cv::inRange( - img_hsv, - 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::Mat img_annotated; - cv_img->image.copyTo(img_annotated, img_thresholded); - - // 二値化した領域の輪郭を取得 - std::vector> contours; - cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); - - if (contours.size()) { - // 最も面積の大きい領域を取得 - std::vector object_moments; - int max_area_i = -1; - int i = 0; - for (const auto & contour : contours) { - object_moments.push_back(cv::moments(contour)); - if (object_moments[max_area_i].m00 < object_moments[i].m00) { - max_area_i = i; - } - i++; + // 距離を取得できないか遠すぎる場合は把持しない + 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; } - if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { - // 画像座標系における物体検出位置（2D） - cv::Point2d object_point; - object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; - object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; - - RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); - - // 検出領域と検出位置を描画 - const cv::Scalar ANNOTATE_COLOR(256, 0, 256); - const int ANNOTATE_THICKNESS = 4; - const int ANNOTATE_RADIUS = 10; - cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); - cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); - - // 画像の中心を原点とした検出位置に変換 - cv::Point2d translated_object_point; - translated_object_point.x = object_point.x - msg->width / 2.0; - translated_object_point.y = object_point.y - msg->height / 2.0; - - // 検出位置を-1.0 ~ 1.0に正規化 - cv::Point2d normalized_object_point_; - if (msg->width != 0 && msg->height != 0) { - normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); - normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); - } - - // 検出位置を配信 - geometry_msgs::msg::PointStamped object_point_msg; - object_point_msg.header = msg->header; - object_point_msg.point.x = normalized_object_point_.x; - object_point_msg.point.y = normalized_object_point_.y; - object_point_publisher_->publish(object_point_msg); - } + // 把持対象物の位置を計算 + cv::Point3d object_position( + ray.x * center_distance, + ray.y * center_distance, + ray.z * center_distance); + + // 把持対象物の位置をTFに配信 + geometry_msgs::msg::TransformStamped t; + t.header = color_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; + 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); + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_thresholded_msg = + cv_bridge::CvImage(color_msg->header, "mono8", img_thresholded).toImageMsg(); + image_thresholded_publisher_->publish(*img_thresholded_msg); } +}; - // 閾値による二値化画像を配信 - sensor_msgs::msg::Image::SharedPtr img_annotated_msg = - cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); - image_annotated_publisher_->publish(*img_annotated_msg); +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; } - -} // namespace sciurus17_examples - -#include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) diff --git a/sciurus17_examples/src/color_detection_2d.cpp b/sciurus17_examples/src/color_detection_2d.cpp new file mode 100644 index 00000000..a3a0c9ce --- /dev/null +++ b/sciurus17_examples/src/color_detection_2d.cpp @@ -0,0 +1,148 @@ +// 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 +// https://docs.opencv.org/4.5.4/d0/d49/tutorial_moments.html + +#include "sciurus17_examples/color_detection_2d.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "geometry_msgs/msg/point_stamped.hpp" +#include "sensor_msgs/msg/image.hpp" +#include "opencv2/opencv.hpp" +#include "cv_bridge/cv_bridge.hpp" +using std::placeholders::_1; + +namespace sciurus17_examples +{ + +ColorDetection2D::ColorDetection2D(const rclcpp::NodeOptions & options) +: Node("color_detection_2d", options) +{ + image_subscription_ = this->create_subscription( + "/image_raw", 10, std::bind(&ColorDetection2D::image_callback, this, _1)); + + image_annotated_publisher_ = + this->create_publisher("image_annotated", 10); + + object_point_publisher_ = + this->create_publisher("target_position", 10); +} + +void ColorDetection2D::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +{ + // オレンジ色の物体を検出するようにHSVの範囲を設定 + const int LOW_H = 5, HIGH_H = 20; + const int LOW_S = 120, HIGH_S = 255; + const int LOW_V = 120, HIGH_V = 255; + + // 画像全体の10%以上の大きさで映った物体を検出 + const auto MIN_OBJECT_SIZE = msg->width * msg->height * 0.01; + + auto cv_img = cv_bridge::toCvShare(msg, msg->encoding); + + // 画像をRGBからHSVに変換 + cv::Mat img_hsv; + cv::cvtColor(cv_img->image, img_hsv, cv::COLOR_RGB2HSV); + + // 画像の二値化 + cv::Mat img_thresholded; + cv::inRange( + img_hsv, + 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::Mat img_annotated; + cv_img->image.copyTo(img_annotated, img_thresholded); + + // 二値化した領域の輪郭を取得 + std::vector> contours; + cv::findContours(img_thresholded, contours, CV_RETR_TREE, CV_CHAIN_APPROX_SIMPLE); + + if (contours.size()) { + // 最も面積の大きい領域を取得 + std::vector object_moments; + int max_area_i = -1; + int i = 0; + for (const auto & contour : contours) { + object_moments.push_back(cv::moments(contour)); + if (object_moments[max_area_i].m00 < object_moments[i].m00) { + max_area_i = i; + } + i++; + } + + if (object_moments[max_area_i].m00 > MIN_OBJECT_SIZE) { + // 画像座標系における物体検出位置（2D） + cv::Point2d object_point; + object_point.x = object_moments[max_area_i].m10 / object_moments[max_area_i].m00; + object_point.y = object_moments[max_area_i].m01 / object_moments[max_area_i].m00; + + RCLCPP_DEBUG_STREAM(this->get_logger(), "Detect at" << object_point << "."); + + // 検出領域と検出位置を描画 + const cv::Scalar ANNOTATE_COLOR(256, 0, 256); + const int ANNOTATE_THICKNESS = 4; + const int ANNOTATE_RADIUS = 10; + cv::drawContours(img_annotated, contours, max_area_i, ANNOTATE_COLOR, ANNOTATE_THICKNESS); + cv::circle(img_annotated, object_point, ANNOTATE_RADIUS, ANNOTATE_COLOR, -1); + + // 画像の中心を原点とした検出位置に変換 + cv::Point2d translated_object_point; + translated_object_point.x = object_point.x - msg->width / 2.0; + translated_object_point.y = object_point.y - msg->height / 2.0; + + // 検出位置を-1.0 ~ 1.0に正規化 + cv::Point2d normalized_object_point_; + if (msg->width != 0 && msg->height != 0) { + normalized_object_point_.x = translated_object_point.x / (msg->width / 2.0); + normalized_object_point_.y = translated_object_point.y / (msg->height / 2.0); + } + + // 検出位置を配信 + geometry_msgs::msg::PointStamped object_point_msg; + object_point_msg.header = msg->header; + object_point_msg.point.x = normalized_object_point_.x; + object_point_msg.point.y = normalized_object_point_.y; + object_point_publisher_->publish(object_point_msg); + } + } + + // 閾値による二値化画像を配信 + sensor_msgs::msg::Image::SharedPtr img_annotated_msg = + cv_bridge::CvImage(msg->header, msg->encoding, img_annotated).toImageMsg(); + image_annotated_publisher_->publish(*img_annotated_msg); +} + +} // namespace sciurus17_examples + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection2D) diff --git a/sciurus17_examples/src/pick_and_place_tf.cpp b/sciurus17_examples/src/pick_and_place_tf.cpp index f11f9558..3d1bcb71 100644 --- a/sciurus17_examples/src/pick_and_place_tf.cpp +++ b/sciurus17_examples/src/pick_and_place_tf.cpp @@ -125,7 +125,7 @@ class PickAndPlaceTf : public rclcpp::Node const auto FILTERING_TIME = rclcpp::Duration(2s); const auto STOP_TIME_THRESHOLD = rclcpp::Duration(3s); const double DISTANCE_THRESHOLD = 0.01; - const double TARGET_Z_MIN_LIMIT = 0.04; + const double TARGET_Z_MIN_LIMIT = 0.02; const double TARGET_X_MIN_LIMIT = 0.13; const double TARGET_X_MAX_LIMIT = 0.3; diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 0612f2c7..a259acf7 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -16,6 +16,7 @@ - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) + - [color\_detection](#color_detection) ## Setup @@ -50,6 +51,7 @@ ros2 launch sciurus17_examples_py example.launch.py example:='gripper_control' - [waist\_control](#waist_control) - [pick\_and\_place\_right\_arm\_waist](#pick_and_place_right_arm_waist) - [pick\_and\_place\_left\_arm](#pick_and_place_left_arm) +- [color\_detection](#color_detection) >[!NOTE] > 実行できるサンプルの一覧は、`example.launch.py`にオプション`-s`を付けて実行することで表示できます。 @@ -159,3 +161,22 @@ ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_lef [Back to example list](#examples) --- + +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +- デフォルトでは青い物体の位置をtfのフレームとして配信します。 +- 検出した物体のフレーム名は `target_0` です。 +- 色の検出にはOpenCVを使用しています。 +- 検出した物体の距離は深度画像から取得します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples_py camera_example.launch.py example:='color_detection' +``` + +[Back to example list](#examples) + +--- diff --git a/sciurus17_examples_py/launch/camera_example.launch.py b/sciurus17_examples_py/launch/camera_example.launch.py new file mode 100644 index 00000000..437993ff --- /dev/null +++ b/sciurus17_examples_py/launch/camera_example.launch.py @@ -0,0 +1,92 @@ +# Copyright 2026 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. + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder +from sciurus17_description.robot_description_loader import RobotDescriptionLoader + + +def generate_launch_description(): + declare_example_name = DeclareLaunchArgument( + 'example', + default_value='color_detection', + description=('Set an example executable name: [color_detection]'), + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) + + description_loader = RobotDescriptionLoader() + declare_loaded_description = DeclareLaunchArgument( + 'loaded_description', + default_value=description_loader.load(), + description='Set robot_description text. \ + It is recommended to use RobotDescriptionLoader() \ + in sciurus17_description.', + ) + + moveit_config = ( + MoveItConfigsBuilder('sciurus17') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('sciurus17_examples_py') + + '/config/sciurus17_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + moveit_config.move_group_capabilities = {'capabilities': ''} + + # 下記Issue対応のためここでパラメータを設定する + # https://github.com/moveit/moveit2/issues/2940#issuecomment-2401302214 + config_dict = moveit_config.to_dict() + config_dict.update({'use_sim_time': LaunchConfiguration('use_sim_time')}) + + picking_node = Node( + name='pick_and_place_tf', + package='sciurus17_examples_py', + executable='pick_and_place_tf', + output='screen', + parameters=[config_dict], + ) + + detection_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='sciurus17_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[config_dict], + ) + + return LaunchDescription( + [ + declare_use_sim_time, + declare_example_name, + declare_loaded_description, + picking_node, + detection_node, + ] + ) diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml index e3f630d1..fbcfbd54 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -11,8 +11,17 @@ rclpy std_msgs + cv_bridge + geometry_msgs + image_geometry + message_filters moveit moveit_py + python3-numpy + python3-opencv + python3-scipy + sensor_msgs + tf2_ros ament_copyright ament_flake8 diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py new file mode 100644 index 00000000..ef90ba3e --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -0,0 +1,160 @@ +# Copyright 2026 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 cv2 +from cv_bridge import CvBridge +from geometry_msgs.msg import TransformStamped +from image_geometry import PinholeCameraModel +import message_filters +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +from tf2_ros import TransformBroadcaster + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('color_detection') + self.color_sub = message_filters.Subscriber(self, Image, '/head_camera/color/image_raw') + self.depth_sub = message_filters.Subscriber( + self, Image, '/head_camera/aligned_depth_to_color/image_raw' + ) + self.info_sub = message_filters.Subscriber( + self, CameraInfo, '/head_camera/color/camera_info' + ) + + self.sync = message_filters.TimeSynchronizer( + [self.color_sub, self.depth_sub, self.info_sub], 10 + ) + self.sync.registerCallback(self.sync_callback) + + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) + + self.tf_broadcaster = TransformBroadcaster(self) + self.bridge = CvBridge() + + def sync_callback(self, color_msg, depth_msg, info_msg): + # 青い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H, HIGH_H = 100, 125 + LOW_S, HIGH_S = 100, 255 + LOW_V, HIGH_V = 30, 255 + + # カメラ画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(color_msg, desired_encoding=color_msg.encoding) + + # 画像をRGBからHSVに変換 + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_thresholded = cv2.inRange(cv_img, (LOW_H, LOW_S, LOW_V), (HIGH_H, HIGH_S, HIGH_V)) + + # ノイズ除去の処理 + kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_OPEN, kernel) + + # 穴埋めの処理 + img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) + + # 画像の検出領域におけるモーメントを計算 + moment = cv2.moments(img_thresholded) + d_m01 = moment['m01'] + d_m10 = moment['m10'] + d_area = moment['m00'] + + # 検出した領域のピクセル数が10000より大きい場合に把持位置を配信 + if d_area <= 10000: + return + + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(info_msg) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyPoint(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # 把持対象物までの距離を取得 + # 把持対象物の表面より少し奥を掴むように設定 + DEPTH_OFFSET = 0.015 + cv_depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding=depth_msg.encoding) + + # カメラから把持対象物の表面までの距離 + depth = cv_depth[int(point[1]), int(point[0])] + if depth_msg.encoding == '16UC1': + # RealSenseの深度画像フォーマット + front_distance = float(depth) / 1000.0 + elif depth_msg.encoding == '32FC1': + # Gazeboの深度画像フォーマット + front_distance = float(depth) + else: + self.get_logger().warn(f'Unsupported depth encoding: {depth_msg.encoding}') + return + + center_distance = front_distance + DEPTH_OFFSET + + # 距離を取得できないか遠すぎる場合は把持しない + DEPTH_MAX = 0.5 + DEPTH_MIN = 0.2 + if center_distance < DEPTH_MIN or center_distance > DEPTH_MAX: + self.get_logger().info(f'Failed to get depth at {point}.') + return + + # 把持対象物の位置を計算 + object_position = [ + ray[0] * center_distance, + ray[1] * center_distance, + ray[2] * center_distance, + ] + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = color_msg.header + t.child_frame_id = 'target_0' + t.transform.translation.x = object_position[0] + t.transform.translation.y = object_position[1] + t.transform.translation.z = object_position[2] + t.transform.rotation.x = 0.0 + t.transform.rotation.y = 0.0 + t.transform.rotation.z = 0.0 + t.transform.rotation.w = 1.0 + self.tf_broadcaster.sendTransform(t) + + # 閾値による二値化画像を配信 + img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding='mono8') + img_thresholded_msg.header = color_msg.header + self.image_thresholded_publisher.publish(img_thresholded_msg) + + +def main(args=None): + rclpy.init(args=args) + + image_subscriber = ImageSubscriber() + rclpy.spin(image_subscriber) + + image_subscriber.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py new file mode 100644 index 00000000..933892c6 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py @@ -0,0 +1,384 @@ +# Copyright 2026 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 math + +from geometry_msgs.msg import PoseStamped + +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) +from moveit_msgs.msg import Constraints, JointConstraint + +import numpy as np + +import rclpy +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sciurus17_examples_py.utils import plan_and_execute +from tf2_ros import TransformException, TransformListener, TransformStamped +from tf2_ros.buffer import Buffer + + +class ArmSide: + LEFT = 'left' + RIGHT = 'right' + + +class PickAndPlaceTf(Node): + def __init__(self): + super().__init__('pick_and_place_tf') + self.logger = self.get_logger() + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + self.tf_past = TransformStamped() + + # instantiate MoveItPy instance and get planning component + self.sciurus17 = MoveItPy(node_name='moveit_py') + self.logger.info('MoveItPy instance created') + + # 首制御用 planning component + self.neck = self.sciurus17.get_planning_component('neck_group') + # 左腕と腰制御用 planning component + self.l_arm_waist = self.sciurus17.get_planning_component('l_arm_waist_group') + # 左グリッパ制御用 planning component + self.l_gripper = self.sciurus17.get_planning_component('l_gripper_group') + # 右腕と腰制御用 planning component + self.r_arm_waist = self.sciurus17.get_planning_component('r_arm_waist_group') + # 右グリッパ制御用 planning component + self.r_gripper = self.sciurus17.get_planning_component('r_gripper_group') + + # instantiate a RobotState instance using the current robot model + self.robot_model = self.sciurus17.get_robot_model() + + self.arm_plan_request_params = PlanRequestParameters( + self.sciurus17, + 'ompl_rrtc_default', + ) + self.gripper_plan_request_params = PlanRequestParameters( + self.sciurus17, + 'ompl_rrtc_default', + ) + + # 動作速度の調整 + self.arm_plan_request_params.max_acceleration_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_velocity_scaling_factor = 0.1 # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # 可動範囲を制限する + constraints = Constraints() + constraints.name = 'arm_constraints' + + # 腰軸の可動範囲を制限する + jointConstraint = JointConstraint() + jointConstraint.joint_name = 'waist_yaw_joint' + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(45) + jointConstraint.tolerance_below = math.radians(45) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + self.l_arm_waist.set_path_constraints(path_constraints=constraints) + self.r_arm_waist.set_path_constraints(path_constraints=constraints) + + # 姿勢を初期化 + self.init_body() + + # Call on_timer function every 0.5 second + self.timer = self.create_timer(0.5, self.on_timer) + + def on_timer(self): + # target_0のtf位置姿勢を取得 + try: + tf_msg = self.tf_buffer.lookup_transform('base_link', 'target_0', rclpy.time.Time()) + except TransformException as ex: + self.logger.info(f'Could not transform base_link to target: {ex}') + return + + now = self.get_clock().now() + FILTERING_TIME = rclpy.duration.Duration(seconds=2) + STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) + DISTANCE_THRESHOLD = 0.01 + TARGET_Z_MIN_LIMIT = 0.02 + TARGET_X_MIN_LIMIT = 0.13 + TARGET_X_MAX_LIMIT = 0.3 + # 経過時間と停止時間を計算(nsec) + # 経過時間 + + tf_elapsed_time = now - rclpy.time.Time.from_msg(tf_msg.header.stamp) + # 停止時間 + tf_stop_time = now - rclpy.time.Time.from_msg(self.tf_past.header.stamp) + + # 掴む物体位置を制限する + if tf_msg.transform.translation.z < TARGET_Z_MIN_LIMIT: + return + if ( + tf_msg.transform.translation.x < TARGET_X_MIN_LIMIT + or tf_msg.transform.translation.x > TARGET_X_MAX_LIMIT + ): + return + + # 検出されてから2秒以上経過した物体は掴まない + if tf_elapsed_time > FILTERING_TIME: + return + + tf_diff = np.linalg.norm( + [ + self.tf_past.transform.translation.x - tf_msg.transform.translation.x, + self.tf_past.transform.translation.y - tf_msg.transform.translation.y, + self.tf_past.transform.translation.z - tf_msg.transform.translation.z, + ] + ) + + # 動いている物体は掴まない + if tf_diff > DISTANCE_THRESHOLD: + self.tf_past = tf_msg + return + + # 物体が3秒以上停止している場合ピッキング動作開始 + if tf_stop_time < STOP_TIME_THRESHOLD: + return + + self._picking(tf_msg.transform.translation) + + def _picking(self, target_position): + GRIPPER_CLOSE = 0.0 + GRIPPER_OPEN = math.radians(50.0) + GRIPPER_GRASP = math.radians(20.0) + + PLACE_POSITION_X = 0.35 + PLACE_POSITION_Y = 0.0 + PLACE_POSITION_Z = 0.05 + APPROACH_OFFSET_Z = 0.12 + GRASP_OFFSET_Z = 0.08 + + # 物体位置に応じて左右の腕を切り替え + if target_position.y > 0: + current_arm = ArmSide.LEFT + else: + current_arm = ArmSide.RIGHT + + # 何かを掴んでいた時のためにハンドを開閉 + self._control_gripper(current_arm, GRIPPER_OPEN) + self._control_gripper(current_arm, GRIPPER_CLOSE) + + # 掴む準備をする + self._control_arm( + current_arm, + target_position.x, + target_position.y, + target_position.z + APPROACH_OFFSET_Z, + ) + + # ハンドを開く + self._control_gripper(current_arm, GRIPPER_OPEN) + + # 掴みに行く + self._control_arm( + current_arm, + target_position.x, + target_position.y, + target_position.z + GRASP_OFFSET_Z, + ) + + # ハンドを閉じる + self._control_gripper(current_arm, GRIPPER_GRASP) + + # 持ち上げる + self._control_arm( + current_arm, + target_position.x, + target_position.y, + target_position.z + APPROACH_OFFSET_Z, + ) + + # 移動する + self._control_arm( + current_arm, + PLACE_POSITION_X, + PLACE_POSITION_Y, + PLACE_POSITION_Z + APPROACH_OFFSET_Z, + ) + + # 下ろす + self._control_arm( + current_arm, + PLACE_POSITION_X, + PLACE_POSITION_Y, + PLACE_POSITION_Z + GRASP_OFFSET_Z, + ) + + # ハンドを開く + self._control_gripper(current_arm, GRIPPER_OPEN) + + # 少しだけハンドを持ち上げる + self._control_arm( + current_arm, + PLACE_POSITION_X, + PLACE_POSITION_Y, + PLACE_POSITION_Z + APPROACH_OFFSET_Z, + ) + + # 初期姿勢に戻る + self.init_arm(current_arm) + + # ハンドを閉じる + self._control_gripper(current_arm, GRIPPER_CLOSE) + + # グリッパ制御 + def _control_gripper(self, current_arm, angle): + robot_state = RobotState(self.robot_model) + + if current_arm == ArmSide.LEFT: + self.l_gripper.set_start_state_to_current_state() + robot_state.set_joint_group_positions('l_gripper_group', [-angle]) + self.l_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.sciurus17, + self.l_gripper, + self.logger, + single_plan_parameters=self.gripper_plan_request_params, + ) + if current_arm == ArmSide.RIGHT: + self.r_gripper.set_start_state_to_current_state() + robot_state.set_joint_group_positions('r_gripper_group', [angle]) + self.r_gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.sciurus17, + self.r_gripper, + self.logger, + single_plan_parameters=self.gripper_plan_request_params, + ) + + # アーム制御 + def _control_arm(self, current_arm, x, y, z): + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'base_link' + goal_pose.pose.position.x = x + goal_pose.pose.position.y = y + goal_pose.pose.position.z = z + + if current_arm == ArmSide.LEFT: + quat = Rotation.from_euler('xyz', [-90, 0, 0], degrees=True).as_quat() + goal_pose.pose.orientation.x = quat[0] + goal_pose.pose.orientation.y = quat[1] + goal_pose.pose.orientation.z = quat[2] + goal_pose.pose.orientation.w = quat[3] + self.l_arm_waist.set_start_state_to_current_state() + self.l_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='l_link7') + result = plan_and_execute( + self.sciurus17, + self.l_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + if current_arm == ArmSide.RIGHT: + quat = Rotation.from_euler('xyz', [90, 0, 0], degrees=True).as_quat() + goal_pose.pose.orientation.x = quat[0] + goal_pose.pose.orientation.y = quat[1] + goal_pose.pose.orientation.z = quat[2] + goal_pose.pose.orientation.w = quat[3] + self.r_arm_waist.set_start_state_to_current_state() + self.r_arm_waist.set_goal_state(pose_stamped_msg=goal_pose, pose_link='r_link7') + result = plan_and_execute( + self.sciurus17, + self.r_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + return None + + def init_body(self): + INITIAL_YAW_ANGLE = math.radians(0.0) + INITIAL_PITCH_ANGLE = math.radians(-80.0) + + self.l_arm_waist.set_start_state_to_current_state() + self.l_arm_waist.set_goal_state(configuration_name='l_arm_waist_init_pose') + plan_and_execute( + self.sciurus17, + self.l_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + self.r_arm_waist.set_start_state_to_current_state() + self.r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + self.sciurus17, + self.r_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + joint_values = [ + INITIAL_YAW_ANGLE, + INITIAL_PITCH_ANGLE, + ] + robot_state = RobotState(self.robot_model) + robot_state.set_joint_group_positions('neck_group', joint_values) + self.neck.set_start_state_to_current_state() + self.neck.set_goal_state(robot_state=robot_state) + plan_and_execute( + self.sciurus17, + self.neck, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + def init_arm(self, current_arm): + if current_arm == ArmSide.LEFT: + self.l_arm_waist.set_start_state_to_current_state() + self.l_arm_waist.set_goal_state(configuration_name='l_arm_waist_init_pose') + plan_and_execute( + self.sciurus17, + self.l_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + if current_arm == ArmSide.RIGHT: + self.r_arm_waist.set_start_state_to_current_state() + self.r_arm_waist.set_goal_state(configuration_name='r_arm_waist_init_pose') + plan_and_execute( + self.sciurus17, + self.r_arm_waist, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + +def main(args=None): + rclpy.init(args=args) + + pick_and_place_tf_node = PickAndPlaceTf() + + executor = MultiThreadedExecutor() + executor.add_node(pick_and_place_tf_node) + executor.spin() + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + pick_and_place_tf_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 5ba9b4f4..50feab65 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -33,6 +33,8 @@ 'pick_and_place_right_arm_waist = \ sciurus17_examples_py.pick_and_place_right_arm_waist:main', 'pick_and_place_left_arm = sciurus17_examples_py.pick_and_place_left_arm:main', + 'pick_and_place_tf = sciurus17_examples_py.pick_and_place_tf:main', + 'color_detection = sciurus17_examples_py.color_detection:main', ], }, )