Kp

ros_ws/low_level/low_level/speed_controller.py

@@ -5,25 +5,36 @@ from rclpy.node import Node
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Float32, Bool
 from nav_msgs.msg import Odometry
+from sensor_msgs.msg import Joy
 from threading import Lock
 from .PID import PID
 import datetime
 
+# Define button mappings for the remote controller
+AXIS_CROSS_HORIZONTAL = 6  # D-pad horizontal direction
+AXIS_CROSS_VERTICAL = 7    # D-pad vertical direction
+
 class SpeedController(Node):
     def __init__(self):
         super().__init__('speed_controller')
 
         # Initialize attributes
         self.target_twist = Twist()  # Target velocity
-        self.lock = Lock()           # Mutex lock to protect target_twist
+        self.lock = Lock()           # Mutex lock to protect target velocity
         self.hover_mode = False      # Hover mode
         self.odom_velocity = Twist()
 
+        # Initialize PID controller with initial gains
+        initial_kp = 0.0
+        self.gains = {'Kp': initial_kp, 'Ki': 0.0, 'Kd': 0.0}
+        self.pid_controller_x = PID(self.gains)
+        self.pid_controller_y = PID(self.gains)
+
         # Create publishers
         self.cmd_vel_pub = self.create_publisher(Twist, '/bebop/cmd_vel', 10)
         self.target_vel_pub = self.create_publisher(Twist, '/target_vel', 10)
 
-        # Create additional publishers for monitoring
+        # Monitoring topic publishers
         self.linear_x_pub = self.create_publisher(Float32, '/cmd_vel_mux/input/teleop/linear/x', 10)
         self.linear_y_pub = self.create_publisher(Float32, '/cmd_vel_mux/input/teleop/linear/y', 10)
         self.linear_z_pub = self.create_publisher(Float32, '/cmd_vel_mux/input/teleop/linear/z', 10)
@@ -36,15 +47,12 @@ class SpeedController(Node):
         self.linear_z_sub = self.create_subscription(Float32, '/linear_z', self.linear_z_callback, 10)
         self.angular_z_sub = self.create_subscription(Float32, '/angular_z', self.angular_z_callback, 10)
         self.hover_sub = self.create_subscription(Bool, '/hover', self.hover_callback, 10)
+        self.joy_sub = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
 
         # Control loop
-        self.timer = self.create_timer(0.1, self.control_loop)  # Control frequency is 10Hz
+        self.timer = self.create_timer(0.1, self.control_loop)  # Control frequency 10Hz
         self.t0 = self.get_clock().now()
 
-        # PID controllers
-        self.pid_controller_x = PID({'Kp': 0.0, 'Ki': 0.0, 'Kd': 0.0})
-        self.pid_controller_y = PID({'Kp': 0.0, 'Ki': 0.0, 'Kd': 0.0})
-
     def reset_target_twist(self):
         # Reset target velocity
         with self.lock:
@@ -59,49 +67,66 @@ 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().info(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().info(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().info(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().info(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().info("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().info("Exiting hover mode")
 
     def send_null_twist(self):
-        # Publish a null Twist message to trigger hover mode
+        # 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().info("Published null twist for hover mode")
 
     def rclpy_time_to_datetime(self, rclpy_time):
         """Convert rclpy time to Python datetime object"""
         seconds = rclpy_time.nanoseconds * 1e-9
         return datetime.datetime.fromtimestamp(seconds)
 
+    def joy_callback(self, msg):
+        """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
+        elif msg.axes[AXIS_CROSS_VERTICAL] < -0.5:
+            self.gains['Kp'] = max(self.gains['Kp'] - 0.5,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
@@ -115,7 +140,7 @@ class SpeedController(Node):
                 cmd_vel_msg.linear.z = max(min(self.target_twist.linear.z, 1.0), -1.0)
                 cmd_vel_msg.angular.z = max(min(self.target_twist.angular.z, 1.0), -1.0)
 
-                # Update the target value for the PID controller
+                # Update target values for the PID controller
                 self.pid_controller_x.update(self.rclpy_time_to_datetime(self.get_clock().now()), cmd_vel_msg.linear.x - self.odom_velocity.linear.x)
                 self.pid_controller_y.update(self.rclpy_time_to_datetime(self.get_clock().now()), cmd_vel_msg.linear.y - self.odom_velocity.linear.y)
 
@@ -127,7 +152,7 @@ class SpeedController(Node):
                 self.cmd_vel_pub.publish(cmd_vel_msg)
                 self.target_vel_pub.publish(self.target_twist)
 
-                # Publish each directional velocity separately to new topics for monitoring with rqt_plot
+                # Publish each directional velocity to new topics for monitoring in rqt_plot
                 self.linear_x_pub.publish(Float32(data=self.target_twist.linear.x))
                 self.linear_y_pub.publish(Float32(data=self.target_twist.linear.y))
                 self.linear_z_pub.publish(Float32(data=self.target_twist.linear.z))