CI対応

rt-net · Oct 31, 2024 · 8ab3822 · 8ab3822
1 parent 057338c
commit 8ab3822
Show file tree

Hide file tree

Showing 7 changed files with 109 additions and 80 deletions.
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
@@ -12,16 +12,16 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
+from crane_plus_examples_py.utils import rotation_matrix_to_quaternion
 import cv2
 from cv2 import aruco
 from cv_bridge import CvBridge
-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
+import tf2_ros
 
 
 class ImageSubscriber(Node):

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
@@ -24,14 +24,16 @@
 
 class ImageSubscriber(Node):
     def __init__(self):
-        super().__init__("color_detection")
+        super().__init__('color_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.image_thresholded_publisher = self.create_publisher(Image, 'image_thresholded',  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()
@@ -50,25 +52,35 @@ def image_callback(self, msg):
             HIGH_V = 255
 
             # ウェブカメラの画像を受け取る
-            cv_img = self.bridge.imgmsg_to_cv2(msg, desired_encoding=msg.encoding)
+            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_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_OPEN, kernel
+                )
 
             # 穴埋めの処理
             kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (5, 5))
-            img_thresholded = cv2.morphologyEx(img_thresholded, cv2.MORPH_CLOSE, kernel)
+            img_thresholded = cv2.morphologyEx(
+                img_thresholded, cv2.MORPH_CLOSE, kernel
+                )
 
             # 画像の検出領域におけるモーメントを計算
             moment = cv2.moments(img_thresholded)
@@ -106,14 +118,16 @@ 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):

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
@@ -13,6 +13,7 @@
 # limitations under the License.
 
 from crane_plus_examples_py.utils import plan_and_execute
+
 import math
 
 # moveit python library

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
@@ -12,9 +12,10 @@
 # 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
 
+from crane_plus_examples_py.utils import plan_and_execute
+
 # moveit python library
 from moveit.core.robot_state import RobotState
 from moveit.planning import (

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
@@ -12,36 +12,32 @@
 # See the License for the specific language governing permissions and
 # limitations under the License.
 
-import math
-
-import rclpy
+from crane_plus_examples_py.utils import plan_and_execute, euler_to_quaternion
 from geometry_msgs.msg import PoseStamped
-
-# generic ros libraries
-from rclpy.logging import get_logger
+import math
 
 # 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
+import rclpy
+from rclpy.logging import get_logger
 
 
 def main(args=None):
     # ros2の初期化
     rclpy.init(args=args)
 
     # ロガー生成
-    logger = get_logger("pick_and_place")
+    logger = get_logger('pick_and_place')
 
     # MoveItPy初期化
-    crane_plus = MoveItPy(node_name="moveit_py")
-    crane_plus_arm = crane_plus.get_planning_component("arm")
-    crane_plus_gripper = crane_plus.get_planning_component("gripper")
-    logger.info("MoveItPy instance created")
+    crane_plus = MoveItPy(node_name='moveit_py')
+    crane_plus_arm = crane_plus.get_planning_component('arm')
+    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()
@@ -51,27 +47,35 @@ def main(args=None):
     # armのパラメータ設定用
     arm_plan_request_params = PlanRequestParameters(
         crane_plus,
-        "ompl_rrtc",
+        '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
+
+    # Set 0.0 ~ 1.0
+    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
 
     # gripperのパラメータ設定用
     gripper_plan_request_params = PlanRequestParameters(
         crane_plus,
-        "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
+
+    # 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  
 
     # gripperの開閉角度
     GRIPPER_DEFAULT = 0.0
     GRIPPER_OPEN = math.radians(-30.0)
     GRIPPER_CLOSE = math.radians(10.0)
 
-    # armを現在の位置から"vertical"ポジションに動かす
+    # armを現在の位置から'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,
@@ -80,7 +84,7 @@ def main(args=None):
     )
 
     # gripperを0度の位置に動かす
-    robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT])
+    robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT])
     crane_plus_gripper.set_start_state_to_current_state()
     crane_plus_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -89,11 +93,11 @@ def main(args=None):
         logger,
         single_plan_parameters=gripper_plan_request_params,
     )
-    
+
     # ----- Picking Preparation -----
-    # armを現在の位置から"home"ポジションに動かす
+    # armを現在の位置から'home'ポジションに動かす
     crane_plus_arm.set_start_state_to_current_state()
-    crane_plus_arm.set_goal_state(configuration_name="home")
+    crane_plus_arm.set_goal_state(configuration_name='home')
     plan_and_execute(
         crane_plus,
         crane_plus_arm,
@@ -102,7 +106,7 @@ def main(args=None):
     )
 
     # なにか掴んでいたときのためにgripperを開く
-    robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN])
+    robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN])
     crane_plus_gripper.set_start_state_to_current_state()
     crane_plus_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -114,8 +118,8 @@ def main(args=None):
 
     # armが掴みに行く位置を指定して動かす
     pose_goal = PoseStamped()
-    pose_goal.header.frame_id = "crane_plus_base"
-    
+    pose_goal.header.frame_id = 'crane_plus_base'
+
     pose_goal.pose.position.x = 0.0
     pose_goal.pose.position.y = -0.09
     pose_goal.pose.position.z = 0.17
@@ -124,10 +128,10 @@ def main(args=None):
     pose_goal.pose.orientation.y = q[1]
     pose_goal.pose.orientation.z = q[2]
     pose_goal.pose.orientation.w = q[3]
-       
+
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(
-        pose_stamped_msg=pose_goal, pose_link="crane_plus_link4"
+        pose_stamped_msg=pose_goal, pose_link='crane_plus_link4'
     )
     plan_and_execute(
         crane_plus,
@@ -144,7 +148,7 @@ def main(args=None):
     pose_goal.pose.orientation.w = q[3]
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(
-        pose_stamped_msg=pose_goal, pose_link="crane_plus_link4"
+        pose_stamped_msg=pose_goal, pose_link='crane_plus_link4'
     )
     plan_and_execute(
         crane_plus,
@@ -157,7 +161,7 @@ def main(args=None):
     pose_goal.pose.position.z = 0.14
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(
-        pose_stamped_msg=pose_goal, pose_link="crane_plus_link4"
+        pose_stamped_msg=pose_goal, pose_link='crane_plus_link4'
     )
     plan_and_execute(
         crane_plus,
@@ -167,7 +171,7 @@ def main(args=None):
     )
 
     # Grasp
-    robot_state.set_joint_group_positions("gripper", [GRIPPER_CLOSE])
+    robot_state.set_joint_group_positions('gripper', [GRIPPER_CLOSE])
     crane_plus_gripper.set_start_state_to_current_state()
     crane_plus_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -176,12 +180,12 @@ def main(args=None):
         logger,
         single_plan_parameters=gripper_plan_request_params,
     )
-    
+
     # armをz軸上に動かす
     pose_goal.pose.position.z = 0.17
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(
-        pose_stamped_msg=pose_goal, pose_link="crane_plus_link4"
+        pose_stamped_msg=pose_goal, pose_link='crane_plus_link4'
     )
     plan_and_execute(
         crane_plus,
@@ -191,9 +195,9 @@ def main(args=None):
     )
 
     # ----- Placing Preparation -----
-    # armを現在の位置から"home"ポジションに動かす
+    # armを現在の位置から'home'ポジションに動かす
     crane_plus_arm.set_start_state_to_current_state()
-    crane_plus_arm.set_goal_state(configuration_name="home")
+    crane_plus_arm.set_goal_state(configuration_name='home')
     plan_and_execute(
         crane_plus,
         crane_plus_arm,
@@ -212,7 +216,7 @@ def main(args=None):
     pose_goal.pose.orientation.w = q[3]
     crane_plus_arm.set_start_state_to_current_state()
     crane_plus_arm.set_goal_state(
-        pose_stamped_msg=pose_goal, pose_link="crane_plus_link4"
+        pose_stamped_msg=pose_goal, pose_link='crane_plus_link4'
     )
     plan_and_execute(
         crane_plus,
@@ -222,7 +226,7 @@ def main(args=None):
     )
 
     # Release
-    robot_state.set_joint_group_positions("gripper", [GRIPPER_OPEN])
+    robot_state.set_joint_group_positions('gripper', [GRIPPER_OPEN])
     crane_plus_gripper.set_start_state_to_current_state()
     crane_plus_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -233,19 +237,19 @@ def main(args=None):
     )
 
     # Return to home and vertical pose
-    # armを現在の位置から"home"ポジションに動かす
+    # armを現在の位置から'home'ポジションに動かす
     crane_plus_arm.set_start_state_to_current_state()
-    crane_plus_arm.set_goal_state(configuration_name="home")
+    crane_plus_arm.set_goal_state(configuration_name='home')
     plan_and_execute(
         crane_plus,
         crane_plus_arm,
         logger,
         single_plan_parameters=arm_plan_request_params,
     )
 
-    # armを現在の位置から"vertical"ポジションに動かす
+    # armを現在の位置から'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,
@@ -254,7 +258,7 @@ def main(args=None):
     )
 
     # gripperの開閉をデフォルトの間隔に戻す
-    robot_state.set_joint_group_positions("gripper", [GRIPPER_DEFAULT])
+    robot_state.set_joint_group_positions('gripper', [GRIPPER_DEFAULT])
     crane_plus_gripper.set_start_state_to_current_state()
     crane_plus_gripper.set_goal_state(robot_state=robot_state)
     plan_and_execute(
@@ -263,7 +267,7 @@ def main(args=None):
         logger,
         single_plan_parameters=gripper_plan_request_params,
     )
-    
+
     # MoveItPyの終了
     crane_plus.shutdown()