diff --git a/src/control/controllers/controllers/node_point_fitting2.py b/src/control/controllers/controllers/node_point_fitting2.py index fefbfe0f0..0a2b76092 100644 --- a/src/control/controllers/controllers/node_point_fitting2.py +++ b/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 diff --git a/src/control/controllers/controllers/node_reactive_control.py b/src/control/controllers/controllers/node_reactive_control.py index a6541aa68..bfdf693b8 100644 --- a/src/control/controllers/controllers/node_reactive_control.py +++ b/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