From d2aeeaca349fbcb2913477749197733d7c0f3f10 Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Tue, 26 Nov 2024 18:51:11 +0300 Subject: [PATCH 1/3] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=20=D1=84=D0=B0=D0=B9=D0=BB=20=D0=B4=D0=BB=D1=8F=20=D0=B4?= =?UTF-8?q?=D0=B7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/homework.launch.py | 52 +++++++++++++++++++ lesson_07/homework.py | 102 ++++++++++++++++++++++++++++++++++++++ 2 files changed, 154 insertions(+) create mode 100644 launch/homework.launch.py create mode 100644 lesson_07/homework.py diff --git a/launch/homework.launch.py b/launch/homework.launch.py new file mode 100644 index 0000000..2fb06ec --- /dev/null +++ b/launch/homework.launch.py @@ -0,0 +1,52 @@ +import os +from ament_index_python import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import Node + + +def config(pkg, filename): + path = os.path.join(get_package_share_directory(pkg), 'config', filename).split('/') + print("DEADBEAF") + print(path) + for i in range(path.index("share"), path.index("install") - 1, -1): + path.pop(i) + path.insert(i, "src") + return "/".join(path) + + +def generate_launch_description(): + namespace = 'waterstrider' + config_file = os.path.join(get_package_share_directory('lesson_07'), + 'config', + 'control-config.yaml') + + return LaunchDescription([ + Node( + package='lesson_07', + namespace=namespace, + executable='pid_node', + name='pid_distance', + parameters=[config_file] + ), + Node( + package='lesson_07', + namespace=namespace, + executable='pid_node', + name='pid_heading', + parameters=[config_file] + ), + Node( + package='lesson_07', + namespace=namespace, + executable='homework', + name='controller' + ), + Node( + package='rviz2', + namespace=namespace, + executable='rviz2', + name='rviz2', + arguments=['-d' + config('lesson_07', 'waterstrider.rviz')] + ), + ]) diff --git a/lesson_07/homework.py b/lesson_07/homework.py new file mode 100644 index 0000000..1c35081 --- /dev/null +++ b/lesson_07/homework.py @@ -0,0 +1,102 @@ +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 -- GitLab From 65043835226ddc91460668a8c53492f6bd25e402 Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Wed, 27 Nov 2024 00:08:56 +0300 Subject: [PATCH 2/3] =?UTF-8?q?=D0=9A=D0=BE=D0=BD=D1=81=D0=BE=D0=BB=D1=8C?= =?UTF-8?q?=D0=BD=D1=8B=D0=B9=20=D1=81=D0=BA=D1=80=D0=B8=D0=BF=D1=82=20?= =?UTF-8?q?=D0=B4=D0=B7=20=D0=B4=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5=D0=BD?= =?UTF-8?q?=20=D0=B2=20setup.py?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- setup.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/setup.py b/setup.py index 2cffc5a..3311343 100644 --- a/setup.py +++ b/setup.py @@ -32,7 +32,8 @@ setup( 'step_0 = lesson_07.step_0:main', 'step_1 = lesson_07.step_1:main', 'pid_node = lesson_07.pid_node:main', - 'move2point = lesson_07.move2point:main' + 'move2point = lesson_07.move2point:main', + 'homework = lesson_07.homework:main', ], }, ) -- GitLab From c1acf01c72c0f8f3aa7dad4ac8a737c094776977 Mon Sep 17 00:00:00 2001 From: Alexander Zhandarov <4l4st4rgrum@gmail.com> Date: Wed, 27 Nov 2024 00:12:01 +0300 Subject: [PATCH 3/3] =?UTF-8?q?=D0=A0=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE?= =?UTF-8?q?=D0=B2=D0=B0=D0=BD=D0=BE=20=D1=83=D0=BF=D1=80=D0=B0=D0=B2=D0=BB?= =?UTF-8?q?=D0=B5=D0=BD=D0=B8=D0=B5=20=D1=81=20=D0=BF=D0=BE=D0=BC=D0=BE?= =?UTF-8?q?=D1=89=D1=8C=D1=8E=20Rviz?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- lesson_07/homework.py | 45 +++++++++++++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/lesson_07/homework.py b/lesson_07/homework.py index 1c35081..cb07d25 100644 --- a/lesson_07/homework.py +++ b/lesson_07/homework.py @@ -14,7 +14,6 @@ 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] @@ -51,23 +50,29 @@ class RurMover(Node): self.CONTROLER_PERIOD = 1 / 10 self.DIST_THRESHOLD = 0.2 - self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) - + self.heading = 0 + self.self_point = [0.0, 0.0] + self.goal_point = [0.0, 0.0] zero_point = Float64() zero_point.data = 0.0 + + self.publisher_twist = self.create_publisher(Twist, '/waterstrider/twist_command', 10) + 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.heading_pid_output = 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_pid_output = 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 = None - self.goal_point = None - + self.heading_setpoint.publish(zero_point) self.timer = self.create_timer(self.CONTROLER_PERIOD, self.update) self.result = Twist() @@ -75,7 +80,27 @@ class RurMover(Node): self.publisher_twist.publish(self.result) def pose_callback(self, msg): - pass + curr_heading = Float64() + desired_heading = Float64() + position_message = Float64() + dist_threshold = Float64() + + 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) + + curr_heading.data = get_yaw_from_quaternion(msg.pose.orientation) + position_message.data, desired_heading.data = get_dist_course(delta_point) + dist_threshold.data = self.DIST_THRESHOLD + + self.heading_pub.publish(curr_heading) + self.position_pub.publish(position_message) + 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 -- GitLab