code contoller updated update

configs/params.yaml

@@ -57,16 +57,16 @@ p1:
  - 0
  - 0
 p2:
- - 200
+ - 300
  - 0
 p3:
- - 50
+ - 10
  - 720
 p4:
  - 0
  - 720
 p5: 
- - 980
+ - 880
  - 0
 p6:
  - 1280
@@ -75,5 +75,5 @@ p7:
  - 1280
  - 720
 p8: 
- - 1180
+ - 1270
  - 720

scripts/controller.py

@@ -0,0 +1,68 @@
+# Class to compute the control parameters 
+import numpy as np
+
+# Function to wrap an angle to the interval [-pi,pi]
+def wrapToPi(theta):
+ while theta < -np.pi:
+ theta = theta + 2*np.pi
+ while theta > np.pi:
+ theta = theta - 2*np.pi
+ return theta
+
+
+# Function to compute the controls given the current state and the desired state and velocity
+def visualServoingCtl (camera, desiredState, actualState, v_des):
+ # specifiy the acutal state for better readability
+ x = actualState[0]
+ y = actualState[1]
+ theta = actualState[2]
+
+ # some crazy parameters 
+ lambda_x_1 = 10
+ lambda_w_1= 3000
+ lambdavec = np.array([lambda_x_1,lambda_w_1])
+
+ # state if it is a row or a column controller
+ controller_type = 0
+
+ # s_c = J * u = L_sc * T_r->c * u_r
+
+ # Computation of the Interaction Matrix as presented by Cherubini & Chaumette
+ # relates the control variables [v,w] in the camera frame to the change of the features [x,y,theta]
+ angle = camera.tilt_angle
+ delta_z = camera.deltaz
+ IntMat = np.array([[(-np.sin(angle)-y*np.cos(angle))/delta_z, 0, x*(np.sin(angle)+y*np.cos(angle))/delta_z, x*y, -1-x**2, y],
+ [0, -(np.sin(angle)+y*np.cos(angle))/delta_z, y*(np.sin(angle)+y*np.cos(angle))/delta_z, 1+y**2, -x*y, -x],
+ [np.cos(angle)*np.power(np.cos(theta),2)/delta_z, np.cos(angle)*np.cos(theta)*np.sin(theta)/delta_z, 
+ -(np.cos(angle)*np.cos(theta)*(y*np.sin(theta) + x*np.cos(theta)))/delta_z, 
+ -(y*np.sin(theta) + x*np.cos(theta))*np.cos(theta), -(y*np.sin(theta) + x*np.cos(theta))*np.sin(theta), -1]])
+
+ # Computation of the transformation from the robot to the camera frame
+ delta_y = camera.deltay
+ TransfMat = np.array([[0,-delta_y],
+ [-np.sin(angle),0],
+ [np.cos(angle),0],
+ [0,0],
+ [0,-np.cos(angle)],
+ [0,-np.sin(angle)]])
+ Trans_vel = TransfMat[:,0]
+ Trans_ang = TransfMat[:,1]
+
+ # Computation of the Jacobi-Matrix for velocity and angular velocity and their pseudo inverse
+ # The Jacobi-Matrix relates the control variables in the robot frame to the change of the features
+ Jac = np.array([IntMat[controller_type,:],IntMat[2,:]])
+ Jac_vel = np.matmul(Jac,Trans_vel)
+ # Jac_vel_pi = np.linalg.pinv([Jac_vel])
+ Jac_ang = np.matmul(Jac,Trans_ang)
+ Jac_ang_pi = np.linalg.pinv([Jac_ang])
+
+ # Compute the delta, in this case the difference between the actual state and the desired state
+ trans_delta = actualState[controller_type] - desiredState[controller_type]
+ ang_delta = actualState[2] - desiredState[2]
+ delta = np.array([trans_delta,wrapToPi(ang_delta)])
+
+ # Compute the feedback control for the angular velocity
+ temp = lambdavec * delta
+ ang_fb = np.matmul(-Jac_ang_pi.T,(temp + Jac_vel * v_des))
+ return ang_fb
+

scripts/imageProc.py

@@ -63,6 +63,9 @@ def __init__(self, windowProp, roiProp, fexProp):
 
  self.count = 0
 
+ self.pointsInTop = 0
+ self.pointsInBottom = 0
+
  # counter of newly detected rows in switching process
  self.newDetectedRows = 0
 
@@ -112,19 +115,19 @@ def initialize(self, bushy=False):
  else:
  print("#[INF] feEx - Searching for crop rows, please wait...")
  # steps create linspace
- winidx = np.linspace(self.winSweepStart,
+ cropRowWindows = np.linspace(self.winSweepStart,
  self.winSweepEnd, self.steps)
  # search for lines at given window locations using LQ-Adjustment
- lParams, winLoc, _ = self.getLinesAtWindow(winidx)
+ lParams, winLoc, _ = self.getLinesAtWindow(cropRowWindows)
  # if any feature is observed during the scan
  if len(lParams) != 0:
  np.set_printoptions(suppress=True)
  # compute moving standard deviation
- movVarM = movingStd(lParams[:, 1])
+ movVarM = movingStd(lParams[:, 0])
  # find positive an negative peaks of the signal
- peaksNeg, peaksPos = findPicks(movVarM, 0.5)
+ peaksPos, peaksNeg = findPicksTroughths(movVarM, 0.5)
  # locate best lines
- paramsBest, _ = self.findBestLines(peaksPos, peaksNeg,
+ paramsBest, _ = self.findCropRows(peaksPos, peaksNeg,
  movVarM, lParams, winLoc)
  # if there is any proper line to follow
  if len(paramsBest) != 0:
@@ -159,7 +162,7 @@ def initialize(self, bushy=False):
  '--- Initialisation failed - No lines detected by sweeping window ---')
  self.lineFound = False
 
- def findBestLines(self, peaksPos, peaksNeg, movVarM, lParams, winLoc):
+ def findCropRows(self, peaksPos, peaksNeg, movVarM, lParams, winLoc):
  # if multiple lines
  if len(peaksPos) != 0 and len(peaksNeg) != 0 and len(movVarM) != 0:
  # best line one each side of the crop row transition
@@ -216,29 +219,29 @@ def findBestLines(self, peaksPos, peaksNeg, movVarM, lParams, winLoc):
 
  return self.paramsBest, self.windowLocations
 
- def getLinesAtWindow(self, winidx):
+ def getLinesAtWindow(self, cropRowWindows):
  """function to get lines at given window locations 
  Args:
- winidx (_type_): _description_
+ cropRowWindows (_type_): _description_
  Returns:
  _type_: lines in image
  """
  # greenidx, contour center points
  self.contourCenters, x, y = self.getImageData()
 
  # initialization
- lParams = np.zeros((len(winidx), 2))
- winLoc = np.zeros((len(winidx), 1))
- winMean = np.zeros((len(winidx), 1))
- self.windowPoints = np.zeros((len(winidx), 6))
+ lParams = np.zeros((len(cropRowWindows), 2))
+ winLoc = np.zeros((len(cropRowWindows), 1))
+ winMean = np.zeros((len(cropRowWindows), 1))
+ self.windowPoints = np.zeros((len(cropRowWindows), 6))
  var = None
 
  # counting the points in the top and bottom half
- self.pointsB = 0
- self.pointsT = 0
+ self.pointsInBottom = 0
+ self.pointsInTop = 0
 
  # for all windows
- for i in range(len(winidx)):
+ for i in range(len(cropRowWindows)):
  # define window
  if self.isInitialized and len(self.linesTracked) != 0:
  # get x values, where the top and bottom of the window intersect
@@ -262,9 +265,9 @@ def getLinesAtWindow(self, winidx):
  xWinBR, self.imgHeight-yWinB, self.imgHeight-yWinT]
  else:
  # initial window positions
- # window is centered around winidx[i]
- xWinBL = winidx[i]
- xWinBR = winidx[i] + self.winSize
+ # window is centered around cropRowWindows[i]
+ xWinBL = cropRowWindows[i]
+ xWinBR = cropRowWindows[i] + self.winSize
  xWinTL = xWinBL
  xWinTR = xWinBR
  yWinB = self.offsB
@@ -274,9 +277,16 @@ def getLinesAtWindow(self, winidx):
  self.windowPoints[i, :] = [xWinBL, xWinTL, xWinTR,
  xWinBR, self.imgHeight-yWinB, self.imgHeight-yWinT]
 
- ptsIn = np.zeros((len(x), 2))
+ plantsInCropRow = np.zeros((len(x), 2))
  # get points inside window
+ self.pointsInTop = 0
+ self.pointsInBottom = 0
  for m in range(0, len(x)):
+ # checking #points in upper/lower half of the image
+ if y[m] < self.imgHeight/2:
+ self.pointsInTop += 1
+ else:
+ self.pointsInBottom += 1
  # different query needed, if the window is a parallelogram
  if self.isInitialized and len(self.linesTracked) != 0 and self.linesTracked is not None:
  # get the x value of the tracked line of the previous step
@@ -297,16 +307,18 @@ def getLinesAtWindow(self, winidx):
  else:
  ptInWin = x[m] > xWinBL and x[m] < xWinBR and y[m] > yWinB and y[m] < yWinT
 
- # if the point is inside the window, add it to ptsIn
+ # if the point is inside the window, add it to plantsInCropRow
  if ptInWin:
- ptsIn[m, :] = self.contourCenters[:, m]
+ plantsInCropRow[m, :] = self.contourCenters[:, m]
 
  # remove zero rows
- ptsIn = ptsIn[~np.all(ptsIn == 0, axis=1)]
+ plantsInCropRow = plantsInCropRow[~np.all(plantsInCropRow == 0, axis=1)]
+ print(len(plantsInCropRow))
+
  # line fit
- if len(ptsIn) > 2:
+ if len(plantsInCropRow) > 2:
  # flipped points
- ptsFlip = np.flip(ptsIn, axis=1)
+ ptsFlip = np.flip(plantsInCropRow, axis=1)
  # get line at scanning window
  xM, xB = getLineRphi(ptsFlip)
  t_i, b_i = lineIntersectIMG(xM, xB, self.imgHeight)
@@ -318,9 +330,9 @@ def getLinesAtWindow(self, winidx):
  # store the window location, which generated these line
  # parameters
  if self.isInitialized == False:
- winLoc[i] = winidx[i]
+ winLoc[i] = cropRowWindows[i]
  # mean x value of all points used for fitting the line
- winMean[i] = np.median(ptsIn, axis=0)[0]
+ winMean[i] = np.median(plantsInCropRow, axis=0)[0]
 
  # if the feature extractor is initalized, adjust the window
  # size with the variance of the line fit

scripts/movingVariance.py

@@ -26,9 +26,9 @@ def movingStd(data, winSize=5):
  return stdVec
 
 
-def findPicks(movVarM, prominence=0.5):
+def findPicksTroughths(movVarM, prominence=0.5):
  # find negative peaks (=least std, most stable line)
  peaksNeg, _ = find_peaks(-movVarM)
  # find positive peaks (=least stable lines = crop row transition)
  peaksPos, _ = find_peaks(movVarM, prominence=0.5)
- return peaksNeg, peaksPos
+ return peaksPos, peaksNeg

scripts/vs_nodeHandler.py

@@ -56,6 +56,7 @@ def __init__(self):
  self.minOmega = rospy.get_param('minOmega')
  self.maxLinearVel = rospy.get_param('maxLinearVel')
  self.minLinearVel = rospy.get_param('minLinearVel')
+ self.rotationDir = 1
  # direction of motion 1: forward, -1:backward
  if self.navigationMode == 1 or self.navigationMode == 2:
  self.linearMotionDir = 1