Get position

controllers/annotation_keypoints/annotation_keypoints.py

@@ -10,28 +10,28 @@
 deviceImagePath = os.getcwd()
 #robot = Robot()
 supervisor = Supervisor()
-timestep = int(supervisor.getBasicTimeStep()*10)
+timestep = int(supervisor.getBasicTimeStep())
 camera = supervisor.getDevice('camera_sensor')
 camera.enable(timestep)
 camera.recognitionEnable(timestep)
 camera.enableRecognitionSegmentation()
 number = 0
 
-print("hasRecognition(): " + str(camera.hasRecognition()))
-print("hasRecognitionSegmentation(): " + str(camera.hasRecognitionSegmentation()))
+#print("hasRecognition(): " + str(camera.hasRecognition()))
+#print("hasRecognitionSegmentation(): " + str(camera.hasRecognitionSegmentation()))
 
-cv2.startWindowThread()
-cv2.namedWindow("preview")
+#cv2.startWindowThread()
+#cv2.namedWindow("preview")
 
 color_dict = { \
-    "ball_color"    :(0.0, 0.0, 1.0), \
+    #"ball_color"    :(0.0, 0.0, 1.0), \
     "colorHead"     :(1.0, 1.0, 1.0), \
-    "colorNeck"     :(0.5, 0.5, 0.5), \
-    "colorShoulderL":(0.0, 0.5, 0.0), \
-    "colorShoulderR":(0.5, 0.0, 0.0), \
-    "colorElbowL"   :(0.0, 0.6, 0.0), \
-    "colorElbowR"   :(0.6, 0.0, 0.0), \
-    "colorWristL"   :(0.0, 0.7, 0.0), \
+#    "colorNeck"     :(0.5, 0.5, 0.5), \
+    "colorShoulderL":(0.0, 0.499, 0.0), \
+    "colorShoulderR":(0.0, 0.0, 0.499), \
+    "colorElbowL"   :(0.0, 0.599, 0.0), \
+    "colorElbowR"   :(0.599, 0.0, 0.0), \
+    "colorWristL"   :(0.0, 0.0, 1.0), \
     "colorWristR"   :(0.7, 0.0, 0.0), \
     "colorHipL"     :(0.0, 0.8, 0.0), \
     "colorHipR"     :(0.8, 0.0, 0.0), \
@@ -40,48 +40,96 @@
     "colorAnkleL"   :(0.0, 1.0, 0.0), \
     "colorAnkleR"   :(1.0, 0.0, 0.0) \
 }
-pos_dict = dict()
+#pos_dict = dict()
 lines = [
-  ["colorHead"     , "colorNeck"     ],
-  ["colorNeck"     , "colorShoulderL"],
-  ["colorShoulderL", "colorElbowL"   ],
-  ["colorElbowL"   , "colorWristL"   ],
-  ["colorNeck"     , "colorShoulderR"],
-  ["colorShoulderR", "colorElbowR"   ],
-  ["colorElbowR"   , "colorWristR"   ],
-  ["colorNeck"     , "colorHipL"     ],
-  ["colorHipL"     , "colorKneeL"    ],
-  ["colorKneeL"    , "colorAnkleL"   ],
-  ["colorNeck"     , "colorHipR"     ],
-  ["colorHipR"     , "colorKneeR"    ],
-  ["colorKneeR"    , "colorAnkleR"   ]
+    ["colorHead"     , "colorNeck"     ],
+    ["colorNeck"     , "colorShoulderL"],
+    ["colorShoulderL", "colorElbowL"   ],
+    ["colorElbowL"   , "colorWristL"   ],
+    ["colorNeck"     , "colorShoulderR"],
+    ["colorShoulderR", "colorElbowR"   ],
+    ["colorElbowR"   , "colorWristR"   ],
+    ["colorNeck"     , "colorHipL"     ],
+    ["colorHipL"     , "colorKneeL"    ],
+    ["colorKneeL"    , "colorAnkleL"   ],
+    ["colorNeck"     , "colorHipR"     ],
+    ["colorHipR"     , "colorKneeR"    ],
+    ["colorKneeR"    , "colorAnkleR"   ]
 ]
 
+
+memomemo_x = 0
+memomemo_y = 0
+memo_x = 0
+memo_y = 0
+cnt_2 = -1 
+
 while supervisor.step(timestep) != -1:
-    supervisor.getFromDef('BALL').getField('translation').setSFVec3f([random.uniform(0.0, 4.0), random.uniform(-1.0, 1.0), 0.1])
-    supervisor.getFromDef('ENEMY1').getField('rotation').setSFRotation([0, 0, 1, random.uniform(-1.0, 1.0)+3.14])
-    for i in range(10):
-        supervisor.step(timestep)
-    camera.saveImage(deviceImagePath + '/images/image' + str(number) + '.jpg', 80)
-    camera.saveRecognitionSegmentationImage(deviceImagePath + '/images/segmentation_image' + str(number) + '.jpg', 80)
+    # supervisor.getFromDef('BALL').getField('translation').setSFVec3f([random.uniform(0.0, 4.0), random.uniform(-1.0, 1.0), 0.1])
+    # supervisor.getFromDef('ENEMY1').getField('rotation').setSFRotation([0, 0, 1, random.uniform(-1.0, 1.0)+3.14])
+#    for i in range(10):
+#        supervisor.step(timestep)
+    camera.saveImage(deviceImagePath + '/images_new/image' + str(number) + '.jpg', 80)
+    #camera.saveRecognitionSegmentationImage(deviceImagePath + '/images/segmentation_image' + str(number) + '.jpg', 80)
     number += 1
     seg_img = camera.getRecognitionSegmentationImage()
     img = np.frombuffer(seg_img, np.uint8).reshape((camera.getHeight(), camera.getWidth(), 4))
 
+    cnt = -1
+    file_path_2d = 'txt_2d_use/image' + str(cnt_2) + '.txt'
+    file_path_3d = 'image_txt.txt'
+
+
     for key, val in color_dict.items():
+        cnt +=1
+        cnt_2 +=1
         color = (val[0]*255, val[1]*255, val[2]*255, 255)
         area = cv2.inRange(img, color, color)
-        x, y, width, height = cv2.boundingRect(area)
+        x, y, w, h = cv2.boundingRect(area)
         #cv2.rectangle(img, (x, y), (x + width, y + height), color=color, thickness=2)
-        pos_dict[key] = (x + width / 2, y + height / 2)
-        print(key + ": " + str(x) + ", y: " + str(y) + ", with: " + str(width) + ", height: " + str(height))
+        #pos_dict[key] = (x + width / 2, y + height / 2)
+        #print(key + ": " + str(x) + ", y: " + str(y) + ", with: " + str(width) + ", height: " + str(height))
+        #print(key + ": " + str(x) + ", y: " + str(y))
+        x = x + (w-1)/2
+        y = y + (h-1)/2
+        if x == -0.5 and y == -0.5: 
+            x = memomemo_x 
+            y = memomemo_y
+            y += 20
+
+        with open(file_path_2d, 'a') as f:
+            f.write("%s" % int(x))
+            f.write(',')
+            f.write("%s" % int(y))
+#            if int(x)!=0 or int(y)!=0:
+            f.write(',2\n')
+
+        with open(file_path_3d, 'a') as f:
+            f.write("%s" % int(x))
+            f.write(',')
+            f.write("%s" % int(y))
+            if cnt < 12:  
+                f.write(',')
+            else:
+                f.write('\n')
+
+#            if int(x)!=0 or int(y)!=0:
+#            else:
+#                f.write(',0\n')
+        memomemo_x = memo_x
+        memomemo_y = memo_y
+        memo_x = x
+        memo_y = y
+#    if cnt >= 600: 
+#        print("anno_finish\n")
+#        sys.exit()
 
-    for line in lines:
-        x0, y0 = int(pos_dict[line[0]][0]), int(pos_dict[line[0]][1])
-        x1, y1 = int(pos_dict[line[1]][0]), int(pos_dict[line[1]][1])
-        print("x0: "+str(x0)+", y0: "+str(y0)+", x1: "+str(x1)+", y1: "+str(y1)+"\r\n")
-        cv2.line(img, (x0, y0), (x1, y1), color=(255, 255, 255, 255), thickness=2)
+    # for line in lines:
+    #     x0, y0 = int(pos_dict[line[0]][0]), int(pos_dict[line[0]][1])
+    #     x1, y1 = int(pos_dict[line[1]][0]), int(pos_dict[line[1]][1])
+    #     print("x0: "+str(x0)+", y0: "+str(y0)+", x1: "+str(x1)+", y1: "+str(y1)+"\r\n")
+    #     cv2.line(img, (x0, y0), (x1, y1), color=(255, 255, 255, 255), thickness=2)
 
-    cv2.imshow("preview", img)
-    cv2.waitKey(timestep)
+    # cv2.imshow("preview", img)
+    # cv2.waitKey(timestep)
 

controllers/get_position/.vscode/settings.json

@@ -0,0 +1,68 @@
+{
+	"files.associations": {
+		"iomanip": "cpp",
+		"cctype": "cpp",
+		"clocale": "cpp",
+		"cmath": "cpp",
+		"cstdarg": "cpp",
+		"cstddef": "cpp",
+		"cstdio": "cpp",
+		"cstdlib": "cpp",
+		"cstring": "cpp",
+		"ctime": "cpp",
+		"cwchar": "cpp",
+		"cwctype": "cpp",
+		"array": "cpp",
+		"atomic": "cpp",
+		"hash_map": "cpp",
+		"hash_set": "cpp",
+		"strstream": "cpp",
+		"*.tcc": "cpp",
+		"bitset": "cpp",
+		"chrono": "cpp",
+		"codecvt": "cpp",
+		"complex": "cpp",
+		"condition_variable": "cpp",
+		"cstdint": "cpp",
+		"deque": "cpp",
+		"list": "cpp",
+		"unordered_map": "cpp",
+		"unordered_set": "cpp",
+		"vector": "cpp",
+		"exception": "cpp",
+		"algorithm": "cpp",
+		"filesystem": "cpp",
+		"functional": "cpp",
+		"iterator": "cpp",
+		"map": "cpp",
+		"memory": "cpp",
+		"memory_resource": "cpp",
+		"numeric": "cpp",
+		"optional": "cpp",
+		"random": "cpp",
+		"ratio": "cpp",
+		"set": "cpp",
+		"string": "cpp",
+		"string_view": "cpp",
+		"system_error": "cpp",
+		"tuple": "cpp",
+		"type_traits": "cpp",
+		"utility": "cpp",
+		"fstream": "cpp",
+		"initializer_list": "cpp",
+		"iosfwd": "cpp",
+		"iostream": "cpp",
+		"istream": "cpp",
+		"limits": "cpp",
+		"mutex": "cpp",
+		"new": "cpp",
+		"ostream": "cpp",
+		"sstream": "cpp",
+		"stdexcept": "cpp",
+		"streambuf": "cpp",
+		"thread": "cpp",
+		"cfenv": "cpp",
+		"cinttypes": "cpp",
+		"typeinfo": "cpp"
+	}
+}