From 057338ce7ebc3b2b855883a193888124597d501f Mon Sep 17 00:00:00 2001 From: mizonon Date: Thu, 31 Oct 2024 15:09:00 +0900 Subject: [PATCH] =?UTF-8?q?CI=E5=AF=BE=E5=BF=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- crane_plus_examples/launch/demo.launch.py | 2 +- .../crane_plus_examples_py/aruco_detection.py | 56 ++++---- .../crane_plus_examples_py/color_detection.py | 51 ++++---- .../crane_plus_examples_py/gripper_control.py | 30 ++--- .../crane_plus_examples_py/joint_values.py | 36 +++--- .../pick_and_place_tf.py | 122 +++++++++--------- .../crane_plus_examples_py/utils.py | 28 ++-- crane_plus_examples_py/setup.py | 9 +- 8 files changed, 159 insertions(+), 175 deletions(-) diff --git a/crane_plus_examples/launch/demo.launch.py b/crane_plus_examples/launch/demo.launch.py index 17cd579c..12a7dbeb 100644 --- a/crane_plus_examples/launch/demo.launch.py +++ b/crane_plus_examples/launch/demo.launch.py @@ -30,7 +30,7 @@ def generate_launch_description(): default_value='/dev/ttyUSB0', description='Set port name.' ) - + declare_use_camera = DeclareLaunchArgument( 'use_camera', default_value='false', 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 index a7fac85f..b17a1d9f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/aruco_detection.py @@ -1,45 +1,40 @@ # Copyright 2020 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 rclpy -from rclpy.node import Node - -import numpy as np - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image import cv2 from cv2 import aruco from cv_bridge import CvBridge -import tf2_ros - from crane_plus_examples_py.utils import rotation_matrix_to_quaternion +from geometry_msgs.msg import TransformStamped +import numpy as np +import rclpy +from rclpy.node import Node +import tf2_ros +from sensor_msgs.msg import CameraInfo, Image class ImageSubscriber(Node): def __init__(self): - super().__init__("aruco_detection") + super().__init__('aruco_detection') self.image_subscription = self.create_subscription( - Image, "image_raw", self.image_callback, 10 + Image, 'image_raw', self.image_callback, 10 ) self.camera_info_subscription = self.create_subscription( - CameraInfo, "camera_info", self.camera_info_callback, 10 + CameraInfo, 'camera_info', self.camera_info_callback, 10 ) self.tf_broadcaster = tf2_ros.TransformBroadcaster() - + self.camera_info = None self.bridge = CvBridge() @@ -47,31 +42,31 @@ 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): @@ -81,7 +76,7 @@ def image_callback(self, msg): # tfの配信 t = TransformStamped() t.header = msg.header - t.child_frame_id = "target_" + str(ids[i][0]) + 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] @@ -94,20 +89,17 @@ def image_callback(self, msg): t.transform.rotation.z = q.z t.transform.rotation.w = q.w 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) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) image_subscriber.destroy_node() rclpy.shutdown() 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 index 69783445..ae31d84f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/color_detection.py +++ b/crane_plus_examples_py/crane_plus_examples_py/color_detection.py @@ -1,27 +1,25 @@ # Copyright 2020 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 rclpy -from rclpy.node import Node - -# from std_msgs.msg import String -from geometry_msgs.msg import TransformStamped -from sensor_msgs.msg import CameraInfo, Image -from image_geometry import PinholeCameraModel 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 import tf2_ros +from sensor_msgs.msg import CameraInfo, Image class ImageSubscriber(Node): @@ -50,24 +48,24 @@ def image_callback(self, msg): HIGH_S = 255 LOW_V = 50 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) - + # 穴埋めの処理 kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5)) img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel) @@ -77,26 +75,26 @@ def image_callback(self, msg): 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 = { @@ -104,7 +102,7 @@ def image_callback(self, msg): 'y': ray.y * CAMERA_HEIGHT, 'z': ray.z * CAMERA_HEIGHT, } - + # 把持対象物の位置をTFに配信 t = TransformStamped() t.header = msg.header @@ -113,14 +111,14 @@ def image_callback(self, msg): t.transform.translation.y = object_position['y'] t.transform.translation.z = object_position['z'] 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) @@ -128,12 +126,9 @@ def main(args=None): image_subscriber = ImageSubscriber() rclpy.spin(image_subscriber) - # Destroy the node explicitly - # (optional - otherwise it will be done automatically - # when the garbage collector destroys the node object) image_subscriber.destroy_node() rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + 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 index d1b351bf..bb5f8b90 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py +++ b/crane_plus_examples_py/crane_plus_examples_py/gripper_control.py @@ -12,11 +12,8 @@ # See the License for the specific language governing permissions and # limitations under the License. +from crane_plus_examples_py.utils import plan_and_execute import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger # moveit python library from moveit.core.robot_state import RobotState @@ -24,20 +21,21 @@ MoveItPy, PlanRequestParameters, ) - -from crane_plus_examples_py.utils import plan_and_execute +import rclpy +from rclpy.logging import get_logger def main(args=None): # ros2の初期化 rclpy.init(args=args) + # ロガー生成 - logger = get_logger("gripper_control") + logger = get_logger('gripper_control') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_gripper = crane_plus.get_planning_component("gripper") - logger.info("MoveItPy instance created") + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_gripper = crane_plus.get_planning_component('gripper') + logger.info('MoveItPy instance created') # instantiate a RobotState instance using the current robot model robot_model = crane_plus.get_robot_model() @@ -46,7 +44,7 @@ def main(args=None): # planningのパラメータ設定 plan_request_params = PlanRequestParameters( crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) # 速度＆加速度のスケーリングファクタを設定 @@ -54,7 +52,7 @@ def main(args=None): plan_request_params.max_velocity_scaling_factor = 1.0 # Set 0.0 ~ 1.0 # グリッパーを+30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(30)]) + robot_state.set_joint_group_positions('gripper', [math.radians(30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -63,8 +61,9 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) + # グリッパーを-30[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [math.radians(-30)]) + robot_state.set_joint_group_positions('gripper', [math.radians(-30)]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -73,8 +72,9 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) + # グリッパーを0[deg]の位置に動かす - robot_state.set_joint_group_positions("gripper", [0.0]) + robot_state.set_joint_group_positions('gripper', [0.0]) crane_plus_gripper.set_start_state_to_current_state() crane_plus_gripper.set_goal_state(robot_state=robot_state) plan_and_execute( @@ -83,7 +83,7 @@ def main(args=None): logger, single_plan_parameters=plan_request_params, ) - + # MoveItPyの終了 crane_plus.shutdown() 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 index 687b56d5..35bdc02f 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/joint_values.py +++ b/crane_plus_examples_py/crane_plus_examples_py/joint_values.py @@ -12,13 +12,9 @@ # See the License for the specific language governing permissions and # limitations under the License. +from crane_plus_examples_py.utils import plan_and_execute import math -import rclpy - -# generic ros libraries -from rclpy.logging import get_logger - # moveit python library from moveit.core.robot_state import RobotState from moveit.planning import ( @@ -26,8 +22,8 @@ PlanRequestParameters, ) from moveit.core.kinematic_constraints import construct_joint_constraint - -from crane_plus_examples_py.utils import plan_and_execute +import rclpy +from rclpy.logging import get_logger def main(args=None): @@ -35,37 +31,41 @@ def main(args=None): rclpy.init(args=args) # ロガー生成 - logger = get_logger("joint_values") + logger = get_logger('joint_values') # MoveItPy初期化 - crane_plus = MoveItPy(node_name="moveit_py") - crane_plus_arm = crane_plus.get_planning_component("arm") - logger.info("MoveItPy instance created") - + crane_plus = MoveItPy(node_name='moveit_py') + crane_plus_arm = crane_plus.get_planning_component('arm') + logger.info('MoveItPy instance created') + # instantiate a 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", + '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"] + 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: 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"), + joint_model_group=crane_plus + .get_robot_model().get_joint_model_group('arm'), ) crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(motion_plan_constraints=[joint_constraint]) + crane_plus_arm.set_goal_state( + motion_plan_constraints=[joint_constraint] + ) plan_and_execute( crane_plus, crane_plus_arm, @@ -73,9 +73,9 @@ def main(args=None): single_plan_parameters=plan_request_params, ) - # 現在の位置から"vertical"ポジションに動かす + # 現在の位置から'vertical'ポジションに動かす crane_plus_arm.set_start_state_to_current_state() - crane_plus_arm.set_goal_state(configuration_name="vertical") + crane_plus_arm.set_goal_state(configuration_name='vertical') plan_and_execute( crane_plus, crane_plus_arm, 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 index d0ea07d8..779e463e 100644 --- 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 @@ -1,59 +1,55 @@ # Copyright 2020 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 -import numpy as np +from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion import datetime - -# import angles from geometry_msgs.msg import Pose -from moveit_msgs.msg import Constraints, JointConstraint -import rclpy -from rclpy.time import Time -from rclpy.node import Node -from rclpy.logging import get_logger -import tf2_ros -from tf2_ros import TransformStamped, TransformListener -from tf2_ros.buffer import Buffer - +import math +import numpy as np # moveit python library from moveit.core.robot_state import RobotState +from moveit_msgs.msg import Constraints, JointConstraint from moveit.planning import ( MoveItPy, PlanRequestParameters, ) - -from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion +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 TransformStamped, TransformListener +from tf2_ros.buffer import Buffer class PickAndPlaceTf(Node): def __init__(self, crane_plus): - super().__init__("pick_and_place_tf_node") - self.logger = get_logger("pick_and_place_tf") + 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") + 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", + 'ompl_rrtc', ) 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 # Set 0.0 ~ 1.0 @@ -61,14 +57,14 @@ def __init__(self, crane_plus): # gripperのパラメータ設定用 self.gripper_plan_request_params = PlanRequestParameters( self.crane_plus, - "ompl_rrtc", + 'ompl_rrtc', ) 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 # Set 0.0 ~ 1.0 - # SRDFに定義されている"home"の姿勢にする + # SRDFに定義されている'home'の姿勢にする self.crane_plus_arm.set_start_state_to_current_state() - self.crane_plus_arm.set_goal_state(configuration_name="home") + self.crane_plus_arm.set_goal_state(configuration_name='home') plan_and_execute( self.crane_plus, self.crane_plus_arm, @@ -76,45 +72,47 @@ def __init__(self, crane_plus): self.logger, single_plan_parameters=self.arm_plan_request_params, ) - + # 可動範囲を制限する constraints = Constraints() - constraints.name = "arm_constraints" - + constraints.name = 'arm_constraints' + jointConstraint = JointConstraint() - jointConstraint.joint_name = "crane_plus_joint1" + 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.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位置姿勢を取得 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}') + self.get_logger().info( + f'Could not transform base_link to target: {ex}' + ) now = Time() FILTERING_TIME = datetime.timedelta(seconds=2) @@ -129,7 +127,7 @@ def on_timer(self): 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 @@ -151,53 +149,53 @@ 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, + 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.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( @@ -207,7 +205,7 @@ def _control_gripper(self, angle): self.logger, single_plan_parameters=self.gripper_plan_request_params, ) - + # アーム制御 def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose = Pose() @@ -222,7 +220,7 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): target_pose.orientation.w = q[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" + pose_stamped_msg=target_pose, pose_link='crane_plus_link4' ) result = plan_and_execute( self.crane_plus, @@ -232,25 +230,25 @@ def _control_arm(self, x, y, z, roll, pitch, yaw): 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") + 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() \ No newline at end of file + 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 index 56709c9a..3b48f781 100644 --- a/crane_plus_examples_py/crane_plus_examples_py/utils.py +++ b/crane_plus_examples_py/crane_plus_examples_py/utils.py @@ -1,23 +1,21 @@ # Copyright 2020 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 import numpy as np -import quaternion - -# generic ros libraries from rclpy.logging import get_logger +import time +import quaternion # plan and execute @@ -31,8 +29,8 @@ def plan_and_execute( ): """Helper function to plan and execute a motion.""" # plan to goal - logger = get_logger("plan_and_execute") - logger.info("Planning trajectory") + logger = get_logger('plan_and_execute') + logger.info('Planning trajectory') if multi_plan_parameters is not None: plan_result = planning_component.plan( multi_plan_parameters=multi_plan_parameters @@ -50,16 +48,16 @@ def plan_and_execute( # execute the plan result = None if plan_result: - logger.info("Executing plan") + logger.info('Executing plan') robot_trajectory = plan_result.trajectory result = robot.execute(robot_trajectory, controllers=[]) else: - logger.error("Planning failed") + logger.error('Planning failed') result = False time.sleep(sleep_time) return result - - + + # euler --> quaternion def euler_to_quaternion(roll, pitch, yaw): cy = np.cos(yaw * 0.5) @@ -80,6 +78,6 @@ def euler_to_quaternion(roll, pitch, yaw): # rotation matrix --> quaternion def rotation_matrix_to_quaternion(rotation_matrix): # numpy-quaternionを使用して回転行列からクォータニオンを計算 - #3x3の回転行列をnumpy.quaternionに変換する + # 3x3の回転行列をnumpy.quaternionに変換する q = quaternion.from_rotation_matrix(rotation_matrix) - return q \ No newline at end of file + return q diff --git a/crane_plus_examples_py/setup.py b/crane_plus_examples_py/setup.py index e6a0e1bf..fcb6446b 100644 --- a/crane_plus_examples_py/setup.py +++ b/crane_plus_examples_py/setup.py @@ -1,20 +1,21 @@ -import os from glob import glob +import os from setuptools import find_packages, setup package_name = 'crane_plus_examples_py' setup( name=package_name, - # version='0.0.0', 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', '*'))), + (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,