Kp

ros_ws/low_level/low_level/speed_controller.py

@@ -67,43 +67,43 @@ class SpeedController(Node):
         with self.lock:
             self.target_twist.linear.x = msg.data
         self.hover_mode = False  # Exit hover mode
-        # self.get_logger().info(f"Received linear_x: {msg.data}")
+        self.get_logger().debug(f"Received linear_x: {msg.data}")
 
     def linear_y_callback(self, msg):
         with self.lock:
             self.target_twist.linear.y = msg.data
         self.hover_mode = False  # Exit hover mode
-        # self.get_logger().info(f"Received linear_y: {msg.data}")
+        self.get_logger().debug(f"Received linear_y: {msg.data}")
 
     def linear_z_callback(self, msg):
         with self.lock:
             self.target_twist.linear.z = msg.data
         self.hover_mode = False  # Exit hover mode
-        # self.get_logger().info(f"Received linear_z: {msg.data}")
+        self.get_logger().debug(f"Received linear_z: {msg.data}")
 
     def angular_z_callback(self, msg):
         with self.lock:
             self.target_twist.angular.z = msg.data
         self.hover_mode = False  # Exit hover mode
-        # self.get_logger().info(f"Received angular_z: {msg.data}")
+        self.get_logger().debug(f"Received angular_z: {msg.data}")
 
     def hover_callback(self, msg):
         self.hover_mode = msg.data
         if self.hover_mode:
-            # self.get_logger().info("Entering hover mode")
+            self.get_logger().debug("Entering hover mode")
             # Reset target velocity and PID errors
             self.reset_target_twist()
             # Immediately send a null Twist message
             self.send_null_twist()
-        # else:
-            # self.get_logger().info("Exiting hover mode")
+        else:
+            self.get_logger().debug("Exiting hover mode")
 
     def send_null_twist(self):
         # Publish a null Twist message to trigger hover
         null_twist = Twist()
         self.cmd_vel_pub.publish(null_twist)
         self.target_vel_pub.publish(null_twist)
-        # self.get_logger().info("Published null twist for hover mode")
+        self.get_logger().debug("Published null twist for hover mode")
 
     def rclpy_time_to_datetime(self, rclpy_time):
         """Convert rclpy time to Python datetime object"""
@@ -114,19 +114,18 @@ class SpeedController(Node):
         """Adjust Kp value using D-pad"""
         previous_kp = self.gains['Kp']
         if msg.axes[AXIS_CROSS_VERTICAL] > 0.5:
-            self.gains['Kp'] = self.gains['Kp'] + 0.5
+            self.gains['Kp'] = max(self.gains['Kp'] + 0.5, 0.0)
         elif msg.axes[AXIS_CROSS_VERTICAL] < -0.5:
-            self.gains['Kp'] = max(self.gains['Kp'] - 0.5,0)
+            self.gains['Kp'] = max(self.gains['Kp'] - 0.5, 0.0)
 
         # Only output Kp if it has changed
         if self.gains['Kp'] != previous_kp:
             self.get_logger().info(f"Updated Kp: {self.gains['Kp']}")
-    
+        
         # Update gains for the PID controllers
         self.pid_controller_x.gains = self.gains.copy()
         self.pid_controller_y.gains = self.gains.copy()
 
-
     def control_loop(self):
         if self.hover_mode:
             # In hover mode, continuously send a null Twist message
@@ -161,6 +160,9 @@ class SpeedController(Node):
 def main(args=None):
     rclpy.init(args=args)
 
+    # Set logging level to INFO to reduce output
+    rclpy.logging.set_logger_level('speed_controller', rclpy.logging.LoggingSeverity.INFO)
+
     speed_controller = SpeedController()
     rclpy.spin(speed_controller)