From c22cb7a04b221cec48ce305f7c4e71b85e757afa Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Fri, 19 Jun 2026 17:45:59 +0900 Subject: [PATCH 01/24] =?UTF-8?q?[WIP]color=5Fdetection=E3=82=B3=E3=83=B3?= =?UTF-8?q?=E3=83=9D=E3=83=BC=E3=83=8D=E3=83=B3=E3=83=88=E3=82=92color=5Fd?= =?UTF-8?q?etection2=E3=81=AB=E3=83=AA=E3=83=8D=E3=83=BC=E3=83=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 6 +++--- .../{color_detection.hpp => color_detection_2d.hpp} | 10 +++++----- .../launch/chest_camera_tracking.launch.py | 4 ++-- .../launch/head_camera_tracking.launch.py | 4 ++-- .../{color_detection.cpp => color_detection_2d.cpp} | 12 ++++++------ 5 files changed, 18 insertions(+), 18 deletions(-) rename sciurus17_examples/include/sciurus17_examples/{color_detection.hpp => color_detection_2d.hpp} (81%) rename sciurus17_examples/src/{color_detection.cpp => color_detection_2d.cpp} (93%) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 4c60ea06..2ae79bc4 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -34,7 +34,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 @@ -56,13 +56,13 @@ foreach(loop_var IN LISTS components_list) 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 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..21edd264 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/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py index 06f6ab6e..79e7607f 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/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index bf7bca29..711ee4d4 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/src/color_detection.cpp b/sciurus17_examples/src/color_detection_2d.cpp similarity index 93% rename from sciurus17_examples/src/color_detection.cpp rename to sciurus17_examples/src/color_detection_2d.cpp index 71da591c..be997564 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection_2d.cpp @@ -16,7 +16,7 @@ // 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 "sciurus17_examples/color_detection_2d.hpp" #include "rclcpp/rclcpp.hpp" #include "geometry_msgs/msg/point_stamped.hpp" @@ -28,11 +28,11 @@ using std::placeholders::_1; namespace sciurus17_examples { -ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) -: Node("color_detection", options) +ColorDetection2d::ColorDetection2d(const rclcpp::NodeOptions & options) +: Node("color_detection_2d", options) { image_subscription_ = this->create_subscription( - "/image_raw", 10, std::bind(&ColorDetection::image_callback, this, _1)); + "/image_raw", 10, std::bind(&ColorDetection2d::image_callback, this, _1)); image_annotated_publisher_ = this->create_publisher("image_annotated", 10); @@ -41,7 +41,7 @@ ColorDetection::ColorDetection(const rclcpp::NodeOptions & options) this->create_publisher("target_position", 10); } -void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +void ColorDetection2d::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { // オレンジ色の物体を検出するようにHSVの範囲を設定 const int LOW_H = 5, HIGH_H = 20; @@ -145,4 +145,4 @@ void ColorDetection::image_callback(const sensor_msgs::msg::Image::SharedPtr msg } // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection) +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection2d) From 56840ca24d3a741ff39b7d5be38a9b50e9408150 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Fri, 19 Jun 2026 18:51:18 +0900 Subject: [PATCH 02/24] =?UTF-8?q?[WIP]color=5Fdetection=E3=81=AE=E5=AE=9F?= =?UTF-8?q?=E8=A3=85?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 10 ++ sciurus17_examples/package.xml | 3 + sciurus17_examples/src/color_detection.cpp | 190 +++++++++++++++++++++ 3 files changed, 203 insertions(+) create mode 100644 sciurus17_examples/src/color_detection.cpp diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index 2ae79bc4..bab6cc91 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) @@ -51,8 +54,11 @@ foreach(loop_var IN LISTS components_list) cv_bridge geometry_msgs image_geometry + image_transport + message_filters OpenCV rclcpp + tf2_ros control_msgs) endforeach() @@ -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/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 new file mode 100644 index 00000000..5b7da0c8 --- /dev/null +++ b/sciurus17_examples/src/color_detection.cpp @@ -0,0 +1,190 @@ +// 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.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; + +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); + } + +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::toCvShare(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 = 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_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); + + // カメラから把持対象物の表面までの距離 + 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 = 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; + 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); + } +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} From d4305e2a2982a55577a4dbf48d1c720c9129cecf Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 11:34:52 +0900 Subject: [PATCH 03/24] =?UTF-8?q?launch=E3=81=AE=E3=83=A1=E3=83=83?= =?UTF-8?q?=E3=82=BB=E3=83=BC=E3=82=B8=E3=82=84README.md=E3=82=92=E6=9B=B4?= =?UTF-8?q?=E6=96=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 20 +++++++++++++++++++ .../launch/camera_example.launch.py | 5 ++++- sciurus17_examples/launch/example.launch.py | 4 ++-- 3 files changed, 26 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 78c9f3ee..fea6ee14 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,6 +290,25 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py --- +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +- デフォルトでは青い物体の位置をtfのフレームとして配信されます。 +- tfの`frame_id`は`target_0`です。 +- 色の検出にはOpenCVを使用しています。 +- 検出した物体の距離は深度画像から取得します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples camera_example.launch.py example:='color_detection' +``` + +[Back to example list](#examples) + +--- + ### point_cloud_detection 点群から物体を検出して掴むコード例です。 diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 68cb1a79..414371e9 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/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', From a8c40c2ff26d8e51854993e20abc4e0d116c2bae Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 12:21:08 +0900 Subject: [PATCH 04/24] =?UTF-8?q?Python=E7=89=88=E3=82=92crane=5Fx7=5Fros?= =?UTF-8?q?=E3=81=8B=E3=82=89=E7=A7=BB=E6=A4=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/color_detection.py | 144 ++++++++++++++++++ sciurus17_examples_py/setup.py | 1 + 2 files changed, 145 insertions(+) create mode 100644 sciurus17_examples_py/sciurus17_examples_py/color_detection.py 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..ed458c4d --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -0,0 +1,144 @@ +# 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, '/camera/color/image_raw') + self.depth_sub = message_filters.Subscriber( + self, Image, '/camera/aligned_depth_to_color/image_raw' + ) + self.info_sub = message_filters.Subscriber(self, CameraInfo, '/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, '/color_detection/thresholded_image', 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_BGR2HSV) + + # 画像の二値化 + 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 = dm10 / d_area + pixel_y = dm01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象の位置を取得（2D） + rect_point = camera_model.rectifyPoint(point) + + # カメラ座標系から見た把持対象の方向（Razy）を取得する + ray = camera_model.projetctPixelTo3DRay(rect_point) + + # 把持対象までの距離を取得 + # 把持対象の表面より少し奥を掴むように設定 + DEPTH_OFFSET = 0.015 + cv_depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding=depth_msg.encoding) + + # カメラから把持対象の表面までの距離 + from_distance = cv_depth[int(point[1]), int(point[0])] / 1000.0 + 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] + 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() \ No newline at end of file diff --git a/sciurus17_examples_py/setup.py b/sciurus17_examples_py/setup.py index 5ba9b4f4..bc0309a8 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -33,6 +33,7 @@ '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', + 'color_detection = sciurus17_examples_py.color_detection:main', ], }, ) From 605284a74555d2bbc4627bc153feebfec92674a8 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 12:39:25 +0900 Subject: [PATCH 05/24] =?UTF-8?q?=E3=83=88=E3=83=94=E3=83=83=E3=82=AF?= =?UTF-8?q?=E3=82=92S17=E3=81=AB=E5=90=91=E3=81=91=E3=81=9F=E3=82=82?= =?UTF-8?q?=E3=81=AE=E3=81=AB=E3=81=97=E3=81=A6=E6=A7=8B=E6=96=87=E3=83=9F?= =?UTF-8?q?=E3=82=B9=E3=82=82=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/package.xml | 6 +++ .../sciurus17_examples_py/color_detection.py | 50 ++++++++++--------- 2 files changed, 32 insertions(+), 24 deletions(-) diff --git a/sciurus17_examples_py/package.xml b/sciurus17_examples_py/package.xml index e3f630d1..34fa3233 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -11,8 +11,14 @@ rclpy std_msgs + cv_bridge + geometry_msgs + image_geometry + message_filters moveit moveit_py + 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 index ed458c4d..56c33fc9 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -26,18 +26,20 @@ class ImageSubscriber(Node): def __init__(self): super().__init__('color_detection') - self.color_sub = message_filters.Subscriber(self, Image, '/camera/color/image_raw') + self.color_sub = message_filters.Subscriber(self, Image, '/head_camera/color/image_raw') self.depth_sub = message_filters.Subscriber( - self, Image, '/camera/aligned_depth_to_color/image_raw' + self, Image, '/head_camera/aligned_depth_to_color/image_raw' + ) + self.info_sub = message_filters.Subscriber( + self, CameraInfo, '/head_camera/color/camera_info' ) - self.info_sub = message_filters.Subscriber(self, CameraInfo, '/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, '/color_detection/thresholded_image', 10) + self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded', 10) self.tf_broadcaster = TransformBroadcaster(self) self.bridge = CvBridge() @@ -45,15 +47,15 @@ def __init__(self): def sync_callback(self, color_msg, depth_msg, info_msg): # 青い物体を検出するようにHSVの範囲を設定 # 周囲の明るさ等の動作環境に合わせて調整 - LOW_H, HIGH_H = 100, 125, - LOW_S, HIGH_S = 100, 255, + 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_BGR2HSV) + 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)) @@ -65,7 +67,7 @@ def sync_callback(self, color_msg, depth_msg, info_msg): # 穴埋めの処理 img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) - # 画像の検出領域のおけるモーメントを計算 + # 画像の検出領域におけるモーメントを計算 moment = cv2.moments(img_thresholded) d_m01 = moment['m01'] d_m10 = moment['m10'] @@ -74,31 +76,31 @@ def sync_callback(self, color_msg, depth_msg, info_msg): # 検出した猟奇のピクセル数が10000より大きい場合に把持位置を配信 if d_area <= 10000: return - + # カメラモデル作成 camera_model = PinholeCameraModel() # カメラのパラメータを設定 camera_model.fromCameraInfo(info_msg) - # 画像座標系における把持対象の位置（2D） - pixel_x = dm10 / d_area - pixel_y = dm01 / d_area + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area point = (pixel_x, pixel_y) - # 補正後の画像座標系における把持対象の位置を取得（2D） + # 補正後の画像座標系における把持対象物の位置を取得（2D） rect_point = camera_model.rectifyPoint(point) - # カメラ座標系から見た把持対象の方向（Razy）を取得する - ray = camera_model.projetctPixelTo3DRay(rect_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) - # カメラから把持対象の表面までの距離 - from_distance = cv_depth[int(point[1]), int(point[0])] / 1000.0 + # カメラから把持対象物の表面までの距離 + front_distance = cv_depth[int(point[1]), int(point[0])] / 1000.0 center_distance = front_distance + DEPTH_OFFSET # 距離を取得できないか遠すぎる場合は把持しない @@ -107,15 +109,15 @@ def sync_callback(self, color_msg, depth_msg, info_msg): 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 + ray[2] * center_distance, ] - # 把持対象の位置をTFに配信 + # 把持対象物の位置をTFに配信 t = TransformStamped() t.header = color_msg.header t.child_frame_id = 'target_0' @@ -141,4 +143,4 @@ def main(args=None): if __name__ == '__main__': - main() \ No newline at end of file + main() From 3802a104a62279fb26cb81a7060dd16c69475c4d Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 16:19:47 +0900 Subject: [PATCH 06/24] =?UTF-8?q?pick=5Fand=5Fplace=5Ftf.py=20=E3=81=A8=20?= =?UTF-8?q?camera=E7=94=A8=E3=81=AEluanch=E3=81=AE=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../launch/camera_example.launch.py | 92 ++++ sciurus17_examples_py/package.xml | 3 + .../pick_and_place_tf.py | 430 ++++++++++++++++++ sciurus17_examples_py/setup.py | 1 + 4 files changed, 526 insertions(+) create mode 100644 sciurus17_examples_py/launch/camera_example.launch.py create mode 100644 sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py 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 34fa3233..fbcfbd54 100644 --- a/sciurus17_examples_py/package.xml +++ b/sciurus17_examples_py/package.xml @@ -17,6 +17,9 @@ message_filters moveit moveit_py + python3-numpy + python3-opencv + python3-scipy sensor_msgs tf2_ros 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..a25b3713 --- /dev/null +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py @@ -0,0 +1,430 @@ +# 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.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor +from rclpy.node import Node +from sensor_msgs.msg import JointState +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() + # グリッパ制御の開始状態を補正するため、joint_statesを別コールバックで購読する + self.joint_positions = {} + self.joint_states_callback_group = ReentrantCallbackGroup() + self.create_subscription( + JointState, + 'joint_states', + self.joint_states_callback, + 10, + callback_group=self.joint_states_callback_group, + ) + + # 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.l_gripper_lower_limit, self.l_gripper_upper_limit = self._get_joint_position_bounds( + 'l_gripper_group' + ) + self.r_gripper_lower_limit, self.r_gripper_upper_limit = self._get_joint_position_bounds( + 'r_gripper_group' + ) + + 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 joint_states_callback(self, msg): + # グリッパ制御の開始状態を補正するため、現在の関節角度を保持する + for name, position in zip(msg.name, msg.position): + self.joint_positions[name] = position + + def _get_joint_position_bounds(self, group_name): + joint_model_group = self.robot_model.get_joint_model_group(group_name) + variable_bounds = joint_model_group.active_joint_model_bounds[0] + if isinstance(variable_bounds, (list, tuple)): + variable_bounds = variable_bounds[0] + return variable_bounds.min_position, variable_bounds.max_position + + 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.04 + 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: + # MoveItPyが範囲外の開始状態で失敗しないよう、左グリッパの現在角度を範囲内に丸める + start_angle = self.joint_positions.get('l_gripper_joint', 0.0) + start_angle = min( + max(start_angle, self.l_gripper_lower_limit), + self.l_gripper_upper_limit, + ) + start_state = RobotState(self.robot_model) + start_state.set_joint_group_positions('l_gripper_group', [start_angle]) + self.l_gripper.set_start_state(robot_state=start_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: + # MoveItPyが範囲外の開始状態で失敗しないよう、右グリッパの現在角度を範囲内に丸める + start_angle = self.joint_positions.get('r_gripper_joint', 0.0) + start_angle = min( + max(start_angle, self.r_gripper_lower_limit), + self.r_gripper_upper_limit, + ) + start_state = RobotState(self.robot_model) + start_state.set_joint_group_positions('r_gripper_group', [start_angle]) + self.r_gripper.set_start_state(robot_state=start_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 bc0309a8..50feab65 100644 --- a/sciurus17_examples_py/setup.py +++ b/sciurus17_examples_py/setup.py @@ -33,6 +33,7 @@ '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', ], }, From f8ed402eed99de62a5734fa872fa1cfc8138b843 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 16:23:48 +0900 Subject: [PATCH 07/24] =?UTF-8?q?README=E8=BF=BD=E8=A8=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/README.md | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 0612f2c7..d936373d 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のフレームとして配信されます。 +- tfの`frame_id`は`target_0`です。 +- 色の検出にはOpenCVを使用しています。 +- 検出した物体の距離は深度画像から取得します。 + +次のコマンドを実行します。 + +```sh +ros2 launch sciurus17_examples_py camera_example.launch.py example:='color_detection' +``` + +[Back to example list](#examples) + +--- From 4cdf77cdba2f36cb1336effac89650b6754f7a63 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Mon, 22 Jun 2026 17:47:59 +0900 Subject: [PATCH 08/24] =?UTF-8?q?typo=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- sciurus17_examples_py/sciurus17_examples_py/color_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py index 56c33fc9..4a853f67 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -73,7 +73,7 @@ def sync_callback(self, color_msg, depth_msg, info_msg): d_m10 = moment['m10'] d_area = moment['m00'] - # 検出した猟奇のピクセル数が10000より大きい場合に把持位置を配信 + # 検出した領域のピクセル数が10000より大きい場合に把持位置を配信 if d_area <= 10000: return From 57f00f94abe7641f23ac2f79a52a16882f020739 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Mon, 22 Jun 2026 17:50:12 +0900 Subject: [PATCH 09/24] =?UTF-8?q?typo=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- sciurus17_examples/launch/camera_example.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/launch/camera_example.launch.py b/sciurus17_examples/launch/camera_example.launch.py index 414371e9..23422cb6 100644 --- a/sciurus17_examples/launch/camera_example.launch.py +++ b/sciurus17_examples/launch/camera_example.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): default_value='point_cloud_detection', description=( 'Set an example executable name: ' - '[color_detection, point_cloud_detection,]' + '[color_detection, point_cloud_detection]' ), ) From 30778b56072def4796546a82ee668644fc1b6c52 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 18:42:06 +0900 Subject: [PATCH 10/24] =?UTF-8?q?README=E3=81=AEframe=5Fid=E3=81=AB?= =?UTF-8?q?=E9=96=A2=E3=81=99=E3=82=8B=E8=A1=A8=E7=8F=BE=E3=81=AE=E7=B9=BB?= =?UTF-8?q?=E5=AD=90=E6=A0=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index fea6ee14..00676205 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -294,8 +294,8 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py 特定の色の物体を検出して掴むコード例です。 -- デフォルトでは青い物体の位置をtfのフレームとして配信されます。 -- tfの`frame_id`は`target_0`です。 +- デフォルトでは青い物体の位置をtfのフレームとして配信します。 +- `target_0`は、カメラ画像の`frame_id`を親フレームとするtfフレームとして配信します。 - 色の検出にはOpenCVを使用しています。 - 検出した物体の距離は深度画像から取得します。 @@ -314,7 +314,7 @@ ros2 launch sciurus17_examples camera_example.launch.py example:='color_detectio 点群から物体を検出して掴むコード例です。 - 検出された物体位置はtfのフレームとして配信されます。 -- tfの`frame_id`は検出された順に`target_0`、`target_1`、`target_2`…に設定されます。 +- `target_0`、`target_1`、`target_2`…は、`base_link`を親フレームとするtfフレームとして配信されます。 - 掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。 - 物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 From 8f9732398411f9344402f094529860ba1baad331 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 22 Jun 2026 18:43:14 +0900 Subject: [PATCH 11/24] =?UTF-8?q?Python=E7=89=88=E3=81=AEREADME=E3=81=AEfr?= =?UTF-8?q?ame=5Fid=E3=81=AE=E8=A1=A8=E7=8F=BE=E3=82=82=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples_py/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index d936373d..3b3a7642 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -166,8 +166,8 @@ ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_lef 特定の色の物体を検出して掴むコード例です。 -- デフォルトでは青い物体の位置をtfのフレームとして配信されます。 -- tfの`frame_id`は`target_0`です。 +- デフォルトでは青い物体の位置をtfのフレームとして配信します。 +- `target_0`は、カメラ画像の`frame_id`を親フレームとするtfフレームとして配信します。 - 色の検出にはOpenCVを使用しています。 - 検出した物体の距離は深度画像から取得します。 From 452ea5e75051098f02de2197b8af0f3684008063 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 11:08:07 +0900 Subject: [PATCH 12/24] =?UTF-8?q?=E7=8F=BE=E7=8A=B6=E3=81=A0=E3=81=A8?= =?UTF-8?q?=E9=AB=98=E3=81=99=E3=81=8E=E3=81=A6=E5=AF=BE=E8=B1=A1=E7=AF=84?= =?UTF-8?q?=E5=9B=B2=E3=81=AB=E5=85=A5=E3=82=89=E3=81=AA=E3=81=84=E3=81=9F?= =?UTF-8?q?=E3=82=81=E3=80=81=E6=8A=8A=E6=8C=81=E5=AF=BE=E8=B1=A1=E3=81=AE?= =?UTF-8?q?=E7=AF=84=E5=9B=B2=E3=82=920.04->0.02mm=E3=81=AB=E7=B7=A9?= =?UTF-8?q?=E5=92=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/pick_and_place_tf.cpp | 2 +- .../sciurus17_examples_py/pick_and_place_tf.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/pick_and_place_tf.cpp b/sciurus17_examples/src/pick_and_place_tf.cpp index 7458716d..891768f3 100644 --- a/sciurus17_examples/src/pick_and_place_tf.cpp +++ b/sciurus17_examples/src/pick_and_place_tf.cpp @@ -129,7 +129,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/sciurus17_examples_py/pick_and_place_tf.py b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py index a25b3713..fe1dfe99 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py @@ -146,7 +146,7 @@ def on_timer(self): FILTERING_TIME = rclpy.duration.Duration(seconds=2) STOP_TIME_THRESHOLD = rclpy.duration.Duration(seconds=3) DISTANCE_THRESHOLD = 0.01 - TARGET_Z_MIN_LIMIT = 0.04 + TARGET_Z_MIN_LIMIT = 0.02 TARGET_X_MIN_LIMIT = 0.13 TARGET_X_MAX_LIMIT = 0.3 # 経過時間と停止時間を計算(nsec) From 6dc58832a5e7df92e77e80cfafe5d7359d89c5e3 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 11:14:27 +0900 Subject: [PATCH 13/24] =?UTF-8?q?Realsense=E5=AE=9F=E6=A9=9F=E3=81=A8Gazeb?= =?UTF-8?q?o=E4=B8=8A=E3=81=A7=E3=81=AE=E3=82=A8=E3=83=B3=E3=82=B3?= =?UTF-8?q?=E3=83=BC=E3=83=87=E3=82=A3=E3=83=B3=E3=82=B0=E3=81=AE=E9=81=95?= =?UTF-8?q?=E3=81=84=E3=82=92=E5=90=B8=E5=8F=8E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 15 ++++++++++++++- .../sciurus17_examples_py/color_detection.py | 10 +++++++++- 2 files changed, 23 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 5b7da0c8..45d015d4 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -148,7 +148,20 @@ class ImageSubscriber : public rclcpp::Node const auto cv_depth = cv_bridge::toCvShare(depth_msg, depth_msg->encoding); // カメラから把持対象物の表面までの距離 - const auto front_distance = cv_depth->image.at(point) / 1000.0; + double front_distance = 0.0; + + if (depth_msg->encoding == "16UC1") { + front_distance = cv_depth->image.at(point) / 1000.0; + } else if (depth_msg->encoding == "32FC1") { + front_distance = cv_depth->image.at(point); + } else { + RCLCPP_WARN( + this->get_logger(), + "Unsupported depth encoding: %s", + depth_msg->encoding.c_str()); + return; + } + const auto center_distance = front_distance + DEPTH_OFFSET; // 距離を取得できないか遠すぎる場合は把持しない diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py index 4a853f67..3c59d2e6 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -100,7 +100,15 @@ def sync_callback(self, color_msg, depth_msg, info_msg): cv_depth = self.bridge.imgmsg_to_cv2(depth_msg, desired_encoding=depth_msg.encoding) # カメラから把持対象物の表面までの距離 - front_distance = cv_depth[int(point[1]), int(point[0])] / 1000.0 + depth = cv_depth[int(point[1]), int(point[0])] + if depth_msg.encoding == '16UC1': + front_distance = float(depth) / 1000.0 + elif depth_msg.encoding == '32FC1': + front_distance = float(depth) + else: + self.get_logger().warn(f'Unsupported depth encoding: {depth_msg.encoding}') + return + center_distance = front_distance + DEPTH_OFFSET # 距離を取得できないか遠すぎる場合は把持しない From 9382fa6289b21ee82d2bc268d3e1c0b995f6e348 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 11:36:57 +0900 Subject: [PATCH 14/24] =?UTF-8?q?coments()=E3=82=92cv::moments()=E3=81=B8?= =?UTF-8?q?=E3=81=A8=E5=90=8D=E5=89=8D=E7=A9=BA=E9=96=93=E3=81=AE=E6=98=8E?= =?UTF-8?q?=E7=A4=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 45d015d4..f0ee1817 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -115,7 +115,7 @@ class ImageSubscriber : public rclcpp::Node cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5))); // 画像の検出領域におけるモーメントを計算 - cv::Moments moment = moments(img_thresholded); + cv::Moments moment = cv::moments(img_thresholded); double d_m01 = moment.m01; double d_m10 = moment.m10; double d_area = moment.m00; From aacd260cccbe44a3a70ce59a3d4ed6acd4a91271 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 12:12:09 +0900 Subject: [PATCH 15/24] =?UTF-8?q?=E6=9A=97=E9=BB=99=E7=9A=84=E3=81=AA?= =?UTF-8?q?=E5=9E=8B=E5=A4=89=E6=8F=9B=E3=82=92=E6=98=8E=E7=A4=BA=E7=9A=84?= =?UTF-8?q?=E3=81=AA=E3=82=AD=E3=83=A3=E3=82=B9=E3=83=88=E3=81=AB=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index f0ee1817..0089a247 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -149,11 +149,14 @@ class ImageSubscriber : public rclcpp::Node // カメラから把持対象物の表面までの距離 double front_distance = 0.0; + // 深度画像アクセス用に小数座標を整数インデックスに変換 + const cv::Point point_int(static_cast(point.x), static_cast(point.y)); + // エンコーディングの違いによる深度値の取得方法の違いに対応 if (depth_msg->encoding == "16UC1") { - front_distance = cv_depth->image.at(point) / 1000.0; + front_distance = cv_depth->image.at(point_int) / 1000.0; } else if (depth_msg->encoding == "32FC1") { - front_distance = cv_depth->image.at(point); + front_distance = cv_depth->image.at(point_int); } else { RCLCPP_WARN( this->get_logger(), From fc91b8cdfe9dcbbb62d74b40923a0f654bb13aa2 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 23 Jun 2026 12:13:51 +0900 Subject: [PATCH 16/24] =?UTF-8?q?=E3=82=AF=E3=82=A9=E3=83=BC=E3=82=BF?= =?UTF-8?q?=E3=83=8B=E3=82=AA=E3=83=B3=E3=81=AE=E3=83=87=E3=83=95=E3=82=A9?= =?UTF-8?q?=E3=83=AB=E3=83=88=E3=81=AB=E6=81=92=E7=AD=89=E5=9B=9E=E8=BB=A2?= =?UTF-8?q?=EF=BC=88w=3D1.0=EF=BC=89=E3=82=92=E6=98=8E=E7=A4=BA=E3=80=82Po?= =?UTF-8?q?tential=20fix=20for=20pull=20request=20finding?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- sciurus17_examples/src/color_detection.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 0089a247..452bbc37 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -188,6 +188,10 @@ class ImageSubscriber : public rclcpp::Node 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); // 閾値による二値化画像を配信 From cda1781e31d1d8a8f55e89a9d67273023f6b6717 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 23 Jun 2026 13:29:32 +0900 Subject: [PATCH 17/24] =?UTF-8?q?HSV=E5=A4=89=E6=8F=9B=E6=99=82=E3=81=AB?= =?UTF-8?q?=E5=85=B1=E6=9C=89=E3=81=95=E3=82=8C=E3=81=9FROS=E7=94=BB?= =?UTF-8?q?=E5=83=8F=E3=83=90=E3=83=83=E3=83=95=E3=82=A1=E3=82=92=E6=9B=B8?= =?UTF-8?q?=E3=81=8D=E6=8F=9B=E3=81=88=E3=81=AA=E3=81=84=E3=82=88=E3=81=86?= =?UTF-8?q?=E3=81=AB=E3=81=99=E3=82=8B=20Potential=20fix=20for=20pull=20re?= =?UTF-8?q?quest=20finding?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- sciurus17_examples/src/color_detection.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index 452bbc37..a383455b 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -85,11 +85,10 @@ class ImageSubscriber : public rclcpp::Node const int LOW_S = 100, HIGH_S = 255; const int LOW_V = 30, HIGH_V = 255; - auto cv_color = cv_bridge::toCvShare(color_msg, color_msg->encoding); + 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; From 3629acbd6e443a08ad6b0cd8613f048f6ced7eeb Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 23 Jun 2026 13:42:39 +0900 Subject: [PATCH 18/24] Potential fix for pull request finding Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- sciurus17_examples_py/sciurus17_examples_py/color_detection.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py index 3c59d2e6..14f5b87d 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -53,7 +53,7 @@ def sync_callback(self, color_msg, depth_msg, info_msg): # カメラ画像を受け取る 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) From 3ce79a153b3cee70456a74c44c4ecb14f70d41e2 Mon Sep 17 00:00:00 2001 From: Kazushi Kurasawa Date: Tue, 23 Jun 2026 13:45:59 +0900 Subject: [PATCH 19/24] =?UTF-8?q?Python=E7=89=88=E3=81=AB=E3=82=82?= =?UTF-8?q?=E3=82=AF=E3=82=A9=E3=83=BC=E3=82=BF=E3=83=8B=E3=82=AA=E3=83=B3?= =?UTF-8?q?=E3=81=AE=E6=81=92=E5=B8=B8=E5=A7=BF=E5=8B=A2=E3=81=AE=E3=83=87?= =?UTF-8?q?=E3=83=95=E3=82=A9=E3=83=AB=E3=83=88=E5=80=A4=E3=82=92=E8=BF=BD?= =?UTF-8?q?=E5=8A=A0=20Potential=20fix=20for=20pull=20request=20finding?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Copilot Autofix powered by AI <175728472+Copilot@users.noreply.github.com> --- .../sciurus17_examples_py/color_detection.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py index 14f5b87d..249fe2f1 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -132,6 +132,10 @@ def sync_callback(self, color_msg, depth_msg, info_msg): 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) # 閾値による二値化画像を配信 From 8399996d6bb6cab18bfb60c357ce0cf4b2303f6d Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 14:00:08 +0900 Subject: [PATCH 20/24] =?UTF-8?q?CI=E3=81=AB=E5=BE=93=E3=81=A3=E3=81=A6?= =?UTF-8?q?=E3=82=B3=E3=83=BC=E3=83=89=E3=83=95=E3=82=A9=E3=83=BC=E3=83=9E?= =?UTF-8?q?=E3=83=83=E3=83=88=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../sciurus17_examples_py/pick_and_place_tf.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index fe1dfe99..92a6fc94 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py @@ -29,9 +29,9 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.executors import MultiThreadedExecutor from rclpy.node import Node -from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation from sciurus17_examples_py.utils import plan_and_execute +from sensor_msgs.msg import JointState from tf2_ros import TransformException, TransformListener, TransformStamped from tf2_ros.buffer import Buffer From a7a77b298f2497aaac1e89484834d173823e6b9e Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Tue, 23 Jun 2026 14:18:10 +0900 Subject: [PATCH 21/24] =?UTF-8?q?CollorDetection2D=E3=81=AE=E3=82=AF?= =?UTF-8?q?=E3=83=A9=E3=82=B9=E5=90=8D=E3=81=AE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/CMakeLists.txt | 2 +- .../include/sciurus17_examples/color_detection_2d.hpp | 4 ++-- sciurus17_examples/launch/chest_camera_tracking.launch.py | 2 +- sciurus17_examples/launch/head_camera_tracking.launch.py | 2 +- sciurus17_examples/src/color_detection_2d.cpp | 8 ++++---- 5 files changed, 9 insertions(+), 9 deletions(-) diff --git a/sciurus17_examples/CMakeLists.txt b/sciurus17_examples/CMakeLists.txt index bab6cc91..0d14b67b 100644 --- a/sciurus17_examples/CMakeLists.txt +++ b/sciurus17_examples/CMakeLists.txt @@ -62,7 +62,7 @@ foreach(loop_var IN LISTS components_list) control_msgs) endforeach() -rclcpp_components_register_nodes(color_detection_2d "sciurus17_examples::ColorDetection2d") +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") diff --git a/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp b/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp index 21edd264..a669fc07 100644 --- a/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp +++ b/sciurus17_examples/include/sciurus17_examples/color_detection_2d.hpp @@ -23,10 +23,10 @@ namespace sciurus17_examples { -class ColorDetection2d : public rclcpp::Node +class ColorDetection2D : public rclcpp::Node { public: - explicit ColorDetection2d(const rclcpp::NodeOptions & options); + explicit ColorDetection2D(const rclcpp::NodeOptions & options); private: rclcpp::Subscription::SharedPtr image_subscription_; diff --git a/sciurus17_examples/launch/chest_camera_tracking.launch.py b/sciurus17_examples/launch/chest_camera_tracking.launch.py index 79e7607f..54e40d62 100644 --- a/sciurus17_examples/launch/chest_camera_tracking.launch.py +++ b/sciurus17_examples/launch/chest_camera_tracking.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): name='color_detection_2d', namespace='chest_camera_tracking', package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection2d', + plugin='sciurus17_examples::ColorDetection2D', remappings=[('/image_raw', '/chest_camera/image_raw')], extra_arguments=[{'use_intra_process_comms': True}], ), diff --git a/sciurus17_examples/launch/head_camera_tracking.launch.py b/sciurus17_examples/launch/head_camera_tracking.launch.py index 711ee4d4..f66675d1 100644 --- a/sciurus17_examples/launch/head_camera_tracking.launch.py +++ b/sciurus17_examples/launch/head_camera_tracking.launch.py @@ -38,7 +38,7 @@ def generate_launch_description(): name='color_detection_2d', namespace='head_camera_tracking', package='sciurus17_examples', - plugin='sciurus17_examples::ColorDetection2d', + plugin='sciurus17_examples::ColorDetection2D', remappings=[('/image_raw', '/head_camera/color/image_raw')], extra_arguments=[{'use_intra_process_comms': True}], ), diff --git a/sciurus17_examples/src/color_detection_2d.cpp b/sciurus17_examples/src/color_detection_2d.cpp index be997564..a3a0c9ce 100644 --- a/sciurus17_examples/src/color_detection_2d.cpp +++ b/sciurus17_examples/src/color_detection_2d.cpp @@ -28,11 +28,11 @@ using std::placeholders::_1; namespace sciurus17_examples { -ColorDetection2d::ColorDetection2d(const rclcpp::NodeOptions & options) +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_raw", 10, std::bind(&ColorDetection2D::image_callback, this, _1)); image_annotated_publisher_ = this->create_publisher("image_annotated", 10); @@ -41,7 +41,7 @@ ColorDetection2d::ColorDetection2d(const rclcpp::NodeOptions & options) this->create_publisher("target_position", 10); } -void ColorDetection2d::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) +void ColorDetection2D::image_callback(const sensor_msgs::msg::Image::SharedPtr msg) { // オレンジ色の物体を検出するようにHSVの範囲を設定 const int LOW_H = 5, HIGH_H = 20; @@ -145,4 +145,4 @@ void ColorDetection2d::image_callback(const sensor_msgs::msg::Image::SharedPtr m } // namespace sciurus17_examples #include "rclcpp_components/register_node_macro.hpp" -RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection2d) +RCLCPP_COMPONENTS_REGISTER_NODE(sciurus17_examples::ColorDetection2D) From 14f606848960c8bbc939ef4fd2a6e6cd53262d1a Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Mon, 29 Jun 2026 11:49:45 +0900 Subject: [PATCH 22/24] =?UTF-8?q?README=E5=86=85=E3=81=A7=E3=81=AE?= =?UTF-8?q?=E3=82=B5=E3=83=B3=E3=83=97=E3=83=AB=E8=AA=AC=E6=98=8E=E3=81=AE?= =?UTF-8?q?=E8=A1=A8=E7=8F=BE=E4=BF=AE=E6=AD=A3?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/README.md | 4 ++-- sciurus17_examples_py/README.md | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/sciurus17_examples/README.md b/sciurus17_examples/README.md index 00676205..b07b7d4d 100644 --- a/sciurus17_examples/README.md +++ b/sciurus17_examples/README.md @@ -295,7 +295,7 @@ ros2 launch sciurus17_examples chest_camera_tracking.launch.py 特定の色の物体を検出して掴むコード例です。 - デフォルトでは青い物体の位置をtfのフレームとして配信します。 -- `target_0`は、カメラ画像の`frame_id`を親フレームとするtfフレームとして配信します。 +- 検出した物体のフレーム名は `target_0` です。 - 色の検出にはOpenCVを使用しています。 - 検出した物体の距離は深度画像から取得します。 @@ -314,7 +314,7 @@ ros2 launch sciurus17_examples camera_example.launch.py example:='color_detectio 点群から物体を検出して掴むコード例です。 - 検出された物体位置はtfのフレームとして配信されます。 -- `target_0`、`target_1`、`target_2`…は、`base_link`を親フレームとするtfフレームとして配信されます。 +- 検出した物体のフレーム名は `target_0`、`target_1`、`target_2`…です。 - 掴む対象はSciurus17前方の0.3 mの範囲にある`target_0`に設定されています。 - 物体検出には[Point Cloud Library](https://pointclouds.org/)を使用しています。 diff --git a/sciurus17_examples_py/README.md b/sciurus17_examples_py/README.md index 3b3a7642..a259acf7 100644 --- a/sciurus17_examples_py/README.md +++ b/sciurus17_examples_py/README.md @@ -167,7 +167,7 @@ ros2 launch sciurus17_examples_py example.launch.py example:='pick_and_place_lef 特定の色の物体を検出して掴むコード例です。 - デフォルトでは青い物体の位置をtfのフレームとして配信します。 -- `target_0`は、カメラ画像の`frame_id`を親フレームとするtfフレームとして配信します。 +- 検出した物体のフレーム名は `target_0` です。 - 色の検出にはOpenCVを使用しています。 - 検出した物体の距離は深度画像から取得します。 From 9076ced472e4d65f86311d56b09108407a25ead1 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 1 Jul 2026 10:08:03 +0900 Subject: [PATCH 23/24] =?UTF-8?q?=E6=B7=B1=E5=BA=A6=E3=83=95=E3=82=A9?= =?UTF-8?q?=E3=83=BC=E3=83=9E=E3=83=83=E3=83=88=E3=81=AB=E3=81=A4=E3=81=84?= =?UTF-8?q?=E3=81=A6=E3=81=AE=E3=82=B3=E3=83=A1=E3=83=B3=E3=83=88=E3=82=92?= =?UTF-8?q?=E8=BF=BD=E5=8A=A0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sciurus17_examples/src/color_detection.cpp | 2 ++ sciurus17_examples_py/sciurus17_examples_py/color_detection.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/sciurus17_examples/src/color_detection.cpp b/sciurus17_examples/src/color_detection.cpp index a383455b..791e1797 100644 --- a/sciurus17_examples/src/color_detection.cpp +++ b/sciurus17_examples/src/color_detection.cpp @@ -153,8 +153,10 @@ class ImageSubscriber : public rclcpp::Node // エンコーディングの違いによる深度値の取得方法の違いに対応 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( diff --git a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py index 249fe2f1..ef90ba3e 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/color_detection.py +++ b/sciurus17_examples_py/sciurus17_examples_py/color_detection.py @@ -102,8 +102,10 @@ def sync_callback(self, color_msg, depth_msg, info_msg): # カメラから把持対象物の表面までの距離 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}') From 8dac46ff46684c01cb2e9943eb6d871b03e28644 Mon Sep 17 00:00:00 2001 From: KuraZuzu Date: Wed, 1 Jul 2026 16:02:36 +0900 Subject: [PATCH 24/24] =?UTF-8?q?python=E7=89=88=E3=81=A7joint=5Fvalues?= =?UTF-8?q?=E3=82=92=E5=8F=97=E3=81=91=E5=8F=96=E3=81=A3=E3=81=A6=E7=AF=84?= =?UTF-8?q?=E5=9B=B2=E5=A4=96=E3=81=A7=E3=81=82=E3=82=8C=E3=81=B0=E8=A3=9C?= =?UTF-8?q?=E6=AD=A3=E3=81=99=E3=82=8B=E5=87=A6=E7=90=86=E3=81=AE=E5=89=8A?= =?UTF-8?q?=E9=99=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../pick_and_place_tf.py | 50 +------------------ 1 file changed, 2 insertions(+), 48 deletions(-) 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 index 92a6fc94..933892c6 100644 --- a/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py +++ b/sciurus17_examples_py/sciurus17_examples_py/pick_and_place_tf.py @@ -26,12 +26,10 @@ import numpy as np import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup 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 sensor_msgs.msg import JointState from tf2_ros import TransformException, TransformListener, TransformStamped from tf2_ros.buffer import Buffer @@ -50,16 +48,6 @@ def __init__(self): self.tf_buffer = Buffer() self.tf_listener = TransformListener(self.tf_buffer, self) self.tf_past = TransformStamped() - # グリッパ制御の開始状態を補正するため、joint_statesを別コールバックで購読する - self.joint_positions = {} - self.joint_states_callback_group = ReentrantCallbackGroup() - self.create_subscription( - JointState, - 'joint_states', - self.joint_states_callback, - 10, - callback_group=self.joint_states_callback_group, - ) # instantiate MoveItPy instance and get planning component self.sciurus17 = MoveItPy(node_name='moveit_py') @@ -78,12 +66,6 @@ def __init__(self): # instantiate a RobotState instance using the current robot model self.robot_model = self.sciurus17.get_robot_model() - self.l_gripper_lower_limit, self.l_gripper_upper_limit = self._get_joint_position_bounds( - 'l_gripper_group' - ) - self.r_gripper_lower_limit, self.r_gripper_upper_limit = self._get_joint_position_bounds( - 'r_gripper_group' - ) self.arm_plan_request_params = PlanRequestParameters( self.sciurus17, @@ -122,18 +104,6 @@ def __init__(self): # Call on_timer function every 0.5 second self.timer = self.create_timer(0.5, self.on_timer) - def joint_states_callback(self, msg): - # グリッパ制御の開始状態を補正するため、現在の関節角度を保持する - for name, position in zip(msg.name, msg.position): - self.joint_positions[name] = position - - def _get_joint_position_bounds(self, group_name): - joint_model_group = self.robot_model.get_joint_model_group(group_name) - variable_bounds = joint_model_group.active_joint_model_bounds[0] - if isinstance(variable_bounds, (list, tuple)): - variable_bounds = variable_bounds[0] - return variable_bounds.min_position, variable_bounds.max_position - def on_timer(self): # target_0のtf位置姿勢を取得 try: @@ -277,15 +247,7 @@ def _control_gripper(self, current_arm, angle): robot_state = RobotState(self.robot_model) if current_arm == ArmSide.LEFT: - # MoveItPyが範囲外の開始状態で失敗しないよう、左グリッパの現在角度を範囲内に丸める - start_angle = self.joint_positions.get('l_gripper_joint', 0.0) - start_angle = min( - max(start_angle, self.l_gripper_lower_limit), - self.l_gripper_upper_limit, - ) - start_state = RobotState(self.robot_model) - start_state.set_joint_group_positions('l_gripper_group', [start_angle]) - self.l_gripper.set_start_state(robot_state=start_state) + 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( @@ -295,15 +257,7 @@ def _control_gripper(self, current_arm, angle): single_plan_parameters=self.gripper_plan_request_params, ) if current_arm == ArmSide.RIGHT: - # MoveItPyが範囲外の開始状態で失敗しないよう、右グリッパの現在角度を範囲内に丸める - start_angle = self.joint_positions.get('r_gripper_joint', 0.0) - start_angle = min( - max(start_angle, self.r_gripper_lower_limit), - self.r_gripper_upper_limit, - ) - start_state = RobotState(self.robot_model) - start_state.set_joint_group_positions('r_gripper_group', [start_angle]) - self.r_gripper.set_start_state(robot_state=start_state) + 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(