From 8b0606083e2abe499b3f3ef96f36c309eb2e743a Mon Sep 17 00:00:00 2001 From: VLatyshev Date: Mon, 25 Nov 2024 17:27:11 +0300 Subject: [PATCH 1/3] PID-controller for waterstrider --- lesson_07/step_1.py | 93 +++++++++++++++++++++++++ lesson_07/step_PbI6a.py | 146 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 239 insertions(+) create mode 100644 lesson_07/step_1.py create mode 100644 lesson_07/step_PbI6a.py diff --git a/lesson_07/step_1.py b/lesson_07/step_1.py new file mode 100644 index 0000000..9dbe73a --- /dev/null +++ b/lesson_07/step_1.py @@ -0,0 +1,93 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped, PointStamped, 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] + +def get_distance(delta_point): + return sum(map(lambda x: x**2, delta_point))**0.5 + +def get_course_to_point(delta_point): + ln = get_distance(delta_point) + if ln: + dxn = delta_point[0]/ln + dyn = delta_point[1]/ln + ang = math.acos(dxn) + if dyn < 0: + ang = 2*math.pi - ang + return ang + return 0 + +def get_dist_course(delta_point): + dist = get_distance(delta_point) + course = get_course_to_point(delta_point) + return dist, course + +def get_delta_point(from_point, to_point): + return [to_point[0] - from_point[0], to_point[1] - from_point[1]] + + +class RurMover(Node): + def __init__(self): + super().__init__('controller') + self.CONTROLER_PERIOD = 1/10 + self.DIST_THRESHOLD = 0.2 + + self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + + zero_point = Float64() + zero_point.data = 0.0 + self.heading_pub = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) + self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10) + self.heading_result = self.create_subscription(Float64, '/waterstrider/pid_heading/output', self.pid_heading_callback, 10) + self.heading_setpoint.publish(zero_point) + + self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + + self.heading = 0 + self.self_point = None + self.goal_point = None + + self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update) + self.result = Twist() + + def update(self): + self.publisher_twist.publish(self.result) + + def pose_callback(self, msg): + pass + + def pid_distance_callback(self, msg): + self.result.linear.x = -1*msg.data + + def pid_heading_callback(self, msg): + self.result.angular.z = -1*msg.data + + def log_info(self, msg: str) -> None: + self.get_logger().info(msg) + + +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 diff --git a/lesson_07/step_PbI6a.py b/lesson_07/step_PbI6a.py new file mode 100644 index 0000000..ce692fe --- /dev/null +++ b/lesson_07/step_PbI6a.py @@ -0,0 +1,146 @@ +import rclpy +from rclpy.node import Node + +from std_msgs.msg import Float64 +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped, PointStamped, 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] + +def get_distance(delta_point): + return sum(map(lambda x: x**2, delta_point))**0.5 + +def get_course_to_point(delta_point): + ln = get_distance(delta_point) + if ln: + dxn = delta_point[0]/ln + dyn = delta_point[1]/ln + ang = math.acos(dxn) + if dyn < 0: + ang = 2*math.pi - ang + return ang + return 0 + +def get_dist_course(delta_point): + dist = get_distance(delta_point) + course = get_course_to_point(delta_point) + return dist, course + +def get_delta_point(from_point, to_point): + return [to_point[0] - from_point[0], to_point[1] - from_point[1]] + + +class RurMover(Node): + def __init__(self): + super().__init__('controller') + self.CONTROLER_PERIOD = 1/10 + self.DIST_THRESHOLD = 0.2 + + self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + + zero_point = Float64() + zero_point.data = 0.0 + self.heading_pub = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) + self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10) + self.heading_result = self.create_subscription(Float64, '/waterstrider/pid_heading/output', self.pid_heading_callback, 10) + self.heading_setpoint.publish(zero_point) + + self.dist_pub = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10) + self.dist_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10) + self.dist_result = self.create_subscription(Float64, '/waterstrider/pid_distance/output', self.pid_distance_callback, 10) + self.dist_setpoint.publish(zero_point) + + self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + self.create_subscription(PoseStamped, '/goal_pose', self.goal_callback, 10) + + self.heading = 0 + self.self_point = [0.0, 0.0] + self.goal_point = [0.0, 0.0] + + self.Flag = [False, False] + + self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update) + self.result = Twist() + + def update(self): + if self.Flag == [True, True]: + distance, course = get_dist_course(get_delta_point(self.self_point, self.goal_point)) + + msg_pub = Float64() + msg_pub.data = course + self.heading_setpoint.publish(msg_pub) + + msg_pub = Float64() + msg_pub.data = self.heading + self.heading_pub.publish(msg_pub) + + msg_pub = Float64() + msg_pub.data = distance + self.dist_setpoint.publish(msg_pub) + + msg_pub = Float64() + msg_pub.data = 0.0 + self.dist_pub.publish(msg_pub) + + self.Flag = [True, False] + + def pose_callback(self, msg): + self.heading = get_yaw_from_quaternion(msg.pose.orientation) + math.pi + + self_x = msg.pose.position.x + self_y = msg.pose.position.y + + self.self_point = [0.0, 0.0] + + self.self_point.insert(0, self_x) + self.self_point.insert(1, self_y) + + self.Flag.insert(0, True) + + def goal_callback(self, msg): + goal_x = msg.pose.position.x + goal_y = msg.pose.position.y + + self.goal_point = [0.0, 0.0] + + self.goal_point.insert(0, goal_x) + self.goal_point.insert(1, goal_y) + + self.Flag.insert(1, True) + + def pid_distance_callback(self, msg): + if abs(msg.data) >= 0.2: + self.result.linear.x = -1 * msg.data + else: + self.result.linear.x = 0.0 + self.publisher_twist.publish(self.result) + + def pid_heading_callback(self, msg): + self.result.angular.z = -1*msg.data + self.publisher_twist.publish(self.result) + + def log_info(self, msg: str) -> None: + self.get_logger().info(msg) + + +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 689f92da10686ba1fa210ce37bcc8b11c14285f5 Mon Sep 17 00:00:00 2001 From: VLatyshev Date: Mon, 25 Nov 2024 17:33:20 +0300 Subject: [PATCH 2/3] PID-regulator for waterstrider --- lesson_07/step_PbI6a.py | 146 ---------------------------------------- 1 file changed, 146 deletions(-) delete mode 100644 lesson_07/step_PbI6a.py diff --git a/lesson_07/step_PbI6a.py b/lesson_07/step_PbI6a.py deleted file mode 100644 index ce692fe..0000000 --- a/lesson_07/step_PbI6a.py +++ /dev/null @@ -1,146 +0,0 @@ -import rclpy -from rclpy.node import Node - -from std_msgs.msg import Float64 -from geometry_msgs.msg import Twist -from geometry_msgs.msg import PoseStamped, PointStamped, 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] - -def get_distance(delta_point): - return sum(map(lambda x: x**2, delta_point))**0.5 - -def get_course_to_point(delta_point): - ln = get_distance(delta_point) - if ln: - dxn = delta_point[0]/ln - dyn = delta_point[1]/ln - ang = math.acos(dxn) - if dyn < 0: - ang = 2*math.pi - ang - return ang - return 0 - -def get_dist_course(delta_point): - dist = get_distance(delta_point) - course = get_course_to_point(delta_point) - return dist, course - -def get_delta_point(from_point, to_point): - return [to_point[0] - from_point[0], to_point[1] - from_point[1]] - - -class RurMover(Node): - def __init__(self): - super().__init__('controller') - self.CONTROLER_PERIOD = 1/10 - self.DIST_THRESHOLD = 0.2 - - self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) - - zero_point = Float64() - zero_point.data = 0.0 - self.heading_pub = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) - self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10) - self.heading_result = self.create_subscription(Float64, '/waterstrider/pid_heading/output', self.pid_heading_callback, 10) - self.heading_setpoint.publish(zero_point) - - self.dist_pub = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10) - self.dist_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10) - self.dist_result = self.create_subscription(Float64, '/waterstrider/pid_distance/output', self.pid_distance_callback, 10) - self.dist_setpoint.publish(zero_point) - - self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) - self.create_subscription(PoseStamped, '/goal_pose', self.goal_callback, 10) - - self.heading = 0 - self.self_point = [0.0, 0.0] - self.goal_point = [0.0, 0.0] - - self.Flag = [False, False] - - self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update) - self.result = Twist() - - def update(self): - if self.Flag == [True, True]: - distance, course = get_dist_course(get_delta_point(self.self_point, self.goal_point)) - - msg_pub = Float64() - msg_pub.data = course - self.heading_setpoint.publish(msg_pub) - - msg_pub = Float64() - msg_pub.data = self.heading - self.heading_pub.publish(msg_pub) - - msg_pub = Float64() - msg_pub.data = distance - self.dist_setpoint.publish(msg_pub) - - msg_pub = Float64() - msg_pub.data = 0.0 - self.dist_pub.publish(msg_pub) - - self.Flag = [True, False] - - def pose_callback(self, msg): - self.heading = get_yaw_from_quaternion(msg.pose.orientation) + math.pi - - self_x = msg.pose.position.x - self_y = msg.pose.position.y - - self.self_point = [0.0, 0.0] - - self.self_point.insert(0, self_x) - self.self_point.insert(1, self_y) - - self.Flag.insert(0, True) - - def goal_callback(self, msg): - goal_x = msg.pose.position.x - goal_y = msg.pose.position.y - - self.goal_point = [0.0, 0.0] - - self.goal_point.insert(0, goal_x) - self.goal_point.insert(1, goal_y) - - self.Flag.insert(1, True) - - def pid_distance_callback(self, msg): - if abs(msg.data) >= 0.2: - self.result.linear.x = -1 * msg.data - else: - self.result.linear.x = 0.0 - self.publisher_twist.publish(self.result) - - def pid_heading_callback(self, msg): - self.result.angular.z = -1*msg.data - self.publisher_twist.publish(self.result) - - def log_info(self, msg: str) -> None: - self.get_logger().info(msg) - - -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 28a659d4a87e5ad24228ba08b1e0e9706f1a5d6c Mon Sep 17 00:00:00 2001 From: VLatyshev Date: Fri, 29 Nov 2024 20:54:21 +0300 Subject: [PATCH 3/3] =?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_07/step_1.py | 67 +++++++++++++++++++++++++++++++++++--- 2 files changed, 66 insertions(+), 9 deletions(-) diff --git a/config/control-config.yaml b/config/control-config.yaml index 975dca1..7662b5a 100644 --- a/config/control-config.yaml +++ b/config/control-config.yaml @@ -2,11 +2,11 @@ pid_heading: ros__parameters: angular: true - clamp: 1.0 + clamp: 3.5 inverted: false - kd: 0.9 - ki: 0.0 - kp: 0.9 + kd: 300.0 + ki: 1.0 + kp: 20.0 start_type_description_service: true use_sim_time: false pid_distance: diff --git a/lesson_07/step_1.py b/lesson_07/step_1.py index 9dbe73a..6271c36 100644 --- a/lesson_07/step_1.py +++ b/lesson_07/step_1.py @@ -49,12 +49,23 @@ class RurMover(Node): zero_point = Float64() zero_point.data = 0.0 - self.heading_pub = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) + self.heading_state = self.create_publisher(Float64, '/waterstrider/pid_heading/state', 10) self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid_heading/setpoint', 10) self.heading_result = self.create_subscription(Float64, '/waterstrider/pid_heading/output', self.pid_heading_callback, 10) self.heading_setpoint.publish(zero_point) + self.dist_state = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10) + self.dist_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10) + self.dist_result = self.create_subscription(Float64, '/waterstrider/pid_distance/output', + self.pid_distance_callback, 10) + self.dist_setpoint.publish(zero_point) + self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + self.create_subscription(PoseStamped, '/goal_pose', self.goal_callback, 10) + + self.Flag = False + self.Flag_dist = False + self.Flag_head = False self.heading = 0 self.self_point = None @@ -64,16 +75,62 @@ class RurMover(Node): self.result = Twist() def update(self): - self.publisher_twist.publish(self.result) + if self.Flag_dist == True & self.Flag_head == True: + if self.dist <= self.DIST_THRESHOLD : + self.result.linear.x = 0.0 + self.result.angular.z = 0.0 + self.Flag = False + self.publisher_twist.publish(self.result) + + self.Flag_dist = False + self.Flag_head = False + def pose_callback(self, msg): - pass + if self.Flag == True: + heading = get_yaw_from_quaternion(msg.pose.orientation) + self.self_point = [] + + self.self_point.insert(0, msg.pose.position.x) + self.self_point.insert(1, msg.pose.position.y) + + + self.dist, crs = get_dist_course(get_delta_point(self.self_point, self.goal_point)) + + diff = angle_diff(heading, crs) + + msg_pub1 = Float64() + msg_pub1.data = diff + self.heading_state.publish(msg_pub1) + + msg_pub2 = Float64() + msg_pub2.data = 0.0 + self.heading_setpoint.publish(msg_pub2) + + msg_pub3 = Float64() + msg_pub3.data = self.dist + self.dist_setpoint.publish(msg_pub3) + + msg_pub4 = Float64() + msg_pub4.data = 0.0 + self.dist_state.publish(msg_pub4) + + + def goal_callback(self, msg): + self.goal_point = [] + + self.goal_point.insert(0, msg.pose.position.x) + self.goal_point.insert(1, msg.pose.position.y) + + self.Flag = True def pid_distance_callback(self, msg): - self.result.linear.x = -1*msg.data + self.result.linear.x = msg.data + self.Flag_dist = True def pid_heading_callback(self, msg): - self.result.angular.z = -1*msg.data + self.result.angular.z = msg.data + self.Flag_head = True def log_info(self, msg: str) -> None: self.get_logger().info(msg) -- GitLab