From 565576b72c55138856dbf075f5c164a538125e11 Mon Sep 17 00:00:00 2001 From: VLatyshev Date: Mon, 11 Nov 2024 14:51:44 +0300 Subject: [PATCH 1/2] Waterstrider moves in a square trajectory using PID-controller --- lesson_06/step_1.py | 60 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 60 insertions(+) create mode 100644 lesson_06/step_1.py diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py new file mode 100644 index 0000000..742e70f --- /dev/null +++ b/lesson_06/step_1.py @@ -0,0 +1,60 @@ +import rclpy +from rclpy.node import Node + +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped, Pose + +from tf_transformations import euler_from_quaternion + +import math + +def angle_diff(a1, a2): + a = a1-a2 + return (a+math.pi)%(2*math.pi)-math.pi + +def get_yaw_from_quaternion(q): + return euler_from_quaternion([q.x, q.y, q.z, q.w])[2] + + +class RurMover(Node): + def __init__(self): + super().__init__('controller') + self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + self.pose_subscriber = self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + self.desired_heading = - math.pi / 2 + self.SIDE_TIME = 10.0 + self.LINEAR_SPEED = 1.0 + self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + + def pose_callback(self, msg): + heading = get_yaw_from_quaternion(msg.pose.orientation) + self.get_logger().info('Current heading: "%f"' % heading) + self.get_logger().info('Setpoint: "%f"' % self.desired_heading) + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + error = angle_diff(self.desired_heading, heading) + if error < 0: + msg_pub.angular.z = math.radians(90.0) + self.get_logger().info('Turn left') + else: + msg_pub.angular.z = math.radians(-90.0) + self.get_logger().info('Turn right') + self.publisher_twist.publish(msg_pub) + + def pub_timer_callback(self): + self.desired_heading += math.pi / 2 + self.desired_heading %= 2 * math.pi + self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading) + +def main(args=None): + rclpy.init(args=args) + + mover = RurMover() + + rclpy.spin(mover) + + mover.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() \ No newline at end of file -- GitLab From f305d76189c95a4c5b58594709aef91d96df8ba5 Mon Sep 17 00:00:00 2001 From: VLatyshev Date: Fri, 29 Nov 2024 20:57:01 +0300 Subject: [PATCH 2/2] =?UTF-8?q?=D0=9F=D0=BE=D0=BF=D1=8B=D1=82=D0=BA=D0=B0?= =?UTF-8?q?=20=D0=BF=D1=80=D0=B8=D0=B2=D0=B5=D1=81=D1=82=D0=B8=20=D1=84?= =?UTF-8?q?=D0=B0=D0=B9=D0=BB=D1=8B=20=D0=BA=20=D0=B0=D0=BA=D1=82=D1=83?= =?UTF-8?q?=D0=B0=D0=BB=D1=8C=D0=BD=D0=BE=D0=B9=20=D0=B2=D0=B5=D1=80=D1=81?= =?UTF-8?q?=D0=B8=D0=B8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- config/control-config.yaml | 8 ++++---- lesson_06/step_1.py | 34 +++++++++++++++++++++++----------- 2 files changed, 27 insertions(+), 15 deletions(-) diff --git a/config/control-config.yaml b/config/control-config.yaml index 80bbe66..f1c9764 100644 --- a/config/control-config.yaml +++ b/config/control-config.yaml @@ -2,10 +2,10 @@ pid: ros__parameters: angular: true - clamp: 15.0 + clamp: 175.0 inverted: false - kd: 0.9 - ki: 0.0 - kp: 0.04 + kd: 50000.0 + ki: 1000000.0 + kp: 9000.0 start_type_description_service: true use_sim_time: false diff --git a/lesson_06/step_1.py b/lesson_06/step_1.py index 742e70f..d8d3861 100644 --- a/lesson_06/step_1.py +++ b/lesson_06/step_1.py @@ -4,6 +4,8 @@ from rclpy.node import Node from geometry_msgs.msg import Twist from geometry_msgs.msg import PoseStamped, Pose +from std_msgs.msg import Float64 + from tf_transformations import euler_from_quaternion import math @@ -20,31 +22,41 @@ class RurMover(Node): def __init__(self): super().__init__('controller') self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + self.pid_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10) + self.pid_state = self.create_publisher(Float64, '/waterstrider/pid/state', 10, ) + self.pid_output= self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_output, 10) self.pose_subscriber = self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + self.heading = 0 + self.n = 0 self.desired_heading = - math.pi / 2 self.SIDE_TIME = 10.0 self.LINEAR_SPEED = 1.0 self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) - def pose_callback(self, msg): - heading = get_yaw_from_quaternion(msg.pose.orientation) - self.get_logger().info('Current heading: "%f"' % heading) - self.get_logger().info('Setpoint: "%f"' % self.desired_heading) + def pid_output(self, msg): + angle_spd = msg.data msg_pub = Twist() msg_pub.linear.x = self.LINEAR_SPEED - error = angle_diff(self.desired_heading, heading) - if error < 0: - msg_pub.angular.z = math.radians(90.0) - self.get_logger().info('Turn left') - else: - msg_pub.angular.z = math.radians(-90.0) - self.get_logger().info('Turn right') + msg_pub.angular.z = math.radians(angle_spd) + self.get_logger().info(f'Turn speed: {angle_spd:.3f}') self.publisher_twist.publish(msg_pub) + def pose_callback(self, msg): + self.heading = get_yaw_from_quaternion(msg.pose.orientation)+math.pi + self.get_logger().info('Current heading: "%f"' % self.heading) + self.get_logger().info('Setpoint: "%f"' % self.desired_heading) + self.diff = angle_diff(self.heading, self.desired_heading) + msg_pub = Float64() + msg_pub.data = self.diff + self.pid_state.publish(msg_pub) + def pub_timer_callback(self): self.desired_heading += math.pi / 2 self.desired_heading %= 2 * math.pi + msg_pub = Float64() + msg_pub.data = 0.0 self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading) + self.pid_setpoint.publish(msg_pub) def main(args=None): rclpy.init(args=args) -- GitLab