diff --git a/control/velocity_controller/velocity_controller/velocity_controller.py b/control/velocity_controller/velocity_controller/velocity_controller.py index b101245e..308443fd 100644 --- a/control/velocity_controller/velocity_controller/velocity_controller.py +++ b/control/velocity_controller/velocity_controller/velocity_controller.py @@ -6,6 +6,8 @@ from geometry_msgs.msg import Wrench from nav_msgs.msg import Odometry from rclpy.node import Node + + class VelocityNode(Node): def __init__(self): diff --git a/control/velocity_controller_c/scripts/velocity_controller_c.py b/control/velocity_controller_c/scripts/velocity_controller_c.py index aa00ddd7..85fe1883 100755 --- a/control/velocity_controller_c/scripts/velocity_controller_c.py +++ b/control/velocity_controller_c/scripts/velocity_controller_c.py @@ -9,7 +9,6 @@ from std_msgs.msg import Float32MultiArray - #A function to convert orientation quaternions to euler angles def quaternion_to_euler_angle(w, x, y, z): ysqr = y * y @@ -29,6 +28,7 @@ def quaternion_to_euler_angle(w, x, y, z): return X, Y, Z + # A function to convert the angle to the smallest signed angle def ssa(angle): if angle > np.pi: @@ -37,20 +37,21 @@ def ssa(angle): angle += 2 * np.pi return angle + class VelocityNode(Node): def __init__(self): super().__init__("input_subscriber") self.nucleus_subscriber = self.create_subscription( - Odometry, "/nucleus/odom", self.nucleus_callback, 10 - ) - + Odometry, "/nucleus/odom", self.nucleus_callback, 10) + self.guidance_subscriber = self.create_subscription( - Float32MultiArray, "velocity_points", self.guidance_callback, 10 - ) - - self.publisher_controller = self.create_publisher(Wrench, "/thrust/wrench_input", 10) - self.publisher_states = self.create_publisher(Float32MultiArray, "/Velocity/States", 10) + Float32MultiArray, "velocity_points", self.guidance_callback, 10) + + self.publisher_controller = self.create_publisher( + Wrench, "/thrust/wrench_input", 10) + self.publisher_states = self.create_publisher(Float32MultiArray, + "/Velocity/States", 10) self.control_timer = self.create_timer(0.1, self.publish_callback) self.state_timer = self.create_timer(0.3, self.publish_states) self.topic_subscriber = self.create_subscription( @@ -72,92 +73,85 @@ def __init__(self): self.position = np.array([0.0, 0.0, 0.0]) self.twist = np.array([0.0, 0.0, 0.0]) self.orientation = np.array([0.0, 0.0, 0.0]) - self.abu_values = np.array([-0.2, -5.0 , -np.pi/4, np.pi/4]) - + self.abu_values = np.array([-0.2, -5.0, -np.pi / 4, np.pi / 4]) + self.Kp_linear = 100.0 self.Kp_pitch = 2.0 self.Kp_yaw = 2.0 - + self.Ti_pitch = 0.05 self.Ti_yaw = 0.05 self.Ti_surge = 33.0 self.pitch_integral_sum = 0.0 self.yaw_integral_sum = 0.0 self.surge_integral_sum = 0.0 - + self.heave_error = 0.0 self.pitch_error = 0.0 self.yaw_error = 0.0 self.surge_error = 0.0 - def nucleus_callback(self, msg: Odometry): # callback function - self.position = np.array( - [ - msg.pose.pose.position.x, - msg.pose.pose.position.y, - msg.pose.pose.position.z, - ] - ) - - self.twist = np.array( - [ - msg.twist.twist.linear.x, - msg.twist.twist.linear.y, - msg.twist.twist.linear.z - ] - ) - - X,Y,Z = quaternion_to_euler_angle(msg.pose.pose.orientation.w, - msg.pose.pose.orientation.x, - msg.pose.pose.orientation.y, - msg.pose.pose.orientation.z) + self.position = np.array([ + msg.pose.pose.position.x, + msg.pose.pose.position.y, + msg.pose.pose.position.z, + ]) + + self.twist = np.array([ + msg.twist.twist.linear.x, msg.twist.twist.linear.y, + msg.twist.twist.linear.z + ]) + + X, Y, Z = quaternion_to_euler_angle(msg.pose.pose.orientation.w, + msg.pose.pose.orientation.x, + msg.pose.pose.orientation.y, + msg.pose.pose.orientation.z) self.orientation = np.array([X, Y, Z]) - + def guidance_callback(self, msg: Float32MultiArray): # callback function self.abu_values = np.array(msg.data[:3]) - def publish_callback(self): # The controller function self.surge_error = self.abu_values[0] - self.twist[0] self.pitch_error = ssa(self.abu_values[2] - self.orientation[1]) self.yaw_error = ssa(self.abu_values[3] - self.orientation[2]) - + msg = Wrench() - + if abs(self.surge_error) < 0.005: self.surge_integral_sum = 0.0 else: self.surge_integral_sum += self.surge_error - - msg.force.x = self.surge_error*self.Kp_linear + self.surge_integral_sum * self.Ti_surge # SURGE - - - msg.force.z = 0.0 # HEAVE + + msg.force.x = self.surge_error * self.Kp_linear + self.surge_integral_sum * self.Ti_surge # SURGE + + msg.force.z = 0.0 # HEAVE msg.torque.z = 0.0 msg.torque.y = 0.0 - + # if self.pitch_error < 0.4: # self.pitch_integral_sum += self.pitch_error - + # if self.yaw_error < 0.1: # self.yaw_integral_sum += self.yaw_error - + # msg.torque.y = 0.0 - # msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH - - # msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw)# YAW + # msg.torque.y = -(self.pitch_error * self.Kp_pitch + self.pitch_integral_sum * self.Ti_pitch) # PITCH + + # msg.torque.z = -(self.yaw_error * self.Kp_yaw + self.yaw_integral_sum * self.Ti_yaw)# YAW # self.publisher_controller.publish(msg) - + def publish_states(self): msg = Float32MultiArray() - msg.data = [self.abu_values[2], self.abu_values[3], - self.orientation[1], self.orientation[2], - self.abu_values[0], self.abu_values[1], - self.twist[0], self.twist[2]] + msg.data = [ + self.abu_values[2], self.abu_values[3], self.orientation[1], + self.orientation[2], self.abu_values[0], self.abu_values[1], + self.twist[0], self.twist[2] + ] self.publisher_states.publish(msg) - + def main(args=None): # main function rclpy.init(args=args)