opencv param tune

krrish-jindal · Jan 18, 2024 · 1222ea2 · 1222ea2
1 parent f3122e2
commit 1222ea2
Show file tree

Hide file tree

Showing 4 changed files with 49 additions and 25 deletions.
diff --git a/pymoveit2/examples/new_hard.py b/pymoveit2/examples/new_hard.py
@@ -97,6 +97,18 @@ def __init__(self):
             3.15
         ],
     )
+
+    #         self.declare_parameter(
+    #     "joint_positions_positive_back",
+    #     [
+    #         1.57,
+    #         -2.39,
+    #         2.4,
+    #         -3.15,
+    #         -1.58,
+    #         3.15
+    #     ],
+    # )
         self.declare_parameter(
         "joint_positions_negative",
         [
@@ -232,7 +244,7 @@ def servo(self, box_no):
 
  #  ARM Move To BOX 
 
-                    if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.004 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.004 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.004:
+                    if round((box49.transform.translation.y) - (tool0.transform.translation.y),4) > 0.01 or round((box49.transform.translation.x) - (tool0.transform.translation.x),4) > 0.01 or round((box49.transform.translation.z) - (tool0.transform.translation.z),4) > 0.01:
                         __twist_msg = TwistStamped()
                         __twist_msg.header.stamp = self.get_clock().now().to_msg()
                         __twist_msg.header.frame_id = ur5.base_link_name()
@@ -253,7 +265,7 @@ def servo(self, box_no):
                         tool0 = self.tf_buffer.lookup_transform('base_link', "tool0", rclpy.time.Time())
                         roll , pitch , yaw  = euler_from_quaternion([box49.transform.rotation.x, box49.transform.rotation.y, box49.transform.rotation.z, box49.transform.rotation.w])
                         print(f"x = {round(tool0.transform.translation.x,2)}")
-                        print(f"y = {round(tool0.transform.translation.x,2)}")
+                        print(f"y = {round(tool0.transform.translation.y,2)}")
 
                     except Exception as e:
                         print(e)
@@ -297,14 +309,17 @@ def servo(self, box_no):
                         self.moveit2.wait_until_executed()
                         break
 
-                    elif round(yaw) ==((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14) or (round(yaw,2) >= -3.14 and round(yaw,2)<= -2.79)) and (round(tool0.transform.translation.y,2) > 0.25):
+                    elif round(yaw) ==((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14)) and (round(tool0.transform.translation.y,2) < 0.25):
+                        print("----------------")
                         __twist_msg = TwistStamped()
                         __twist_msg.header.stamp = self.get_clock().now().to_msg()
                         __twist_msg.header.frame_id = ur5.base_link_name()
                         __twist_msg.twist.linear.z = 0.1
                         __twist_msg.twist.linear.y = -0.2
                         self.twist_pub.publish(__twist_msg)
-                    elif round(yaw) == ((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14) or (round(yaw,2) >= -3.14 and round(yaw,2)<= -2.79)) and (round(tool0.transform.translation.y,2) <= 0.25):
+                    elif round(yaw) == ((round(yaw,2) >= 2.79 and round(yaw,2)<= 3.14)) and (round(tool0.transform.translation.y,2) >= 0.25):
+                        print("========")
+
                         self.trajactory()
                         self.moveit2.move_to_configuration(joint_positions_final_2)
                         self.moveit2.wait_until_executed()
@@ -388,4 +403,4 @@ def main():
             print(e)
 
 if __name__ == "__main__":
-	main()
+	main()
diff --git a/pymoveit2/examples/task4_without_scene.py b/pymoveit2/examples/task4_without_scene.py
@@ -1,4 +1,4 @@
-declare_parameter#!/usr/bin/env python3
+#!/usr/bin/env python3
 
 from os import path
 from threading import Thread

diff --git a/ur5_moveit/config/moveit.rviz b/ur5_moveit/config/moveit.rviz
@@ -6,8 +6,9 @@ Panels:
       Expanded:
         - /TF1
         - /TF1/Frames1
+        - /Image1
       Splitter Ratio: 0.5
-    Tree Height: 172
+    Tree Height: 117
   - Class: rviz_common/Help
     Name: Help
   - Class: rviz_common/Views
@@ -345,8 +346,6 @@ Visualization Manager:
         All Enabled: false
         base_link:
           Value: false
-        cam_49:
-          Value: false
         camera_bottom_screw_frame:
           Value: false
         camera_color_frame:
@@ -373,8 +372,6 @@ Visualization Manager:
           Value: false
         forearm_link:
           Value: false
-        obj_49:
-          Value: true
         shoulder_link:
           Value: false
         tool0:
@@ -398,8 +395,6 @@ Visualization Manager:
         world:
           base_link:
             camera_link:
-              cam_49:
-                {}
               camera_bottom_screw_frame:
                 camera_link_depth:
                   camera_depth_frame:
@@ -414,8 +409,6 @@ Visualization Manager:
                     camera_right_ir_frame:
                       camera_right_ir_optical_frame:
                         {}
-            obj_49:
-              {}
             shoulder_link:
               upper_arm_link:
                 forearm_link:
@@ -428,6 +421,20 @@ Visualization Manager:
                           {}
       Update Interval: 0
       Value: true
+    - Class: rviz_default_plugins/Image
+      Enabled: true
+      Max Value: 1
+      Median window: 5
+      Min Value: 0
+      Name: Image
+      Normalize Range: true
+      Topic:
+        Depth: 5
+        Durability Policy: Volatile
+        History Policy: Keep Last
+        Reliability Policy: Reliable
+        Value: /camera/color/image_raw
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -469,18 +476,20 @@ Visualization Manager:
 Window Geometry:
   Displays:
     collapsed: false
-  Height: 1012
+  Height: 1026
   Help:
     collapsed: false
   Hide Left Dock: false
   Hide Right Dock: false
+  Image:
+    collapsed: false
   MotionPlanning:
     collapsed: false
   MotionPlanning - Trajectory Slider:
     collapsed: false
-  QMainWindow State: 000000ff00000000fd0000000100000000000002b40000039afc0200000005fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d0000012f000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000172000001a20000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a00560069006500770073010000031a000000bd000000a400ffffff000004c60000039a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  QMainWindow State: 000000ff00000000fd0000000100000000000002b4000003a8fc0200000006fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb000000100044006900730070006c006100790073010000003d000000f8000000c900fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000013b000001d20000017d00fffffffb0000000800480065006c0070000000029a0000006e0000006e00fffffffb0000000a005600690065007700730000000290000000a4000000a400fffffffb0000000a0049006d0061006700650100000313000000d20000002800ffffff000004c6000003a800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
   Views:
     collapsed: false
   Width: 1920
   X: 0
-  Y: 38
+  Y: 28
diff --git a/ur_description/scripts/task1a.py b/ur_description/scripts/task1a.py
@@ -103,15 +103,15 @@ def detect_aruco(image, depth):
 
 #  IMAGE REFINEMENT
 
-			#arucoParams.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX  # Add this line
-			# arucoParams.cornerRefinementWinSize = 5  # You can adjust the window size if needed
+			arucoParams.cornerRefinementMethod = cv2.aruco.CORNER_REFINE_SUBPIX  # Add this line
+			arucoParams.cornerRefinementWinSize = 8  # You can adjust the window size if needed
 
 
-			brightness = 10 
-			contrast = 2.3  
+			brightness = 15 
+			contrast = 2.1  
 			image = cv2.addWeighted(image, contrast, np.zeros(image.shape, image.dtype), 0, brightness)
-			image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
-			image = cv2.medianBlur(image, 5)
+			# image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
+			# image = cv2.medianBlur(image, 5)
 			(corners, aruco_id, rejected) = cv2.aruco.detectMarkers(image, dictionary, parameters=arucoParams)
 			for i in range(len(aruco_id)):
 
@@ -281,7 +281,7 @@ def process_image(self):
 		focalY = 931.1829833984375
 
 		try:
-			center_list, distance_list, angle_list, width_list, ids,tvec = detect_aruco(self.cv_image, self.depth_image)
+			center_list, distance_list, angle_list, width_list, ids,tvec = detect_aruco(self.gray, self.depth_image)
 			rpy = []
 			list_rpy = []
 			# Add your image processing code here