From 68900b4713a4be912337734aa80a63dc3bb15a6a Mon Sep 17 00:00:00 2001 From: Gleb Date: Sun, 4 Feb 2024 02:24:27 +0300 Subject: [PATCH] =?UTF-8?q?=D0=94=D0=BE=D0=B1=D0=B0=D0=B2=D0=BB=D0=B5?= =?UTF-8?q?=D0=BD=20=D0=B7=D0=B0=D0=BF=D1=83=D1=81=D0=BA=20rqt;=20=D1=80?= =?UTF-8?q?=D0=B5=D0=B0=D0=BB=D0=B8=D0=B7=D0=BE=D0=B2=D0=B0=D0=BD=20P-?= =?UTF-8?q?=D1=80=D0=B5=D0=B3=D1=83=D0=BB=D1=8F=D1=82=D0=BE=D1=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- launch/{step_0.launch.py => fin.launch.py} | 8 +++- launch/step_1.launch.py | 19 --------- launch/step_2.launch.py | 19 --------- launch/step_3.launch.py | 19 --------- launch/step_4.launch.py | 19 --------- lesson_05/{step_4.py => fin.py} | 5 ++- lesson_05/step_0.py | 40 ------------------ lesson_05/step_1.py | 45 -------------------- lesson_05/step_2.py | 48 --------------------- lesson_05/step_3.py | 49 ---------------------- setup.py | 6 +-- 11 files changed, 11 insertions(+), 266 deletions(-) rename launch/{step_0.launch.py => fin.launch.py} (69%) delete mode 100644 launch/step_1.launch.py delete mode 100644 launch/step_2.launch.py delete mode 100644 launch/step_3.launch.py delete mode 100644 launch/step_4.launch.py rename lesson_05/{step_4.py => fin.py} (92%) delete mode 100644 lesson_05/step_0.py delete mode 100644 lesson_05/step_1.py delete mode 100644 lesson_05/step_2.py delete mode 100644 lesson_05/step_3.py diff --git a/launch/step_0.launch.py b/launch/fin.launch.py similarity index 69% rename from launch/step_0.launch.py rename to launch/fin.launch.py index cea1f5a..4ec2ed3 100644 --- a/launch/step_0.launch.py +++ b/launch/fin.launch.py @@ -13,7 +13,13 @@ def generate_launch_description(): Node( package='lesson_05', namespace='', - executable='step_0', + executable='fin', name='square' + ), + Node( + package='rqt_plot', + namespace='', + executable='rqt_plot', + name='just_plot' ) ]) diff --git a/launch/step_1.launch.py b/launch/step_1.launch.py deleted file mode 100644 index 7f68209..0000000 --- a/launch/step_1.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_05', - namespace='', - executable='step_1', - name='square' - ) - ]) diff --git a/launch/step_2.launch.py b/launch/step_2.launch.py deleted file mode 100644 index 3ca7e9c..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_05', - 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 5df9760..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_05', - 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 bee9cc5..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_05', - namespace='', - executable='step_4', - name='square' - ) - ]) diff --git a/lesson_05/step_4.py b/lesson_05/fin.py similarity index 92% rename from lesson_05/step_4.py rename to lesson_05/fin.py index 0aadcee..dcc5125 100644 --- a/lesson_05/step_4.py +++ b/lesson_05/fin.py @@ -28,10 +28,10 @@ class RurMover(Node): msg_pub.linear.x = self.LINEAR_SPEED error = angle_diff(self.desired_heading, msg.theta) if error < 0: - msg_pub.angular.z = math.radians(90.0) + msg_pub.angular.z = math.radians(90.0)*(-6*error) self.get_logger().info('Turn left') else: - msg_pub.angular.z = math.radians(-90.0) + msg_pub.angular.z = math.radians(-90.0)*6*error self.get_logger().info('Turn right') self.publisher_twist.publish(msg_pub) @@ -40,6 +40,7 @@ class RurMover(Node): 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) diff --git a/lesson_05/step_0.py b/lesson_05/step_0.py deleted file mode 100644 index 21f4687..0000000 --- a/lesson_05/step_0.py +++ /dev/null @@ -1,40 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.duration import Duration - -from geometry_msgs.msg import Twist -import math - - -class RurMover(Node): - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10) - self.main() - - def publish_twist(self, linear, angular): - self.get_logger().info('Linear: "%d", angular: "%d"' % (linear, angular)) - msg = Twist() - msg.linear.x = float(linear) - msg.angular.z = math.radians(float(angular)) - self.publisher_twist.publish(msg) - self.get_clock().sleep_for(Duration(seconds=1.0)) - - def main(self): - while True: - self.publish_twist(0, 90.0) - self.publish_twist(3.0, 0.0) - - -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_05/step_1.py b/lesson_05/step_1.py deleted file mode 100644 index 5c6f107..0000000 --- a/lesson_05/step_1.py +++ /dev/null @@ -1,45 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.duration import Duration - -from geometry_msgs.msg import Twist -from turtlesim.msg import Pose -import math - - -class RurMover(Node): - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10) - self.pose_subscriber = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10) - self.main() - - def pose_callback(self, msg): - self.get_logger().info('Current heading: "%f"' % msg.theta) - - def publish_twist(self, linear, angular): - self.get_logger().info('Linear: "%d", angular: "%d"' % (linear, angular)) - msg = Twist() - msg.linear.x = float(linear) - msg.angular.z = math.radians(float(angular)) - self.publisher_twist.publish(msg) - self.get_clock().sleep_for(Duration(seconds=1.0)) - - def main(self): - while True: - self.publish_twist(0, 90.0) - self.publish_twist(3.0, 0.0) - - -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_05/step_2.py b/lesson_05/step_2.py deleted file mode 100644 index c3b92f9..0000000 --- a/lesson_05/step_2.py +++ /dev/null @@ -1,48 +0,0 @@ -import rclpy -from rclpy.node import Node -from rclpy.duration import Duration - -from geometry_msgs.msg import Twist -from turtlesim.msg import Pose -import math -import threading - -class RurMover(Node): - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10) - self.pose_subscriber = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10) - self.SIDE_TIME = 2.0 - self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) - self.step = 0 - - def pose_callback(self, msg): - self.get_logger().info('Current heading: "%f"' % msg.theta) - - def publish_twist(self, linear, angular): - self.get_logger().info('Linear: "%d", angular: "%d"' % (linear, angular)) - msg = Twist() - msg.linear.x = float(linear) - msg.angular.z = math.radians(float(angular)) - self.publisher_twist.publish(msg) - - def pub_timer_callback(self): - if self.step % 2: - self.publish_twist(0, 90.0) - else: - self.publish_twist(3.0, 0.0) - self.step += 1 - - -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_05/step_3.py b/lesson_05/step_3.py deleted file mode 100644 index 386fedc..0000000 --- a/lesson_05/step_3.py +++ /dev/null @@ -1,49 +0,0 @@ -import rclpy -from rclpy.node import Node - -from geometry_msgs.msg import Twist -from turtlesim.msg import Pose -import math - - -class RurMover(Node): - def __init__(self): - super().__init__('minimal_publisher') - self.publisher_twist = self.create_publisher(Twist, 'turtle1/cmd_vel', 10) - self.pose_subscriber = self.create_subscription(Pose, 'turtle1/pose', self.pose_callback, 10) - self.desired_heading = math.pi / 2 - self.SIDE_TIME = 5.0 - self.LINEAR_SPEED = 0.5 - self.timer = self.create_timer(self.SIDE_TIME, self.pub_timer_callback) - - def pose_callback(self, msg): - self.get_logger().info('Current heading: "%f"' % msg.theta) - self.get_logger().info('Setpoint: "%f"' % self.desired_heading) - msg_pub = Twist() - msg_pub.linear.x = self.LINEAR_SPEED - if msg.theta - self.desired_heading < 0: - msg_pub.angular.z = math.radians(90.0) - self.get_logger().info('Turn left') - else: - msg_pub.angular.z = math.radians(-90.0) - self.get_logger().info('Turn right') - self.publisher_twist.publish(msg_pub) - - def pub_timer_callback(self): - self.desired_heading += math.pi / 2 - if self.desired_heading >= math.pi: - 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() \ No newline at end of file diff --git a/setup.py b/setup.py index 0e83277..6395dba 100644 --- a/setup.py +++ b/setup.py @@ -22,11 +22,7 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'step_0 = lesson_05.step_0:main', - 'step_1 = lesson_05.step_1:main', - 'step_2 = lesson_05.step_2:main', - 'step_3 = lesson_05.step_3:main', - 'step_4 = lesson_05.step_4:main', + 'fin = lesson_05.fin:main' ], }, ) -- GitLab