robot_vision.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "id": "5e85df40-7090-4fb8-9a32-a783196a0fa2",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Standard imports\n",
    "import cv2\n",
    "import numpy as np;\n",
    "import matplotlib.pyplot as plt\n",
    "import PIL.Image as Image\n",
    "from PIL import Image, ImageOps"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "0569337d-de23-4a81-9e50-6eeddcb80500",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Setup SimpleBlobDetector parameters.\n",
    "params = cv2.SimpleBlobDetector_Params()\n",
    " \n",
    "# Change thresholds\n",
    "params.minThreshold = 10;\n",
    "params.maxThreshold = 200;\n",
    " \n",
    "# Filter by Area.\n",
    "params.filterByArea = True\n",
    "params.minArea = 400\n",
    " \n",
    "# Filter by Circularity\n",
    "params.filterByCircularity = True\n",
    "params.minCircularity = 0.3\n",
    " \n",
    "# Filter by Convexity\n",
    "params.filterByConvexity = False\n",
    "params.minConvexity = 0.1\n",
    " \n",
    "# Filter by Inertia\n",
    "params.filterByInertia = False\n",
    "\n",
    "params.minInertiaRatio = 0.05\n",
    " \n",
    "detector = cv2.SimpleBlobDetector_create(params)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "1143bc27",
   "metadata": {},
   "outputs": [],
   "source": [
    "import cv2\n",
    "import os\n",
    "import numpy as np\n",
    "\n",
    "# define a video capture object\n",
    "cam = cv2.VideoCapture(1, cv2.CAP_DSHOW)\n",
    "\n",
    "\n",
    "\n",
    "try:\n",
    "    if not os.path.exists('Shranjeni frame-i'): #pogleda če obstaja ta mapa\n",
    "        os.makedirs('Shranjeni frame-i') #naredi novo mapo\n",
    "  \n",
    "except OSError:\n",
    "    print ('Error: Creating directory of data') #error, če ni možno naarediti mape \n",
    "\n",
    "\n",
    "\n",
    "def slikeIzVidea(st_slik, frekvenca):\n",
    "\n",
    "    currentframe = 0\n",
    "    shranjene_slike = 0\n",
    "\n",
    "    while (True):\n",
    "\n",
    "        ret, frame = cam.read()\n",
    "        cv2.imshow('frame', frame) #prikaži trenutni frame (prikaz kamere)\n",
    "        if cv2.waitKey(1) & 0xFF == ord('q'): #gumb q ustavi\n",
    "            break\n",
    "        if ret:\n",
    "            if not (currentframe % frekvenca):\n",
    "                name = './Shranjeni frame-i/frame' + str(currentframe) + '.jpg'\n",
    "                print ('Creating...' + name)\n",
    "                #matrika = np.array(frame)\n",
    "                #matrika[:,:,2] = np.zeros([matrika.shape[0], matrika.shape[1]]) #izbriše vse rdeče kanale\n",
    "                #rdeca = Image.fromarray(matrika)\n",
    "                cv2.imwrite(name, frame)\n",
    "                shranjene_slike += 1\n",
    "                if shranjene_slike == st_slik:\n",
    "                    break\n",
    "\n",
    "            currentframe += 1\n",
    "        else:\n",
    "            break\n",
    "        \n",
    "    cam.release()\n",
    "    cv2.destroyAllWindows()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "id": "7e3c0b41",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Creating..../Shranjeni frame-i/frame0.jpg\n",
      "Creating..../Shranjeni frame-i/frame1.jpg\n"
     ]
    }
   ],
   "source": [
    "slikeIzVidea(2,1) #(ŠTEVILO SLIK, KOLIKO FRAMOV NARAZEN (FREKVENCA))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "d1b533ce-127b-469a-b0cc-5998fe709ad6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# Read image\n",
    "im = cv2.imread(\"Shranjeni frame-i/frame0.jpg\")\n",
    "#cv2.imshow(\"im\",cv2.imread(\"Shranjeni frame-i/frame0.jpg\"))\n",
    "\n",
    "alpha = 3 # Contrast control (1.0-3.0) #3 najde kr vredu\n",
    "beta = 25 # Brightness control (0-100) #25 nqjde kr vredu\n",
    "\n",
    "adjusted = cv2.convertScaleAbs(im, alpha=alpha, beta=beta)\n",
    "im = adjusted\n",
    "cv2.imshow(\"im\",im)\n",
    "\n",
    "cv2.waitKey(0)\n",
    "im1=im\n",
    "hsv = cv2.cvtColor(im, cv2.COLOR_BGR2HSV)\n",
    "lower_red = np.array([0,75,200])\n",
    "upper_red = np.array([255,255,255])\n",
    "    \n",
    "mask = cv2.inRange(hsv, lower_red, upper_red)\n",
    "\n",
    "x_slika = []\n",
    "y_slika = []\n",
    "\n",
    "#cv2.imshow('mask',mask)\n",
    "\n",
    "#print(mask)\n",
    "#cv2.waitKey(0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "id": "71d96be7-969e-4c95-9f5d-962cf6cb1a8c",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "335.18719482421875\n",
      "236.12344360351562\n",
      "296.4700927734375\n",
      "236.61827087402344\n"
     ]
    },
    {
     "data": {
      "text/plain": [
       "-1"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "#im = cv2.imread(\"./blob.jpg\", cv2.IMREAD_GRAYSCALE)\n",
    "im = cv2.bitwise_not(mask)\n",
    "#im=mask\n",
    "# Set up the detector with default parameters.\n",
    "detector = cv2.SimpleBlobDetector_create(params)\n",
    "#rgb = cv2.cvtColor(im, cv2.COLOR_HSV2RGB)\n",
    "#gray = cv2.cvtColor(im, cv2.COLOR_RGB2GRAY)\n",
    " \n",
    "# Detect blobs.\n",
    "keypoints = detector.detect(im)\n",
    " \n",
    "# Draw detected blobs as red circles.\n",
    "# cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS ensures the size of the circle corresponds to the size of blob\n",
    "im_with_keypoints = cv2.drawKeypoints(im, keypoints, np.array([]), (0,0,255), cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)\n",
    "#print(keypoints)\n",
    "# Show keypoints\n",
    "stevnik = 0\n",
    "for keyPoint in keypoints:\n",
    "    x = keyPoint.pt[0]\n",
    "    y = keyPoint.pt[1]\n",
    "    s = keyPoint.size\n",
    "    if stevnik == 0:\n",
    "        x_ = keyPoint.pt[0]\n",
    "        y_ = keyPoint.pt[1]\n",
    "        stevnik = 1\n",
    "        \n",
    "    x_slika.append(x)\n",
    "    y_slika.append(y)\n",
    "        \n",
    "    print(x)\n",
    "    print(y)\n",
    "    #print(s)\n",
    "\n",
    "\n",
    "cv2.imshow(\"Keypoints\", im_with_keypoints)\n",
    "cv2.waitKey(0)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "id": "0342c444",
   "metadata": {},
   "source": [
    "Pretvorim v pixle na robotu"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "161608c0",
   "metadata": {},
   "outputs": [],
   "source": [
    "x_ref = 432\n",
    "y_ref = 330\n",
    "\n",
    "x_rob = []\n",
    "y_rob = []\n",
    "\n",
    "for keyPoint in keypoints:\n",
    "    x_vmesni = x_ref - keyPoint.pt[0]\n",
    "    y_vmesni = y_ref - keyPoint.pt[1]\n",
    "    x_rob.append(y_vmesni)\n",
    "    y_rob.append(x_vmesni)\n",
    "\n",
    "\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "id": "10a815ec",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[147.38619354248047, 146.60931472778321]\n",
      "[151.99610412597656, 212.78195434570313]\n"
     ]
    }
   ],
   "source": [
    "img_camera = np.ones_like(im)\n",
    "img_camera = img_camera*255\n",
    "\n",
    "\n",
    "x_rob_pixli = x_rob.copy()\n",
    "y_rob_pixli = y_rob.copy()\n",
    "\n",
    "skalirni_faktor = 1.57\n",
    "\n",
    "x_rob_mm = []\n",
    "y_rob_mm = []\n",
    "for i in range(len(x_rob_pixli)):\n",
    "    x = x_rob_pixli[i]*skalirni_faktor\n",
    "    y = y_rob_pixli[i]*skalirni_faktor\n",
    "    x_rob_mm.append(x)\n",
    "    y_rob_mm.append(y)\n",
    "\n",
    "print(x_rob_mm)\n",
    "print(y_rob_mm)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f1d91df8",
   "metadata": {},
   "source": [
    "cmd /k \"curl \"192.167.1.20/KAREL/setprc?str_coord1=10.0&str_coord2=20.0&str_coord3=0.0&str_coord4=0.0&str_coord5=0.0&str_coord6=0.0\"\""
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "id": "2491bced",
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "x=30.5\n",
    "y=15.5\n",
    "def write_to_cmd(x, y):\n",
    "    set_str = f'cmd /k curl \"192.167.1.20/KAREL/setprc?str_coord1={x}&str_coord2={y}&str_coord3=0.0&str_coord4=0.0&str_coord5=0.0&str_coord6=0.0\".format(x,y)'\n",
    "    os.system(set_str.format(x,y))\n",
    "    print(set_str)\n",
    "    return"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "0fa60e29",
   "metadata": {},
   "outputs": [],
   "source": [
    "write_to_cmd(x_rob_mm[0], y_rob_mm[0])"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.5 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.5"
  },
  "vscode": {
   "interpreter": {
    "hash": "0b8715b07a385016bcb385e29bdbb43cfb57d873c4ee32bd9d120899d08d07fb"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}