diff --git a/crane_plus_control/src/crane_plus_hardware.cpp b/crane_plus_control/src/crane_plus_hardware.cpp index b1b9e7f8..6514e000 100644 --- a/crane_plus_control/src/crane_plus_hardware.cpp +++ b/crane_plus_control/src/crane_plus_hardware.cpp @@ -205,7 +205,9 @@ return_type CranePlusHardware::read( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; + // readに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } else { for (uint i = 0; i < hw_position_states_.size(); ++i) { hw_position_states_[i] = joint_positions[i]; @@ -271,7 +273,9 @@ return_type CranePlusHardware::write( RCLCPP_ERROR( rclcpp::get_logger("CranePlusHardware"), driver_->get_last_error_log().c_str()); - return return_type::ERROR; + // writeに失敗しても通信は継続させる。 + // 不確かなデータをセットしないようにOKを返す。 + return return_type::OK; } prev_comm_timestamp_ = steady_clock_.now(); diff --git a/crane_plus_examples_py/LICENSE b/crane_plus_examples_py/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/crane_plus_examples_py/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + 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. diff --git a/crane_plus_examples_py/README.md b/crane_plus_examples_py/README.md new file mode 100644 index 00000000..000e3676 --- /dev/null +++ b/crane_plus_examples_py/README.md @@ -0,0 +1,230 @@ +# crane_plus_examples_py + +このパッケージはCRANE+ V2 ROS 2パッケージのPythonによるサンプルコード集です。 + +## 準備（実機を使う場合） + +![crane_plus](https://rt-net.github.io/images/crane-plus/CRANEV2-500x500.png) + +### 1. CRANE+ V2本体をPCに接続する + +CRANE+ V2本体をPCに接続します。 +接続方法は製品マニュアルを参照してください。 + +**※CRANE+ V2本体が接触しないように、十分なスペースを確保してください。** + +### 2. USB通信ポートの接続を確認する + +USB通信ポートの設定については`crane_plus_control`の +[README](../crane_plus_control/README.md) +を参照してください。 + +**正しく設定できていない場合、CRANE+ V2が動作しない、振動する、などの不安定な動きになるので注意してください** + +### 3. move_groupとcontrollerを起動する + +#### 標準のCRANE+ V2を使用する場合 + +次のコマンドでmove_group (`crane_plus_moveit_config`)と +controller (`crane_plus_control`)を起動します。 + +```sh +$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 +``` + +#### Webカメラ搭載モデルを使用する場合 + +Webカメラ搭載モデルの場合は、次のコマンドを実行してください。 +```video_device```は使用するWebカメラを指定してください。 + +```sh +$ ros2 launch crane_plus_examples demo.launch.py port_name:=/dev/ttyUSB0 use_camera:=true video_device:=/dev/video0 +``` + +## 準備（Gazeboを使う場合） +======= +![crane_plus_ignition](https://rt-net.github.io/images/crane-plus/crane_plus_ignition.png) + +### 1. move_groupとGazeboを起動する + +次のコマンドでmove_group (`crane_plus_moveit_config`)とGazeboを起動します。 + +```sh +$ ros2 launch crane_plus_gazebo crane_plus_with_table.launch.py +``` + +## Pythonのサンプルプログラムを実行する + +準備ができたらPythonによるサンプルプログラムを実行します。 +例えばグリッパを開閉するサンプルは次のコマンドで実行できます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + +終了するときは`Ctrl+c`を入力します。 + +## Gazeboでサンプルプログラムを実行する場合 + +Gazeboでサンプルプログラムを実行する場合は`use_sim_time`オプションを付けます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' use_sim_time:=true +``` + +## Examples + +`demo.launch.py`を実行している状態で各サンプルを実行できます。 + +- [gripper_control](#gripper_control) +- [pose_groupstate](#pose_groupstate) +- [joint_values](#joint_values) +- [pick_and_place](#pick_and_place) + +実行できるサンプルの一覧は、`examples.launch.py`にオプション`-s`を付けて実行することで表示できます。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py -s +Arguments (pass arguments as ':='): + + 'example': + Set an example executable name: [gripper_control, pose_groupstate, joint_values, pick_and_place] + (default: 'gripper_control') +``` + +--- + +### gripper_control + +グリッパを開閉させるコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='gripper_control' +``` + + + +[back to example list](#examples) + +--- + +### pose_groupstate + +group_stateを使うコード例です。 + +SRDFファイル[crane_plus_moveit_config/config/crane_plus.srdf](../crane_plus_moveit_config/config/crane_plus.srdf) +に記載されている`home`と`vertical`の姿勢に移行します。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='pose_groupstate' +``` + + + +[back to example list](#examples) + +--- + +### joint_values + +アームのジョイント角度を１つずつ変更するコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='joint_values' +``` + + + +[back to example list](#examples) + +--- + +### pick_and_place + +モノを掴む・持ち上げる・運ぶ・置くコード例です。 + +次のコマンドを実行します。 + +```sh +$ ros2 launch crane_plus_examples_py example.launch.py example:='pick_and_place' +``` + + + +[back to example list](#examples) + +--- + +## Camera Examples + +Webカメラ搭載モデルのカメラを使用したサンプルコードです。 + +[「Webカメラ搭載モデルを使用する場合」](#Webカメラ搭載モデルを使用する場合)の手順に従って、 +`demo.launch`を実行している状態で、 +各サンプルを実行できます。 + +- [aruco\_detection](#aruco_detection) +- [color\_detection](#color_detection) + +実行できるサンプルの一覧は、`camera_example.launch.py`にオプション`-s`を付けて実行することで確認できます。 + +```sh +$ ros2 launch crane_plus_examples_py camera_example.launch.py -s +Arguments (pass arguments as ':='): + + 'example': + Set an example executable name: [color_detection] + (default: 'color_detection') +``` + +--- + +### aruco_detection + +モノに取り付けたArUcoマーカをカメラで検出し、マーカ位置に合わせて掴むコード例です。 +マーカは[aruco_markers.pdf](./aruco_markers.pdf)をA4紙に印刷して、一辺50mmの立方体に取り付けて使用します。 + +検出されたマーカの位置姿勢はtfのフレームとして配信されます。 +tfの`frame_id`はマーカIDごとに異なりID0のマーカの`frame_id`は`target_0`になります。 +掴む対象は`target_0`に設定されています。 +マーカ検出には[OpenCV](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html)を使用しています。 + +次のコマンドを実行します。 + +```bash +ros2 launch crane_plus_examples_py camera_example.launch.py example:='aruco_detection' +``` + +#### Videos +[![crane_plus_aruco_detection_demo](https://rt-net.github.io/images/crane-plus/aruco_detection.gif)](https://youtu.be/m9dus6LCocc) + +[back to example list](#examples) + +--- + +### color_detection + +特定の色の物体を検出して掴むコード例です。 + +デフォルトでは赤い物体の位置をtfのフレームとして配信します。 +tfの`frame_id`は`target_0`です。 +色検出には[OpenCV](https://docs.opencv.org/4.x/db/d8e/tutorial_threshold.html)を使用しています。 + +次のコマンドを実行します。 + +```sh +ros2 launch crane_plus_examples_py camera_example.launch.py example:='color_detection' +``` + +#### Videos +[![crane_plus_color_detection_demo](https://rt-net.github.io/images/crane-plus/color_detection.gif)](https://youtu.be/Kn0eWA7sALY) + +[back to example list](#examples) + +--- diff --git a/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml new file mode 100644 index 00000000..bf3489ee --- /dev/null +++ b/crane_plus_examples_py/config/crane_plus_moveit_py_examples.yaml @@ -0,0 +1,26 @@ +planning_scene_monitor_options: + name: "planning_scene_monitor" + robot_description: "robot_description" + joint_state_topic: "/joint_states" + attached_collision_object_topic: "/moveit_cpp/planning_scene_monitor" + publish_planning_scene_topic: "/moveit_cpp/publish_planning_scene" + monitored_planning_scene_topic: "/moveit_cpp/monitored_planning_scene" + wait_for_initial_state_timeout: 10.0 + +planning_pipelines: + pipeline_names: ["ompl"] + +plan_request_params: + planning_attempts: 1 + planning_pipeline: ompl + max_velocity_scaling_factor: 1.0 + max_acceleration_scaling_factor: 1.0 + +ompl_rrtc: # Namespace for individual plan request + plan_request_params: # PlanRequestParameters similar to the ones that are used by the single pipeline planning of moveit_cpp + planning_attempts: 1 # Number of attempts the planning pipeline tries to solve a given motion planning problem + planning_pipeline: ompl # Name of the pipeline that is being used + planner_id: "RRTConnectkConfigDefault:" # Name of the specific planner to be used by the pipeline + max_velocity_scaling_factor: 1.0 # Velocity scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + max_acceleration_scaling_factor: 1.0 # Acceleration scaling parameter for the trajectory generation algorithm that is called (if configured) after the path planning + planning_time: 5.0 # Time budget for the motion plan request. If the planning problem cannot be solved within this time, an empty solution with error code is returned diff --git a/crane_plus_examples_py/crane_plus_examples_py/__init__.py b/crane_plus_examples_py/crane_plus_examples_py/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py new file mode 100644 index 00000000..ec4ead83 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -0,0 +1,101 @@ +# Copyright 2025 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 cv2 import aruco +from cv_bridge import CvBridge +from geometry_msgs.msg import TransformStamped +import numpy as np +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation +from sensor_msgs.msg import CameraInfo, Image +import tf2_ros + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('aruco_detection') + self.image_subscription = self.create_subscription( + Image, 'image_raw', self.image_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, 'camera_info', self.camera_info_callback, 10 + ) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + # 画像データをROSのメッセージからOpenCVの配列に変換 + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2BGR) + + if self.camera_info: + # ArUcoマーカのデータセットを読み込む + # DICT_6x6_50は6x6ビットのマーカが50個収録されたもの + MARKER_DICT = aruco.getPredefinedDictionary(aruco.DICT_6X6_50) + # マーカID + ids = [] + # 画像座標系上のマーカ頂点位置 + corners = [] + # マーカの検出 + corners, ids, _ = aruco.detectMarkers(cv_img, MARKER_DICT) + # マーカの検出数 + n_markers = len(ids) + # カメラパラメータ + CAMERA_MATRIX = np.array(self.camera_info['k']).reshape(3, 3) + DIST_COEFFS = np.array(self.camera_info['d']).reshape(1, 5) + # マーカ一辺の長さ 0.04 [m] + MARKER_LENGTH = 0.04 + + # マーカが一つ以上検出された場合、マーカの位置姿勢をtfで配信 + if n_markers > 0: + for i in range(n_markers): + # 画像座標系上のマーカ位置を三次元のカメラ座標系に変換 + rvecs, tvecs, _ = cv2.aruco.estimatePoseSingleMarkers( + corners, MARKER_LENGTH, CAMERA_MATRIX, DIST_COEFFS) + # tfの配信 + t = TransformStamped() + t.header = msg.header + t.child_frame_id = 'target_' + str(ids[i][0]) + t.transform.translation.x = tvecs[i][0][0] + t.transform.translation.y = tvecs[i][0][1] + t.transform.translation.z = tvecs[i][0][2] + # 回転ベクトルをクォータニオンに変換 + marker_orientation_rot = Rotation.from_rotvec(rvecs) + marker_orientation_quat = marker_orientation_rot.as_quat() + t.transform.rotation.x = marker_orientation_quat[0] + t.transform.rotation.y = marker_orientation_quat[1] + t.transform.rotation.z = marker_orientation_quat[2] + t.transform.rotation.w = marker_orientation_quat[3] + self.tf_broadcaster.sendTransform(t) + + def camera_info_callback(self, msg): + self.camera_info = 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/crane_plus_examples_py/crane_plus_examples_py/color_detection.py b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py new file mode 100644 index 00000000..d2fa0c14 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -0,0 +1,143 @@ +# Copyright 2025 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 rclpy +from rclpy.node import Node +from sensor_msgs.msg import CameraInfo, Image +import tf2_ros + + +class ImageSubscriber(Node): + def __init__(self): + super().__init__('color_detection') + self.image_subscription = self.create_subscription( + Image, 'image_raw', self.image_callback, 10 + ) + self.camera_info_subscription = self.create_subscription( + CameraInfo, 'camera_info', self.camera_info_callback, 10 + ) + self.image_thresholded_publisher = self.create_publisher( + Image, 'image_thresholded', 10 + ) + self.tf_broadcaster = tf2_ros.TransformBroadcaster() + self.camera_info = None + self.bridge = CvBridge() + + def image_callback(self, msg): + if self.camera_info: + # 赤い物体を検出するようにHSVの範囲を設定 + # 周囲の明るさ等の動作環境に合わせて調整 + LOW_H_1 = 0 + HIGH_H_1 = 10 + LOW_H_2 = 170 + HIGH_H_2 = 179 + LOW_S = 100 + HIGH_S = 255 + LOW_V = 100 + HIGH_V = 255 + + # ウェブカメラの画像を受け取る + cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding) + + # 画像をRGBからHSVに変換（取得したカメラ画像にフォーマットを合わせる） + cv_img = cv2.cvtColor(cv_img, cv2.COLOR_RGB2HSV) + + # 画像の二値化 + img_mask_1 = cv2.inRange( + cv_img, (LOW_H_1, LOW_S, LOW_V), (HIGH_H_1, HIGH_S, HIGH_V) + ) + img_mask_2 = cv2.inRange( + cv_img, (LOW_H_2, LOW_S, LOW_V), (HIGH_H_2, HIGH_S, HIGH_V) + ) + + # マスク画像の合成 + img_thresholded = cv2.bitwise_or(img_mask_1, img_mask_2) + + # ノイズ除去の処理 + 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: + # カメラモデル作成 + camera_model = PinholeCameraModel() + + # カメラのパラメータを設定 + camera_model.fromCameraInfo(self.camera_info) + + # 画像座標系における把持対象物の位置（2D） + pixel_x = d_m10 / d_area + pixel_y = d_m01 / d_area + point = (pixel_x, pixel_y) + + # 補正後の画像座標系における把持対象物の位置を取得（2D） + rect_point = camera_model.rectifyImage(point) + + # カメラ座標系から見た把持対象物の方向（Ray）を取得する + ray = camera_model.projectPixelTo3dRay(rect_point) + + # カメラの高さを0.44[m]として把持対象物の位置を計算 + CAMERA_HEIGHT = 0.46 + object_position = [ray.x * CAMERA_HEIGHT, + ray.y * CAMERA_HEIGHT, + ray.z * CAMERA_HEIGHT] + + # 把持対象物の位置をTFに配信 + t = TransformStamped() + t.header = 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' + ) + self.image_thresholded_publisher.publish(img_thresholded_msg) + + def camera_info_callback(self, msg): + self.camera_info = 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/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py new file mode 100644 index 00000000..38867001 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -0,0 +1,94 @@ +# Copyright 2025 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 crane_plus_examples_py.utils import plan_and_execute + +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('gripper_control') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='gripper_control') + logger.info('MoveItPy instance created') + + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + + plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # gripperを閉じる + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(30.0)]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(-30.0)]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # gripperを0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [math.radians(0.0)]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py new file mode 100644 index 00000000..73058387 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -0,0 +1,95 @@ +# Copyright 2025 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 crane_plus_examples_py.utils import plan_and_execute + +from moveit.core.kinematic_constraints import construct_joint_constraint +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('joint_values') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='joint_values') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + + # instantiate a RobotState instance using the current robot model + robot_model = crane_plus.get_robot_model() + robot_state = RobotState(robot_model) + + plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # armの関節のjoint1〜4を順番に45[deg]ずつ動かす + joint_names = ['crane_plus_joint1', 'crane_plus_joint2', + 'crane_plus_joint3', 'crane_plus_joint4'] + target_joint_value = math.radians(45) + + for joint_name in joint_names: + arm.set_start_state_to_current_state() + + joint_values = {joint_name: target_joint_value} + robot_state.joint_positions = joint_values + joint_constraint = construct_joint_constraint( + robot_state=robot_state, + joint_model_group=crane_plus.get_robot_model().get_joint_model_group('arm'), + ) + + arm.set_goal_state( + motion_plan_constraints=[joint_constraint] + ) + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params + ) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py new file mode 100644 index 00000000..1439ef26 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place.py @@ -0,0 +1,251 @@ +# Copyright 2025 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 crane_plus_examples_py.utils import plan_and_execute +from geometry_msgs.msg import Point, Pose, PoseStamped, Quaternion +from moveit.core.robot_state import RobotState +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger +from scipy.spatial.transform import Rotation + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('pick_and_place') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pick_and_place') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + # グリッパ制御用 planning component + gripper = crane_plus.get_planning_component('gripper') + + robot_model = crane_plus.get_robot_model() + + arm_plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + gripper_plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + arm_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + arm_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + gripper_plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # グリッパの開閉角 + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 物体を掴む位置 + rightward = Rotation.from_euler('xyz', [0.0, 90.0, -90.0], degrees=True) + quat = rightward.as_quat() + PRE_GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.17), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + downward = Rotation.from_euler('xyz', [0.0, 180.0, -90.0], degrees=True) + quat = downward.as_quat() + GRASP_POSE = Pose(position=Point(x=0.0, y=-0.09, z=0.14), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + # 物体を置く位置 + forward = Rotation.from_euler('xyz', [0.0, 90.0, 0.0], degrees=True) + quat = forward.as_quat() + RELEASE_POSE = Pose(position=Point(x=0.15, y=0.0, z=0.06), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3])) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 物体の上に腕を伸ばす + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = PRE_GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # 掴みに行く + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # ハンドを閉じる + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # 持ち上げる + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = PRE_GRASP_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # 下ろす + arm.set_start_state_to_current_state() + goal_pose = PoseStamped() + goal_pose.header.frame_id = 'crane_plus_base' + goal_pose.pose = RELEASE_POSE + arm.set_goal_state(pose_stamped_msg=goal_pose, pose_link='crane_plus_link4') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # ハンドを開く + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=arm_plan_request_params, + ) + + # gripperの開閉角を0度にする + gripper.set_start_state_to_current_state() + robot_state = RobotState(robot_model) + robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT]) + gripper.set_goal_state(robot_state=robot_state) + plan_and_execute( + crane_plus, + gripper, + logger, + single_plan_parameters=gripper_plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py new file mode 100644 index 00000000..bc93c5e1 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pick_and_place_tf.py @@ -0,0 +1,266 @@ +# Copyright 2025 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 datetime +import math + +from crane_plus_examples_py.utils import plan_and_execute + +from geometry_msgs.msg import PoseStamped + +# moveit python library +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.logging import get_logger +from rclpy.node import Node +from rclpy.time import Time +import tf2_ros +from tf2_ros import TransformListener, TransformStamped +from tf2_ros.buffer import Buffer +from scipy.spatial.transform import Rotation + + +class PickAndPlaceTf(Node): + def __init__(self, crane_plus): + super().__init__('pick_and_place_tf_node') + self.logger = get_logger('pick_and_place_tf') + self.tf_past = TransformStamped() + self.crane_plus = crane_plus + self.crane_plus_arm = crane_plus.get_planning_component('arm') + self.crane_plus_gripper = crane_plus.get_planning_component('gripper') + # instantiate a RobotState instance using the current robot model + self.robot_model = crane_plus.get_robot_model() + self.robot_state = RobotState(self.robot_model) + + # planningのパラメータ設定 + # armのパラメータ設定用 + self.arm_plan_request_params = PlanRequestParameters( + self.crane_plus, + 'ompl_rrtc', + ) + # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_velocity_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + self.arm_plan_request_params.max_acceleration_scaling_factor = 1.0 + + # gripperのパラメータ設定用 + self.gripper_plan_request_params = PlanRequestParameters( + self.crane_plus, + 'ompl_rrtc', + ) + # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_velocity_scaling_factor = 1.0 + + # Set 0.0 ~ 1.0 + self.gripper_plan_request_params.max_acceleration_scaling_factor = 1.0 + + # SRDFに定義されている'home'の姿勢にする + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state(configuration_name='home') + plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + + # 可動範囲を制限する + constraints = Constraints() + constraints.name = 'arm_constraints' + + jointConstraint = JointConstraint() + jointConstraint.joint_name = 'crane_plus_joint1' + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(100) + jointConstraint.tolerance_below = math.radians(100) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + jointConstraint.joint_name = 'crane_plus_joint3' + jointConstraint.position = 0.0 + jointConstraint.tolerance_above = math.radians(0) + jointConstraint.tolerance_below = math.radians(180) + jointConstraint.weight = 1.0 + constraints.joint_constraints.append(jointConstraint) + + self.crane_plus_arm.set_path_constraints(constraints) + + # 待機姿勢 + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # tf + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) + + # Call on_timer function every second + self.timer = self.create_timer(0.5, self.on_timer) + + def on_timer(self): + # target_0のtf位置姿勢を取得 + tf_msg = TransformStamped() + try: + tf_msg = self.tf_buffer.lookup_transform( + 'base_link', 'target_0', Time()) + except tf2_ros.LookupException as ex: + self.get_logger().info( + f'Could not transform base_link to target: {ex}' + ) + + now = Time() + FILTERING_TIME = datetime.timedelta(seconds=2) + STOP_TIME_THRESHOLD = datetime.timedelta(seconds=3) + DISTANCE_THRESHOLD = 0.01 + + # 経過時間と停止時間を計算(nsec) + # 経過時間 + TF_ELAPSED_TIME = now.nanoseconds - tf_msg.header.stamp.nanosec + # 停止時間 + if self.tf_past is not None: + TF_STOP_TIME = now.nanoseconds - self.tf_past.header.stamp.nanosec + else: + TF_STOP_TIME = now.nanoseconds + + # 現在時刻から2秒以内に受け取ったtfを使用 + if TF_ELAPSED_TIME < FILTERING_TIME.total_seconds() * 1e9: + tf_diff = np.sqrt((self.tf_past.transform.translation.x + - tf_msg.transform.translation.x) ** 2 + + (self.tf_past.transform.translation.y + - tf_msg.transform.translation.y) ** 2 + + (self.tf_past.transform.translation.z + - tf_msg.transform.translation.z) ** 2 + ) + # 把持対象の位置が停止していることを判定 + if tf_diff < DISTANCE_THRESHOLD: + # 把持対象が3秒以上停止している場合ピッキング動作開始 + if TF_STOP_TIME > STOP_TIME_THRESHOLD.total_seconds() * 1e9: + self._picking(tf_msg) + else: + self.tf_past = tf_msg + + def _picking(self, tf_msg): + GRIPPER_DEFAULT = 0.0 + GRIPPER_OPEN = math.radians(-30.0) + GRIPPER_CLOSE = math.radians(10.0) + + # 何かを掴んでいた時のためにハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # ロボット座標系（2D）の原点から見た把持対象物への角度を計算 + x = tf_msg.transform.translation.x + y = tf_msg.transform.translation.y + theta_rad = math.atan2(y, x) + theta_deg = math.degrees(theta_rad) + + # 把持対象物に正対する + self._control_arm(0.0, 0.0, 0.17, 0, 90, theta_deg) + + # 掴みに行く + GRIPPER_OFFSET = 0.13 + gripper_offset_x = GRIPPER_OFFSET * math.cos(theta_rad) + gripper_offset_y = GRIPPER_OFFSET * math.sin(theta_rad) + if not self._control_arm(x - gripper_offset_x, y - gripper_offset_y, + 0.05, 0, 90, theta_deg): + # アーム動作に失敗した時はpick_and_placeを中断して待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + return + + # ハンドを閉じる + self._control_gripper(GRIPPER_CLOSE) + + # 移動する + self._control_arm(0.0, 0.0, 0.17, 0, 90, 0) + + # 下ろす + self._control_arm(0.0, -0.15, 0.06, 0, 90, -90) + + # ハンドを開く + self._control_gripper(GRIPPER_OPEN) + + # 少しだけハンドを持ち上げる + self._control_arm(0.0, -0.15, 0.10, 0, 90, -90) + + # 待機姿勢に戻る + self._control_arm(0.0, 0.0, 0.17, 0, 0, 0) + + # ハンドを閉じる + self._control_gripper(GRIPPER_DEFAULT) + + # グリッパ制御 + def _control_gripper(self, angle): + self.robot_state.set_joint_group_positions('gripper', [angle]) + self.crane_plus_gripper.set_start_state_to_current_state() + self.crane_plus_gripper.set_goal_state(robot_state=self.robot_state) + plan_and_execute( + self.crane_plus, + self.crane_plus_gripper, + self.logger, + single_plan_parameters=self.gripper_plan_request_params, + ) + + # アーム制御 + def _control_arm(self, x, y, z, roll, pitch, yaw): + target_pose = PoseStamped() + target_pose.header.frame_id = 'crane_plus_base' + target_pose.pose.position.x = x + target_pose.pose.position.y = y + target_pose.pose.position.z = z + rotation = Rotation.from_euler('xyz', [roll, pitch, yaw], degrees=True) + quat = rotation.as_quat() + target_pose.pose.orientation.x = quat[0] + target_pose.pose.orientation.y = quat[1] + target_pose.pose.orientation.z = quat[2] + target_pose.pose.orientation.w = quat[3] + self.crane_plus_arm.set_start_state_to_current_state() + self.crane_plus_arm.set_goal_state( + pose_stamped_msg=target_pose, pose_link='crane_plus_link4' + ) + result = plan_and_execute( + self.crane_plus, + self.crane_plus_arm, + # logger=None, + self.logger, + single_plan_parameters=self.arm_plan_request_params, + ) + return result + + +def main(args=None): + # ros2の初期化 + rclpy.init(args=args) + + # MoveItPy初期化 + crane_plus = MoveItPy(node_name='moveit_py') + + # node生成 + pick_and_place_tf_node = PickAndPlaceTf(crane_plus) + rclpy.spin(pick_and_place_tf_node) + + # MoveItPyの終了 + crane_plus.shutdown() + + # rclpyの終了 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py new file mode 100644 index 00000000..428bf700 --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/pose_groupstate.py @@ -0,0 +1,82 @@ +# Copyright 2025 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 crane_plus_examples_py.utils import plan_and_execute + +from moveit.planning import ( + MoveItPy, + PlanRequestParameters, +) + +import rclpy +from rclpy.logging import get_logger + + +def main(args=None): + rclpy.init(args=args) + logger = get_logger('pose_groupstate') + + # instantiate MoveItPy instance and get planning component + crane_plus = MoveItPy(node_name='pose_groupstate') + logger.info('MoveItPy instance created') + + # アーム制御用 planning component + arm = crane_plus.get_planning_component('arm') + + plan_request_params = PlanRequestParameters( + crane_plus, + 'ompl_rrtc', + ) + + # 動作速度の調整 + plan_request_params.max_acceleration_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDF内に定義されている'vertical'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='vertical') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # SRDF内に定義されている'home'の姿勢にする + arm.set_start_state_to_current_state() + arm.set_goal_state(configuration_name='home') + plan_and_execute( + crane_plus, + arm, + logger, + single_plan_parameters=plan_request_params, + ) + + # Finish with error. Related Issue + # https://github.com/moveit/moveit2/issues/2693 + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/crane_plus_examples_py/crane_plus_examples_py/utils.py b/crane_plus_examples_py/crane_plus_examples_py/utils.py new file mode 100644 index 00000000..8e363cee --- /dev/null +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -0,0 +1,42 @@ +# Copyright 2025 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 time + + +def plan_and_execute( + robot, + planning_component, + logger, + single_plan_parameters=None, + multi_plan_parameters=None, + sleep_time=0.0, +): + logger.info('Planning trajectory') + if multi_plan_parameters is not None: + plan_result = planning_component.plan(multi_plan_parameters=multi_plan_parameters) + elif single_plan_parameters is not None: + plan_result = planning_component.plan(single_plan_parameters=single_plan_parameters) + else: + plan_result = planning_component.plan() + + # execute the plan + result = None + if plan_result: + logger.info('Executing plan') + robot_trajectory = plan_result.trajectory + result = robot.execute(robot_trajectory, controllers=[]) + else: + logger.error('Planning failed') + result = False + + time.sleep(sleep_time) + return result diff --git a/crane_plus_examples_py/launch/camera_example.launch.py b/crane_plus_examples_py/launch/camera_example.launch.py new file mode 100644 index 00000000..35ea2af1 --- /dev/null +++ b/crane_plus_examples_py/launch/camera_example.launch.py @@ -0,0 +1,91 @@ +# Copyright 2025 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 crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + 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 crane_plus_description.', + ) + + moveit_config = ( + MoveItConfigsBuilder('crane_plus') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + declare_example_name = DeclareLaunchArgument( + 'example', default_value='color_detection', + description=('Set an example executable name: ' + '[aruco_detection, color_detection]') + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) + + # 下記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='crane_plus_examples_py', + executable='pick_and_place_tf', + output='screen', + parameters=[config_dict] + ) + + example_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[config_dict], + ) + + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + picking_node, + example_node + ]) diff --git a/crane_plus_examples_py/launch/example.launch.py b/crane_plus_examples_py/launch/example.launch.py new file mode 100644 index 00000000..3915f40f --- /dev/null +++ b/crane_plus_examples_py/launch/example.launch.py @@ -0,0 +1,84 @@ +# Copyright 2025 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 crane_plus_description.robot_description_loader \ + import RobotDescriptionLoader +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_launch_description(): + 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 crane_plus_description.', + ) + + moveit_config = ( + MoveItConfigsBuilder('crane_plus') + .planning_scene_monitor( + publish_robot_description=True, + publish_robot_description_semantic=True, + ) + .moveit_cpp( + file_path=get_package_share_directory('crane_plus_examples_py') + + '/config/crane_plus_moveit_py_examples.yaml' + ) + .to_moveit_configs() + ) + + moveit_config.robot_description = { + 'robot_description': LaunchConfiguration('loaded_description') + } + + declare_example_name = DeclareLaunchArgument( + 'example', + default_value='gripper_control', + description='Set an example executable name: \ + [gripper_control, pose_groupstate, \ + joint_values, pick_and_place]' + ) + + declare_use_sim_time = DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description=('Set true when using the gazebo simulator.'), + ) + + # 下記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')}) + + example_node = Node( + name=[LaunchConfiguration('example'), '_node'], + package='crane_plus_examples_py', + executable=LaunchConfiguration('example'), + output='screen', + parameters=[config_dict], + ) + + return LaunchDescription([ + declare_loaded_description, + declare_example_name, + declare_use_sim_time, + example_node + ]) diff --git a/crane_plus_examples_py/package.xml b/crane_plus_examples_py/package.xml new file mode 100644 index 00000000..e73cd183 --- /dev/null +++ b/crane_plus_examples_py/package.xml @@ -0,0 +1,28 @@ + + + + crane_plus_examples_py + 0.1.0 + Python examples of CRANE+ V2 ROS package + RT Corporation + Apache-2.0 + + Nozomi Mizoguchi + Atsushi Kuwagata + + rclpy + std_msgs + moveit + moveit_py + python3-numpy + python3-scipy + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/crane_plus_examples_py/resource/crane_plus_examples_py b/crane_plus_examples_py/resource/crane_plus_examples_py new file mode 100644 index 00000000..e69de29b diff --git a/crane_plus_examples_py/setup.cfg b/crane_plus_examples_py/setup.cfg new file mode 100644 index 00000000..54c93bce --- /dev/null +++ b/crane_plus_examples_py/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/crane_plus_examples_py +[install] +install_scripts=$base/lib/crane_plus_examples_py diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py new file mode 100644 index 00000000..567c4009 --- /dev/null +++ b/crane_plus_examples_py/setup.py @@ -0,0 +1,40 @@ +from glob import glob +import os + +from setuptools import find_packages, setup + +package_name = 'crane_plus_examples_py' + +setup( + name=package_name, + version='0.1.0', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + (os.path.join('share', package_name, 'launch'), + glob(os.path.join('launch', '*launch.[pxy][yma]'))), + (os.path.join('share', package_name, 'config'), + glob(os.path.join('config', '*'))), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='user', + maintainer_email='user@todo.todo', + description='TODO: Package description', + license='TODO: License declaration', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'aruco_detection = crane_plus_examples_py.aruco_detectio:main', + 'color_detection = crane_plus_examples_py.color_detection:main', + 'gripper_control = crane_plus_examples_py.gripper_control:main', + 'joint_values = crane_plus_examples_py.joint_values:main', + 'pick_and_place_tf = crane_plus_examples_py.pick_and_place_tf:main', + 'pick_and_place = crane_plus_examples_py.pick_and_place:main', + 'pose_groupstate= crane_plus_examples_py.pose_groupstate:main', + 'utils = crane_plus_examples_py.utils:main', + ], + }, +) diff --git a/crane_plus_examples_py/test/test_copyright.py b/crane_plus_examples_py/test/test_copyright.py new file mode 100644 index 00000000..97a39196 --- /dev/null +++ b/crane_plus_examples_py/test/test_copyright.py @@ -0,0 +1,25 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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_copyright.main import main +import pytest + + +# Remove the `skip` decorator once the source file(s) have a copyright header +@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.') +@pytest.mark.copyright +@pytest.mark.linter +def test_copyright(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found errors' diff --git a/crane_plus_examples_py/test/test_flake8.py b/crane_plus_examples_py/test/test_flake8.py new file mode 100644 index 00000000..27ee1078 --- /dev/null +++ b/crane_plus_examples_py/test/test_flake8.py @@ -0,0 +1,25 @@ +# Copyright 2017 Open Source Robotics Foundation, Inc. +# +# 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_flake8.main import main_with_errors +import pytest + + +@pytest.mark.flake8 +@pytest.mark.linter +def test_flake8(): + rc, errors = main_with_errors(argv=[]) + assert rc == 0, \ + 'Found %d code style errors / warnings:\n' % len(errors) + \ + '\n'.join(errors) diff --git a/crane_plus_examples_py/test/test_pep257.py b/crane_plus_examples_py/test/test_pep257.py new file mode 100644 index 00000000..b234a384 --- /dev/null +++ b/crane_plus_examples_py/test/test_pep257.py @@ -0,0 +1,23 @@ +# Copyright 2015 Open Source Robotics Foundation, Inc. +# +# 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_pep257.main import main +import pytest + + +@pytest.mark.linter +@pytest.mark.pep257 +def test_pep257(): + rc = main(argv=['.', 'test']) + assert rc == 0, 'Found code style errors / warnings' diff --git a/crane_plus_moveit_config/launch/run_move_group.launch.py b/crane_plus_moveit_config/launch/run_move_group.launch.py old mode 100755 new mode 100644