From f7bcac20c8db9e87b9f7526f436805ce4f165355 Mon Sep 17 00:00:00 2001 From: vkolesnikov Date: Tue, 19 Nov 2024 01:08:12 +0300 Subject: [PATCH 1/3] The coefficients of the PID controller have been changed --- config/control-config.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/config/control-config.yaml b/config/control-config.yaml index 975dca1..94c123c 100644 --- a/config/control-config.yaml +++ b/config/control-config.yaml @@ -2,20 +2,20 @@ pid_heading: ros__parameters: angular: true - clamp: 1.0 + clamp: 10.0 inverted: false - kd: 0.9 + kd: 0.0 ki: 0.0 - kp: 0.9 + kp: 10.0 start_type_description_service: true use_sim_time: false pid_distance: ros__parameters: - angular: true + angular: false clamp: 1.0 inverted: false - kd: 0.9 + kd: 5.0 ki: 0.0 - kp: 0.4 + kp: 1.0 start_type_description_service: true use_sim_time: false -- GitLab From a0344c649ff0f04650d5c9675941e3e4e2c664ae Mon Sep 17 00:00:00 2001 From: vkolesnikov Date: Tue, 19 Nov 2024 01:10:17 +0300 Subject: [PATCH 2/3] Minor changes have been made to the PID-node code --- lesson_07/pid_node.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/lesson_07/pid_node.py b/lesson_07/pid_node.py index 6c390a5..6decae7 100644 --- a/lesson_07/pid_node.py +++ b/lesson_07/pid_node.py @@ -4,6 +4,7 @@ import rclpy from rclpy.node import Node from std_msgs.msg import Float64 from rcl_interfaces.msg import SetParametersResult +import math class PID(Node): @@ -53,9 +54,11 @@ class PID(Node): self.pub_result.publish(message) def angular_constraint(self, e): - return (e + 180.0) % (2 * 180.0) - 180.0 + return (e+math.pi)%(2*math.pi)-math.pi def update(self, state): + self.get_logger().warn(f"State: {state}") + self.get_logger().warn(f"SETPOINT: {self.sp}") error = self.sp - state if not self.config['inverted'] else state - self.sp if self.config['angular']: error = self.angular_constraint(error) -- GitLab From 823b3cc7ec73350b2e54201086d3d6357acf0ab7 Mon Sep 17 00:00:00 2001 From: vkolesnikov Date: Tue, 19 Nov 2024 01:12:16 +0300 Subject: [PATCH 3/3] The Step_1 glider control file has been created --- lesson_07/step_1.py | 119 ++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 119 insertions(+) create mode 100644 lesson_07/step_1.py diff --git a/lesson_07/step_1.py b/lesson_07/step_1.py new file mode 100644 index 0000000..005ee79 --- /dev/null +++ b/lesson_07/step_1.py @@ -0,0 +1,119 @@ +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 + +from tf_transformations import euler_from_quaternion + +import math + + +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.position_pub = self.create_publisher(Float64, '/waterstrider/pid_distance/state', 10) + self.position_setpoint = self.create_publisher(Float64, '/waterstrider/pid_distance/setpoint', 10) + self.position_result = self.create_subscription(Float64, '/waterstrider/pid_distance/output', self.pid_distance_callback, 10) + + self.create_subscription(PoseStamped, '/waterstrider/ground_truth_to_tf_waterstrider/pose', self.pose_callback, 10) + self.create_subscription(PoseStamped, '/goal_pose', self.goal_pose_callback, 10) + + self.heading = 0 + self.self_point = [0.0, 0.0] + self.goal_point = [0.0, 0.0] + + 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): + desired_heading = Float64() + message_position = Float64() + curent_heading = Float64() + dist_threshold = Float64() + + dist_threshold.data = self.DIST_THRESHOLD + + curent_heading.data = get_yaw_from_quaternion(msg.pose.orientation) + self.heading_pub.publish(curent_heading) + + + self.self_point[0] = msg.pose.position.x + self.self_point[1] = msg.pose.position.y + + delta_point = get_delta_point(self.self_point, self.goal_point) + message_position.data, desired_heading.data = get_dist_course(delta_point) + self.position_pub.publish(message_position) + self.heading_setpoint.publish(desired_heading) + self.position_setpoint.publish(dist_threshold) + + def goal_pose_callback(self, msg): + self.goal_point[0] = msg.pose.position.x + self.goal_point[1] = msg.pose.position.y + + + + def pid_distance_callback(self, msg): + self.result.linear.x = -1*msg.data + + def pid_heading_callback(self, msg): + self.result.angular.z = 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 -- GitLab