Skip to content

Commit

Permalink
Automated autoyapf fixes
Browse files Browse the repository at this point in the history
  • Loading branch information
github-actions committed Sep 29, 2024
1 parent 87e0576 commit f40edd0
Show file tree
Hide file tree
Showing 2 changed files with 51 additions and 55 deletions.
2 changes: 2 additions & 0 deletions control/velocity_controller/velocity_controller/velocity_controller.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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):
Expand Down
104 changes: 49 additions & 55 deletions control/velocity_controller_c/scripts/velocity_controller_c.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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:
Expand All @@ -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(
Expand All @@ -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)
Expand Down

0 comments on commit f40edd0

Please sign in to comment.