crane_x7_exanples_pyのコード一部修正

crane_x7_examples_py/crane_x7_examples_py/color_detection.py

@@ -29,17 +29,17 @@
 
 class ImageSubscriber(Node):
     def __init__(self):
-        super().__init__("color_detection")
+        super().__init__('color_detection')
         self.image_subscription = self.create_subscription(
-            Image, "/camera/color/image_raw",
+            Image, '/camera/color/image_raw',
             self.image_callback, 10
         )
         self.depth_info_subscription = self.create_subscription(
-            Image, "/camera/aligned_depth_to_color/image_raw",
+            Image, '/camera/aligned_depth_to_color/image_raw',
             self.depth_callback, 10
         )
         self.camera_info_subscription = self.create_subscription(
-            CameraInfo, "/camera/color/camera_info", 
+            CameraInfo, '/camera/color/camera_info', 
             self.camera_info_callback, 10
         )
         self.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded',  10)
@@ -48,7 +48,7 @@ def __init__(self):
         self.depth_image = None
         self.bridge = CvBridge()
         # ロガー生成
-        self.logger = get_logger("pick_and_place")
+        self.logger = get_logger('pick_and_place')
 
     def image_callback(self, msg):
         # カメラのパラメータを取得してから処理を行う
@@ -117,7 +117,7 @@ def image_callback(self, msg):
             DEPTH_MAX = 0.5
             DEPTH_MIN = 0.2
             if center_distance < DEPTH_MIN or center_distance > DEPTH_MAX:
-                self.logger.info(f"Failed to get depth at {point}.")
+                self.logger.info(f'Failed to get depth at {point}.')
                 return
 
             # 把持対象物の位置を計算
@@ -126,22 +126,23 @@ def image_callback(self, msg):
             # 把持対象物の位置をTFに配信
             t = TransformStamped()
             t.header = msg.header
-            t.child_frame_id = "target_0"
+            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']
             self.tf_broadcaster.sendTransform(t)
 
             # 閾値による二値化画像を配信
-            img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8")   
+            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 depth_callback(self, msg):
         self.depth_image = msg
-
+
+
 def main(args=None):
     rclpy.init(args=args)
 
@@ -247,14 +248,14 @@ def main(args=None):
                 # 把持対象物の位置をTFに配信
                 t = TransformStamped()
                 t.header = msg.header
-                t.child_frame_id = "target_0"
+                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']
                 self.tf_broadcaster.sendTransform(t)
 
             # 閾値による二値化画像を配信
-            img_thresholded_msg = self.bridge.cv2_to_imgmsg(img_thresholded, encoding="mono8")   
+            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):

crane_x7_examples_py/crane_x7_examples_py/gripper_control.py

@@ -33,13 +33,13 @@ def main(args=None):
     rclpy.init(args=args)
 
     # ロガー生成
-    logger = get_logger("pick_and_place")
+    logger = get_logger('pick_and_place')
 
     # MoveItPy初期化
-    crane_x7 = MoveItPy(node_name="moveit_py")
-    crane_x7_arm = crane_x7.get_planning_component("arm")
-    crane_x7_gripper = crane_x7.get_planning_component("gripper")
-    logger.info("MoveItPy instance created")
+    crane_x7 = MoveItPy(node_name='moveit_py')
+    crane_x7_arm = crane_x7.get_planning_component('arm')
+    crane_x7_gripper = crane_x7.get_planning_component('gripper')
+    logger.info('MoveItPy instance created')
 
     # instantiate a RobotState instance using the current robot model
     robot_model = crane_x7.get_robot_model()
@@ -49,7 +49,7 @@ def main(args=None):
     # armのパラメータ設定用
     arm_plan_request_params = PlanRequestParameters(
         crane_x7,
-        "ompl_rrtc",
+        'ompl_rrtc',
     )
     arm_plan_request_params.max_acceleration_scaling_factor \
         = 1.0  # Set 0.0 ~ 1.0
@@ -59,16 +59,16 @@ def main(args=None):
     # gripperのパラメータ設定用
     gripper_plan_request_params = PlanRequestParameters(
         crane_x7,
-        "ompl_rrtc",
+        'ompl_rrtc',
     )
     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
 
-    # SRDFに定義されている"home"の姿勢にする
+    # SRDFに定義されている'home'の姿勢にする
     crane_x7_arm.set_start_state_to_current_state()
-    crane_x7_arm.set_goal_state(configuration_name="home")
+    crane_x7_arm.set_goal_state(configuration_name='home')
     plan_and_execute(
         crane_x7,
         crane_x7_arm,
@@ -77,7 +77,7 @@ def main(args=None):
     )
 
     # gripperを60[deg]に開く
-    robot_state.set_joint_group_positions("gripper", [math.radians(60)])
+    robot_state.set_joint_group_positions('gripper', [math.radians(60)])
     crane_x7_gripper.set_start_state_to_current_state()
     crane_x7_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -88,7 +88,7 @@ def main(args=None):
     )
 
     # gripperを0[deg]に閉じる
-    robot_state.set_joint_group_positions("gripper", [math.radians(0)])
+    robot_state.set_joint_group_positions('gripper', [math.radians(0)])
     crane_x7_gripper.set_start_state_to_current_state()
     crane_x7_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -99,7 +99,7 @@ def main(args=None):
     )
 
     # gripperを60[deg]に開く
-    robot_state.set_joint_group_positions("gripper", [math.radians(60)])
+    robot_state.set_joint_group_positions('gripper', [math.radians(60)])
     crane_x7_gripper.set_start_state_to_current_state()
     crane_x7_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -110,7 +110,7 @@ def main(args=None):
     )
 
     # gripperを0[deg]に閉じる
-    robot_state.set_joint_group_positions("gripper", [math.radians(0)])
+    robot_state.set_joint_group_positions('gripper', [math.radians(0)])
     crane_x7_gripper.set_start_state_to_current_state()
     crane_x7_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -127,5 +127,5 @@ def main(args=None):
     rclpy.shutdown()
 
 
-if __name__ == "__main__":
+if __name__ == '__main__':
     main()

crane_x7_examples_py/crane_x7_examples_py/pick_and_place_tf.py

@@ -0,0 +1,35 @@
+# 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 rclpy
+from geometry_msgs.msg import PoseStamped
+
+# generic ros libraries
+from rclpy.logging import get_logger
+
+# moveit python library
+from moveit.core.robot_state import RobotState
+from moveit.planning import (
+    MoveItPy,
+    PlanRequestParameters,
+)
+
+from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion
+
+
+
+if __name__ == "__main__":
+    main()