From 179843c54bf0c9289ee8a390160652df32d27926 Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 11 Nov 2024 20:29:46 +0300 Subject: [PATCH 1/6] Modified it in order to support new script --- setup.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/setup.py b/setup.py index 1d42584..f8aac3b 100644 --- a/setup.py +++ b/setup.py @@ -29,8 +29,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'step_0 = lesson_06.step_0:main', - 'step_1 = lesson_06.step_1:main', + 'step_2 = lesson_06.step_2:main', 'pid_node = lesson_06.pid_node:main', ], }, -- GitLab From 1c341404c3ed2e57f75f68908e3c65293c6c5b9a Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 11 Nov 2024 20:31:49 +0300 Subject: [PATCH 2/6] Added a launch file for a new script --- launch/step_2.launch.py | 26 ++++++++++++++++++++++++++ 1 file changed, 26 insertions(+) create mode 100644 launch/step_2.launch.py diff --git a/launch/step_2.launch.py b/launch/step_2.launch.py new file mode 100644 index 0000000..5c0682f --- /dev/null +++ b/launch/step_2.launch.py @@ -0,0 +1,26 @@ +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='waterstrider', + executable='step_2', + name='controller' + ), + Node( + package='lesson_06', + namespace='waterstrider', + executable='pid_node', + name='pid', + parameters=[config_file] + ) + ]) -- GitLab From a514f2a8dbfd6f737c7d2166a1522845e69d53e1 Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 11 Nov 2024 20:33:53 +0300 Subject: [PATCH 3/6] Adjusted this file to improve the end result --- lesson_06/pid_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py index 6c390a5..c3cea44 100644 --- a/lesson_06/pid_node.py +++ b/lesson_06/pid_node.py @@ -15,10 +15,10 @@ class PID(Node): def __init__(self): super().__init__(f'pid') - self.config['kp'] = self.declare_parameter('kp', 0.0).value + self.config['kp'] = self.declare_parameter('kp', 2.0).value self.config['ki'] = self.declare_parameter('ki', 0.0).value - self.config['kd'] = self.declare_parameter('kd', 0.0).value - self.config['clamp'] = self.declare_parameter('clamp', 1.0).value + self.config['kd'] = self.declare_parameter('kd', 20.0).value + self.config['clamp'] = self.declare_parameter('clamp', 12.0).value self.config['angular'] = self.declare_parameter('angular', False).value self.config['inverted'] = self.declare_parameter('inverted', False).value self.add_on_set_parameters_callback(self.parameter_callback) -- GitLab From cc33f5b680d9028ad4c56f82a3bf0885bf9843c4 Mon Sep 17 00:00:00 2001 From: sberestov Date: Mon, 11 Nov 2024 20:36:39 +0300 Subject: [PATCH 4/6] Made a script for waterstrider --- lesson_06/step_2.py | 56 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 56 insertions(+) create mode 100644 lesson_06/step_2.py diff --git a/lesson_06/step_2.py b/lesson_06/step_2.py new file mode 100644 index 0000000..19c9892 --- /dev/null +++ b/lesson_06/step_2.py @@ -0,0 +1,56 @@ +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Twist +from geometry_msgs.msg import PoseStamped +from std_msgs.msg import Float64 +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] + + +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.result_subscription = self.create_subscription(Float64, '/pid/output', self.result_callback, 10) + self.pub_setpoint = self.create_publisher(Float64, '/pid/setpoint', 10) + self.pub_state = self.create_publisher(Float64, '/pid/state', 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) + message = Float64() + message.data = heading + self.pub_state.publish(message) + + def result_callback(self, msg): + msg_pub = Twist() + msg_pub.linear.x = self.LINEAR_SPEED + msg_pub.angular.z = msg.data + self.publisher_twist.publish(msg_pub) + + def pub_timer_callback(self): + message = Float64() + self.desired_heading += math.pi / 2 + self.desired_heading %= 2 * math.pi + self.get_logger().info('New angle setpoint: "%f"' % self.desired_heading) + message.data = self.desired_heading + self.pub_setpoint.publish(message) + +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 7e5874bde5843f277b375169a3d71ea975e77c72 Mon Sep 17 00:00:00 2001 From: sberestov Date: Sun, 1 Dec 2024 15:20:52 +0300 Subject: [PATCH 5/6] Reverted the unnecessary changes --- lesson_06/pid_node.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/lesson_06/pid_node.py b/lesson_06/pid_node.py index c3cea44..6c390a5 100644 --- a/lesson_06/pid_node.py +++ b/lesson_06/pid_node.py @@ -15,10 +15,10 @@ class PID(Node): def __init__(self): super().__init__(f'pid') - self.config['kp'] = self.declare_parameter('kp', 2.0).value + self.config['kp'] = self.declare_parameter('kp', 0.0).value self.config['ki'] = self.declare_parameter('ki', 0.0).value - self.config['kd'] = self.declare_parameter('kd', 20.0).value - self.config['clamp'] = self.declare_parameter('clamp', 12.0).value + self.config['kd'] = self.declare_parameter('kd', 0.0).value + self.config['clamp'] = self.declare_parameter('clamp', 1.0).value self.config['angular'] = self.declare_parameter('angular', False).value self.config['inverted'] = self.declare_parameter('inverted', False).value self.add_on_set_parameters_callback(self.parameter_callback) -- GitLab From 57205374cc7e94e3cf3520d0c9461df4ca40d859 Mon Sep 17 00:00:00 2001 From: sberestov Date: Sun, 1 Dec 2024 15:25:51 +0300 Subject: [PATCH 6/6] Minor changes to the config file --- config/control-config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/config/control-config.yaml b/config/control-config.yaml index 80bbe66..39c82b9 100644 --- a/config/control-config.yaml +++ b/config/control-config.yaml @@ -4,8 +4,8 @@ angular: true clamp: 15.0 inverted: false - kd: 0.9 + kd: 3.0 ki: 0.0 - kp: 0.04 + kp: 10.0 start_type_description_service: true use_sim_time: false -- GitLab