Include nav launching in controllers

src/control/controllers/controllers/node_point_fitting2.py

@@ -10,7 +10,7 @@
 from rclpy.publisher import Publisher
 
 from ackermann_msgs.msg import AckermannDriveStamped
-from driverless_msgs.msg import Cone, ConeDetectionStamped
+from driverless_msgs.msg import Cone, ConeDetectionStamped, State
 from sensor_msgs.msg import Image
 
 from driverless_common.common import QOS_ALL, QOS_LATEST
@@ -39,13 +39,14 @@
 
 LIDAR_DIST = 1.65  # distance from lidar to front of car
 Y_CROP = 6
-X_CROP = 7
-LINE_LEN = 10.0  # len of spline line
-STEERING_VALS = np.linspace(-90, 90, SPLINES)  # steering values to take
+X_CROP = 13
+LINE_LEN = 13.0  # len of spline line
+STEERING_VALS = np.linspace(-37, 37, SPLINES)  # steering values to take
 
 
 class PointFitController(Node):
-    targ_vel: float = 1.5  # m/s
+    driving = False
+    targ_vel: float = 12.0  # m/s
     debug_img = np.zeros((HEIGHT, WIDTH, 3), dtype=np.uint8)  # create black image
 
     steering_paths: List[List[Point]] = []
@@ -60,6 +61,8 @@ def __init__(self):
         # cone detections
         self.create_subscription(ConeDetectionStamped, "/lidar/cone_detection", self.callback, QOS_ALL)
 
+        self.create_subscription(State, "/system/as_status", self.state_callback, QOS_LATEST)
+
         # publishers
         self.control_publisher: Publisher = self.create_publisher(AckermannDriveStamped, "/control/driving_command", 1)
 
@@ -118,11 +121,22 @@ def initialize_splines(self):
                     cos(angle) * vector.x - sin(angle) * vector.y, sin(angle) * vector.x + cos(angle) * vector.y
                 )  # rotate
 
+    def state_callback(self, msg: State):
+        if msg.state == State.DRIVING and not self.driving and msg.navigation_ready:
+            # delay starting driving for 2 seconds to allow for mapping to start
+            time.sleep(2)
+            self.driving = True
+            self.get_logger().info("Ready to drive, discovery started", once=True)
+
     def img_callback(self, img_msg: Image):
         self.debug_img = cv_bridge.imgmsg_to_cv2(img_msg, desired_encoding="bgr8")
 
     def callback(self, cone_msg: ConeDetectionStamped):
         self.get_logger().debug("Received detection")
+        if not self.driving:
+            self.get_logger().info("Not ready to drive", once=True)
+            return
+
         start: float = time.perf_counter()  # begin a timer
 
         # safety critical, set to 0 if not good detection

src/control/controllers/controllers/node_reactive_control.py

@@ -40,14 +40,14 @@ def initialise_params(self):
         self.driving = False
         self.discovering = False
 
-        self.declare_parameter("ebs_control", False)
+        self.declare_parameter("ebs_control", True)
         self.ebs_test = self.get_parameter("ebs_control").value
 
         self.get_logger().info("EBS Control: " + str(self.ebs_test))
 
         if self.ebs_test:
-            self.Kp_ang = 2.0
-            self.target_vel = 12.0  # m/s
+            self.Kp_ang = 0.2
+            self.target_vel = 5.0  # m/s
         else:
             self.Kp_ang = 1.8
             self.target_vel = 2.0  # m/s
@@ -65,7 +65,7 @@ def get_closest_to_origin(self, cones):
         return cones[min(index + self.target_offset, len(cones) - 1)]
 
     def state_callback(self, msg: State):
-        if msg.state == State.DRIVING and not self.driving:
+        if msg.state == State.DRIVING and not self.driving and msg.navigation_ready:
             # delay starting driving for 2 seconds to allow for mapping to start
             time.sleep(2)
             self.driving = True