Merge branch 'main' of https://github.com/Agricultural-Robotics-Bonn/…

…visual-multi-crop-row-navigation into main

configs/params.yaml

@@ -18,7 +18,7 @@ cmd_vel_topic: /phenobot/twist_mux/cmd_vel
 update rate: 30
 queue_size: 5
 
-img_resize_scale: 100
+imgResizeRatio: 100
 
 stationaryDebug: False
 # run time params

scripts/featureExtractor.py

@@ -7,7 +7,6 @@
 from matplotlib import pyplot as plt
 from movingVariance import movingStd
 import itertools
-
 class featureExtractor:
  def __init__(self, windowProp, roiProp, fexProp):
  """# class FeatureExtractor to extract the the line Features: bottom point, angle
@@ -59,6 +58,8 @@ def __init__(self, windowProp, roiProp, fexProp):
 
  self.count = 0
 
+ self.smoothSize = 5
+
  self.numVec= np.zeros((300,1))
  self.meanVec = np.zeros((300,1))
 
@@ -744,7 +745,6 @@ def handleKey(self, sleepTime=0):
  if key == ord('q'):
  cv.destroyAllWindows()
  exit(1)
-
  # Function to compute the features of the line which has to be recognized
  def detectTrackingFeatures(self, mode):
  print("#[INF] detect Tracking Features")
@@ -858,8 +858,7 @@ def matchTrackingFeatures(self, mode):
  return False, rosIMG
  else:
  print("No row passed, continuing")
- return False, rosIMG
-
+ return False, rosIMG 
  # Function to compute the green index of the image
  def getGreenIDX(self):
  imgInt32 = self.processedIMG.astype('int32')

scripts/vs_nodeHandler.py

@@ -40,9 +40,8 @@ def __init__(self):
 
  # cv bridge
  self.bridge = CvBridge()
-
- self.img_resize_scale = rospy.get_param('img_resize_scale')
-
+ # input image resize ratio
+ self.imgResizeRatio = rospy.get_param('imgResizeRatio')
  # settings
  # Mode 1: Driving forward with front camera (starting mode)
  # Mode 2: Driving forward with back camera
@@ -51,7 +50,7 @@ def __init__(self):
  self.navigationMode = rospy.get_param('navigationMode') 
  # debug mode without publishing velocities 
  self.stationaryDebug = rospy.get_param('stationaryDebug')
- #  speed limits
+ # speed limits
  self.omegaScaler = rospy.get_param('omegaScaler')
  self.maxOmega = rospy.get_param('maxOmega')
  self.minOmega = rospy.get_param('minOmega')
@@ -73,11 +72,15 @@ def __init__(self):
  # buffer of descriptor of detected keypoints
  self.featureDescriptors = []
  self.turnWindowWidth = 50
- self.linesToPass = rospy.get_param('lines_to_pass') 
+ # number of lines to pass while lane switching 
+ # (this gets automoatically initialized basedon detected line too)
+ self.linesToPass = rospy.get_param('lines_to_pass') 
+
+
+ # crop row recognition Difference thersholds
  self.max_matching_dif_features = rospy.get_param('max_matching_dif_features') 
  self.min_matching_dif_features = rospy.get_param('min_matching_dif_features') 
- self.smoothSize = 5 # Parameter for controlling the smoothing effect
- # Threshold for keypoints
+ # Threshold for keypoints
  self.matching_keypoints_th = rospy.get_param('matching_keypoints_th')
  # if there is no plant in the image
  self.noPlantsSeen = False
@@ -237,22 +240,19 @@ def navigate(self):
  self.velocityMsg.linear.x, 
  self.velocityMsg.linear.y, 
  round(self.velocityMsg.angular.z, 3))
-
- # publish the modified image
+ # Publish the Graphics image
  self.imageProcessor.drawGraphics()
  graphic_img = self.bridge.cv2_to_imgmsg(self.imageProcessor.processedIMG, encoding='rgb8')
  self.graphic_pub.publish(graphic_img)
-
+ # publish predicted Mask
  mask_msg = CvBridge().cv2_to_imgmsg(self.mask)
  mask_msg.header.stamp = rospy.Time.now()
  self.mask_pub.publish(mask_msg)
-
+ # publish Exg image 
  exg_msg = CvBridge().cv2_to_imgmsg(self.imageProcessor.greenIDX)
  exg_msg.header.stamp = rospy.Time.now()
  self.exg_pub.publish(exg_msg)
 
-
-
  # updates navigation stagte (one higher or reset to 1 from > 4)
  def updateNavigationStage(self):
  self.navigationMode += 1
@@ -318,7 +318,7 @@ def computeControls(self):
  y = self.imageProcessor.P[1]
  t = self.imageProcessor.ang
  # define desired and actual feature vector
- desiredFeature = np.array([0, self.imgSize[0]/2, 0])
+ desiredFeature = np.array([0.0, self.imgSize[0]/2, 0.0])
  actualFeature = np.array([x, y, t])
  # compute controls
  controls = vs_controller.Controller(self.camera,