From 6f30812c73d309266038c407a0c38a6fae313bcc Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Sun, 24 Mar 2024 02:59:49 +0300 Subject: [PATCH 1/2] =?UTF-8?q?=D1=80=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE?= =?UTF-8?q?=D0=B2=D0=B0=D0=BD=D0=BE=20=D0=B2=D0=B7=D0=B0=D0=B8=D0=BC=D0=BE?= =?UTF-8?q?=D0=B4=D0=B5=D0=B9=D1=81=D1=82=D0=B2=D0=B8=D0=B5=20=D0=BD=D0=BE?= =?UTF-8?q?=D0=B4=20=D1=80=D0=B5=D0=B3=D1=83=D0=BB=D1=8F=D1=82=D0=BE=D1=80?= =?UTF-8?q?=D0=B0=20=D0=B8=20=D0=BA=D0=BE=D0=BD=D1=82=D1=80=D0=BE=D0=BB?= =?UTF-8?q?=D0=BB=D0=B5=D1=80=D0=B0,=20=D0=BF=D0=BE=D0=B4=D1=80=D0=BE?= =?UTF-8?q?=D0=B1=D0=BD=D0=B5=D0=B5=20=D0=B2=20readme?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .idea/.gitignore | 3 +++ .idea/inspectionProfiles/Project_Default.xml | 12 +++++++++++ .../inspectionProfiles/profiles_settings.xml | 6 ++++++ .idea/lesson_06.iml | 13 ++++++++++++ .idea/misc.xml | 7 +++++++ .idea/modules.xml | 8 ++++++++ .idea/vcs.xml | 6 ++++++ README.md | 2 ++ config/control-config.yaml | 9 ++++----- .../{step_1.launch.py => homework.launch.py} | 2 +- launch/step_0.launch.py | 20 ++++++++++++++++--- launch/step_2.launch.py | 19 ------------------ launch/step_3.launch.py | 19 ------------------ launch/step_4.launch.py | 19 ------------------ lesson_06/deleted.py | 13 ++++++++++++ lesson_06/pid_node.py | 5 +++-- lesson_06/step_0.py | 4 ++-- setup.py | 2 +- 18 files changed, 98 insertions(+), 71 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/inspectionProfiles/Project_Default.xml create mode 100644 .idea/inspectionProfiles/profiles_settings.xml create mode 100644 .idea/lesson_06.iml create mode 100644 .idea/misc.xml create mode 100644 .idea/modules.xml create mode 100644 .idea/vcs.xml create mode 100644 README.md rename launch/{step_1.launch.py => homework.launch.py} (95%) delete mode 100644 launch/step_2.launch.py delete mode 100644 launch/step_3.launch.py delete mode 100644 launch/step_4.launch.py create mode 100644 lesson_06/deleted.py diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..26d3352 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1,3 @@ +# Default ignored files +/shelf/ +/workspace.xml diff --git a/.idea/inspectionProfiles/Project_Default.xml b/.idea/inspectionProfiles/Project_Default.xml new file mode 100644 index 0000000..b74a263 --- /dev/null +++ b/.idea/inspectionProfiles/Project_Default.xml @@ -0,0 +1,12 @@ + + + + \ No newline at end of file diff --git a/.idea/inspectionProfiles/profiles_settings.xml b/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/.idea/lesson_06.iml b/.idea/lesson_06.iml new file mode 100644 index 0000000..17714ac --- /dev/null +++ b/.idea/lesson_06.iml @@ -0,0 +1,13 @@ + + + + + + + + + + + + \ No newline at end of file diff --git a/.idea/misc.xml b/.idea/misc.xml new file mode 100644 index 0000000..b950f42 --- /dev/null +++ b/.idea/misc.xml @@ -0,0 +1,7 @@ + + + + + + \ No newline at end of file diff --git a/.idea/modules.xml b/.idea/modules.xml new file mode 100644 index 0000000..09e98d9 --- /dev/null +++ b/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/.idea/vcs.xml b/.idea/vcs.xml new file mode 100644 index 0000000..35eb1dd --- /dev/null +++ b/.idea/vcs.xml @@ -0,0 +1,6 @@ + + + + + + \ No newline at end of file diff --git a/README.md b/README.md new file mode 100644 index 0000000..9159b9c --- /dev/null +++ b/README.md @@ -0,0 +1,2 @@ +Несколько костыльно реализовано общение нод контроллера и регулятора и настраиваемые коэффициенты, подобраны настройки пид, наиболее устроившие меня на данный момент. +Костыльность заключается в том, что в переменную heading из-за использования функции get_yaw_from_quaternion при перелете угла через пи начинает поступать отрицательный угол, это частично поправлено в ноде контроллера, однако серьезно влияет на поведение модели при заданных углах 180 и 270. Попробую разобраться с этим в дальнейшем. diff --git a/config/control-config.yaml b/config/control-config.yaml index 80bbe66..8d48b5c 100644 --- a/config/control-config.yaml +++ b/config/control-config.yaml @@ -1,11 +1,10 @@ -/*: - pid: +pid: ros__parameters: angular: true clamp: 15.0 inverted: false - kd: 0.9 + kd: 0.1 ki: 0.0 - kp: 0.04 + kp: 0.21 start_type_description_service: true - use_sim_time: false + use_sim_time: false \ No newline at end of file diff --git a/launch/step_1.launch.py b/launch/homework.launch.py similarity index 95% rename from launch/step_1.launch.py rename to launch/homework.launch.py index 2908c5e..6732b65 100644 --- a/launch/step_1.launch.py +++ b/launch/homework.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): Node( package='lesson_06', namespace='waterstrider', - executable='step_1', + executable='homework', name='controller' ), Node( diff --git a/launch/step_0.launch.py b/launch/step_0.launch.py index ca123df..5cae864 100644 --- a/launch/step_0.launch.py +++ b/launch/step_0.launch.py @@ -1,13 +1,27 @@ +import os +from ament_index_python import get_package_share_directory + from launch import LaunchDescription from launch_ros.actions import Node def generate_launch_description(): + config_file = os.path.join(get_package_share_directory('lesson_06'), + 'config', + 'control-config.yaml') return LaunchDescription([ Node( package='lesson_06', - namespace='', + namespace='waterstrider', executable='step_0', - name='controller' + name='controller', + parameters=[config_file] + ), + Node( + package='lesson_06', + namespace='waterstrider', + executable='pid_node', + name='pid', + parameters=[config_file] ) - ]) + ]) \ No newline at end of file diff --git a/launch/step_2.launch.py b/launch/step_2.launch.py deleted file mode 100644 index aa29cca..0000000 --- a/launch/step_2.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='turtlesim', - namespace='', - executable='turtlesim_node', - name='sim' - ), - Node( - package='lesson_06', - namespace='', - executable='step_2', - name='square' - ) - ]) diff --git a/launch/step_3.launch.py b/launch/step_3.launch.py deleted file mode 100644 index 512b8c4..0000000 --- a/launch/step_3.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='turtlesim', - namespace='', - executable='turtlesim_node', - name='sim' - ), - Node( - package='lesson_06', - namespace='', - executable='step_3', - name='square' - ) - ]) diff --git a/launch/step_4.launch.py b/launch/step_4.launch.py deleted file mode 100644 index ea6787d..0000000 --- a/launch/step_4.launch.py +++ /dev/null @@ -1,19 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import Node - - -def generate_launch_description(): - return LaunchDescription([ - Node( - package='turtlesim', - namespace='', - executable='turtlesim_node', - name='sim' - ), - Node( - package='lesson_06', - namespace='', - executable='step_4', - name='square' - ) - ]) diff --git a/lesson_06/deleted.py b/lesson_06/deleted.py new file mode 100644 index 0000000..790eebf --- /dev/null +++ b/lesson_06/deleted.py @@ -0,0 +1,13 @@ + +''' +self.heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10) +self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10) +self.pid_result = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10) + + +#вариант реализации коррекции ошибки heading +if get_yaw_from_quaternion(msg.pose.orientation) >= 0 or (0.0 <= self.desired_heading <= math.pi/2): + heading.data = get_yaw_from_quaternion(msg.pose.orientation) +else: + heading.data = 2*math.pi + get_yaw_from_quaternion(msg.pose.orientation) +''' diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py index 6c390a5..9707de5 100644 --- a/lesson_06/pid_node.py +++ b/lesson_06/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,13 +54,13 @@ 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): error = self.sp - state if not self.config['inverted'] else state - self.sp if self.config['angular']: error = self.angular_constraint(error) - self.get_logger().warn(f"PID error: {error}") + #self.get_logger().warn(f"PID error: {error}") p = self.config['kp'] * error self.e_sum += self.config['ki'] * error self.e_sum = min(self.config['clamp'], max(self.e_sum, -self.config['clamp'])) diff --git a/lesson_06/step_0.py b/lesson_06/step_0.py index c318b10..c45b5f9 100644 --- a/lesson_06/step_0.py +++ b/lesson_06/step_0.py @@ -6,7 +6,7 @@ from geometry_msgs.msg import PoseStamped, Pose from tf_transformations import euler_from_quaternion -import math +import math def angle_diff(a1, a2): a = a1-a2 @@ -57,4 +57,4 @@ def main(args=None): rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/setup.py b/setup.py index 1d42584..65b5025 100644 --- a/setup.py +++ b/setup.py @@ -30,7 +30,7 @@ setup( entry_points={ 'console_scripts': [ 'step_0 = lesson_06.step_0:main', - 'step_1 = lesson_06.step_1:main', + 'homework = lesson_06.homework:main', 'pid_node = lesson_06.pid_node:main', ], }, -- GitLab From 1e6c6ef7e4bb23c86c9dae1dce953de4c3d564ff Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Sun, 24 Mar 2024 23:29:35 +0300 Subject: [PATCH 2/2] =?UTF-8?q?=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=D0=B0=20=D0=BE=D1=82=D1=81=D1=83=D1=82=D1=81=D1=82=D0=B2?= =?UTF-8?q?=D0=BE=D0=B2=D0=B0=D0=B2=D1=88=D0=B0=D1=8F=20=D0=BD=D0=BE=D0=B4?= =?UTF-8?q?=D0=B0=20=D1=81=20=D0=B4=D0=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lesson_06/homework.py | 82 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 82 insertions(+) create mode 100644 lesson_06/homework.py diff --git a/lesson_06/homework.py b/lesson_06/homework.py new file mode 100644 index 0000000..a8cbeb2 --- /dev/null +++ b/lesson_06/homework.py @@ -0,0 +1,82 @@ +import rclpy +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 + + +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 delta_time(t1, t2): + t = t2 - t1 + return t + + +class RurMover(Node): + def __init__(self): + super().__init__('controller') + self.heading = self.create_publisher(Float64, '/waterstrider/pid/state', 10) + self.heading_setpoint = self.create_publisher(Float64, '/waterstrider/pid/setpoint', 10) + self.pid_result = self.create_subscription(Float64, '/waterstrider/pid/output', self.pid_callback, 10) + 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 = 20.0 + self.LINEAR_SPEED = 0.5 + self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) + + def pose_callback(self, msg): + heading = Float64() + if self.desired_heading <= math.pi / 2: + heading.data = get_yaw_from_quaternion(msg.pose.orientation) + elif get_yaw_from_quaternion(msg.pose.orientation) < 0: + heading.data = 2 * math.pi + get_yaw_from_quaternion(msg.pose.orientation) + self.get_logger().info('current heading: "%f"' % heading.data) + self.get_logger().info('angle setpoint: "%f"' % self.desired_heading) + desired_heading = Float64() + desired_heading.data = self.desired_heading + self.heading.publish(heading) + self.heading_setpoint.publish(desired_heading) + ''' + + ''' + + def pid_callback(self, msg): + pid_out = msg.data + # self.get_logger().info('regulator output: "%f"' % pid_out) + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + msg_pub.angular.z = pid_out + 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() -- GitLab