Merge pull request #113 from duckietown/daffy-staging

Daffy staging

packages/lane_control/config/lane_controller_node/agent.yaml

@@ -1,7 +1,7 @@
 #lane_control/lane_controller_node
-v_bar: 0.3
-k_d: -6.0
-k_theta: -3
+v_bar: 0.1
+k_d: -10
+k_theta: -2.5
 k_Id: 0.0
 k_Iphi: 0.0
 theta_thres: 0.523

packages/lane_control/config/lane_controller_node/default.yaml

@@ -4,7 +4,9 @@ k_d: -6.0
 k_theta: -5
 k_Id: -0.3
 k_Iphi: 0.0
-theta_thres: 0.523
+#Broken up the 'theta_thres' for more finer tuning 
+theta_thres_max: 0.75  #0.523
+theta_thres_min: -0.5
 d_thres: 0.2615
 d_offset: 0.0
 

packages/lane_control/src/lane_controller_node.py

@@ -65,7 +65,10 @@ def __init__(self, node_name):
         self.params["~k_Iphi"] = DTParam(
             "~k_Iphi", param_type=ParamType.FLOAT, min_value=-100.0, max_value=100.0
         )
-        self.params["~theta_thres"] = rospy.get_param("~theta_thres", None)
+        #self.params["~theta_thres"] = rospy.get_param("~theta_thres", None)
+        #Breaking up the self.params["~theta_thres"] parameter for more finer tuning of phi
+        self.params["~theta_thres_min"] = DTParam("~theta_thres_min", param_type=ParamType.FLOAT, min_value=-100.0, max_value=100.0)  #SUGGESTION mandatorizing the use of DTParam inplace of rospy.get_param for parameters in the entire dt-core repository as it allows active tuning while Robot is in action.
+        self.params["~theta_thres_max"] = DTParam("~theta_thres_max", param_type=ParamType.FLOAT, min_value=-100.0, max_value=100.0) 
         self.params["~d_thres"] = rospy.get_param("~d_thres", None)
         self.params["~d_offset"] = rospy.get_param("~d_offset", None)
         self.params["~integral_bounds"] = rospy.get_param("~integral_bounds", None)
@@ -215,6 +218,10 @@ def getControlAction(self, pose_msg):
             if np.abs(d_err) > self.params["~d_thres"]:
                 self.log("d_err too large, thresholding it!", "error")
                 d_err = np.sign(d_err) * self.params["~d_thres"]
+
+            if phi_err > self.params["~theta_thres_max"].value or phi_err < self.params["~theta_thres_min"].value:
+                self.log("phi_err too large/small, thresholding it!", "error")
+                phi_err = np.maximum(self.params["~theta_thres_min"].value, np.minimum(phi_err, self.params["~theta_thres_max"].value))
 
             wheels_cmd_exec = [self.wheels_cmd_executed.vel_left, self.wheels_cmd_executed.vel_right]
             if self.obstacle_stop_line_detected:

packages/lane_filter/config/lane_filter_node/default.yaml

@@ -2,7 +2,7 @@
 
 #propagation
 use_propagation: True
-debug : False
+debug: False
 lane_filter_histogram_configuration:
   mean_d_0: 0
   mean_phi_0: 0

packages/lane_filter/include/lane_filter/lane_filter.py

@@ -1,4 +1,5 @@
 from math import floor, sqrt
+from math import floor, sqrt
 
 import numpy as np
 import duckietown_code_utils as dtu

packages/line_detector/config/line_detector_node/agent.yaml

@@ -0,0 +1,23 @@
+img_size: [120,160]
+top_cutoff: 40
+
+colors:
+  RED:
+    low_1: [0,140,100]
+    high_1: [15,255,255]
+    low_2: [165,140,100]
+    high_2: [180,255,255]
+  WHITE:
+    low: [0,0,150]
+    high: [180,100,255]
+  YELLOW:
+    low: [14,100,80] #Old Value [25, 140, 100]
+    high: [35,255,255] #Old Value [45, 255, 255] #Should fix the green regions being detected as yellow
+
+line_detector_parameters:
+  canny_thresholds: [80,200]
+  canny_aperture_size: 3
+  dilation_kernel_size: 3
+  hough_threshold: 2
+  hough_min_line_length: 3
+  hough_max_line_gap: 1