cleaning up the code

CMakeLists.txt

@@ -24,6 +24,6 @@ include_directories(
 
 ## Add folders to be run by python nosetests
 # catkin_add_nosetests(test)
-catkin_install_python(PROGRAMS scripts/Camera.py scripts/featureExtractor.py scripts/vs_navigator.py scripts/Controller.py
+catkin_install_python(PROGRAMS scripts/Camera.py scripts/featureExtractor.py scripts/vs_nodeHandler.py scripts/Controller.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

configs/params.yaml

@@ -20,30 +20,30 @@ queue_size: 5
 
 img_resize_scale: 100
 
-stationary_debug: False
+stationaryDebug: False
 # run time params
 # Mode 1: Driving forward with front camera (starting mode)
 # Mode 2: Driving forward with back camera
 # Mode 3: Driving backwards with back camera
 # Mode 4: Driving backwards with front camera
-nav_mode: 1
+navigationMode: 1
 # angular velocity scaler
-max_linear_vel: 0.1 
-min_linear_vel: 0.01 
-angular_scaler: 0.1
-max_angular_vel: 0.05 
-min_angular_vel: 0.01
+maxOmega: 0.05 
+minOmega: 0.01 
+omegaScaler: 0.1
+maxLinearVel: 0.5 
+minLinearVel: 0.01
 # Parameter for setting the number of rows to pass in one switch
 lines_to_pass: 1
 # Max Offset of the Window for extracting the turn features
 max_matching_dif_features: 100
 # Min Offset of the Window for extracting the turn features
 min_matching_dif_features: 0
 # Threshold for keypoints
-matching_keypoints_th: 20
-
-winSweepStart: 100
-winSweepEnd: 980
+matching_keypoints_th: 10
+# scanner window params
+winSweepStart: 0
+winSweepEnd: 1280
 winMinWidth: 128
 # W/8 for small plants, W/5 for big plants
 winSize: 128
@@ -52,6 +52,7 @@ min_contour_area: 300
 # for dividing bushy rows
 max_coutour_height: 120
 #  in case of using bigger size image size, we suggest to set ROI 
+enable_roi: False
 p1: 
  - 0
  - 0

launch/vs_navigation.launch

@@ -9,6 +9,6 @@
     <!-- Run visual-servoing navigation  -->
     <group ns="phenobot">
     <rosparam command="load" file="$(find visual_multi_crop_row_navigation)/configs/params.yaml" />
-    <node name="vs_navigation" pkg="visual_multi_crop_row_navigation" type="vs_navigator.py" output="screen" /> 
+    <node name="vs_navigation" pkg="visual_multi_crop_row_navigation" type="vs_navigationNode.py" output="screen" /> 
   </group>
 </launch>

scripts/Controller.py

@@ -11,11 +11,11 @@ def wrapToPi(theta):
 
 
 # Function to compute the controls given the current state and the desired state and velocity
-def Controller (camera, desired_state, actual_state, v_des):
+def Controller (camera, desiredState, actualState, v_des):
     # specifiy the acutal state for better readability
-    x = actual_state[0]
-    y = actual_state[1]
-    theta = actual_state[2]
+    x = actualState[0]
+    y = actualState[1]
+    theta = actualState[2]
 
     # some crazy parameters   
     lambda_x_1 = 10
@@ -57,8 +57,8 @@ def Controller (camera, desired_state, actual_state, v_des):
     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 = actual_state[controller_type] - desired_state[controller_type]
-    ang_delta = actual_state[2] - desired_state[2]
+    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