movement_task更改

.gitignore

@@ -0,0 +1,7 @@
+build/
+install/
+log/
+*.pyc
+__pycache__
+.DS_Store
+*.swp

README.md

@@ -1,2 +1,177 @@
-# text
+作业提交里的t_shape代码有错，这里更新了一下
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import time
 
+def main():
+    rclpy.init()
+    
+    node = Node('circle_navigation')
+    
+    publisher = node.create_publisher(Twist, '/cmd_vel', 10)
+    
+    node.get_logger().info("=== 开始绕圈导航 ===")
+    
+
+    time.sleep(2.0)
+    
+    try:
+
+        movements = [
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒回到起点", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+        ]
+        
+        for i, move in enumerate(movements):
+            node.get_logger().info(f"动作 {i+1}/{len(movements)}: {move['name']}")
+            
+            twist = Twist()
+            twist.linear.x = move['linear']
+            twist.angular.z = move['angular']
+            publisher.publish(twist)
+            
+            time.sleep(move['duration'])
+        
+        node.get_logger().info("=== 绕圈导航完成 ===")
+        
+    finally:
+        stop_twist = Twist()
+        publisher.publish(stop_twist)
+        node.get_logger().info("机器人已停止")
+        node.get_logger().info("很多没明白用AI写的呜呜呜")
+        
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()
+
+##################################################################################################################
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import time
+
+def main():
+    rclpy.init()
+    
+    node = Node('t_shape_movement')
+    publisher = node.create_publisher(Twist, '/cmd_vel', 10)
+    
+    node.get_logger().info("=== 开始T字形运动控制 ===")
+    time.sleep(2.0)
+    
+    cycle_count = 0
+    
+    try:
+        while True:
+            cycle_count += 1
+            node.get_logger().info(f"=== 第 {cycle_count} 次T字形循环开始 ===")
+            
+            # 1. 从起点到中心点（竖线）- 面向前
+            node.get_logger().info("竖线: 向前移动到中心点")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(2.0)
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            # 2. 左横线：左转90度前进然后返回
+            node.get_logger().info("左横线: 左转90度")
+            twist = Twist()
+            twist.angular.z = 0.5
+            publisher.publish(twist)
+            time.sleep(3.14)  # 90度，现在面向左
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("左横线: 向左前进")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 左横线前进
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            node.get_logger().info("左横线: 后退回中心")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 后退到中心，仍面向左
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            # 3. 右横线：右转180度前进然后返回
+            node.get_logger().info("右横线: 右转180度")
+            twist = Twist()
+            twist.angular.z = -0.5
+            publisher.publish(twist)
+            time.sleep(6.28)  # 180度，从面向左转到面向右
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("右横线: 向右前进")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 右横线前进
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            node.get_logger().info("右横线: 后退回中心")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 后退到中心，仍面向右
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            # 4. 回到起点：左转90度后退
+            node.get_logger().info("回到起点: 左转90度")
+            twist = Twist()
+            twist.angular.z = 0.5
+            publisher.publish(twist)
+            time.sleep(3.14)  # 90度，从面向右转到面向前
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("回到起点: 向后移动")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(2.0)  # 后退到起点
+            
+            node.get_logger().info(f"=== 第 {cycle_count} 次T字形完成 ===")
+            node.get_logger().info("等待3秒后开始下一次循环...")
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(3.0)
+            
+    except KeyboardInterrupt:
+        node.get_logger().info("程序被用户中断")
+    except Exception as e:
+        node.get_logger().error(f"发生错误: {e}")
+    finally:
+        twist = Twist()
+        publisher.publish(twist)
+        node.get_logger().info("机器人已停止")
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/movement_task/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>movement_task</name>
+  <version>0.0.0</version>
+  <description>T-shape movement task for robot</description>
+  <maintainer email="iden_2024@qq.com">iden</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/movement_task/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/movement_task
+[install]
+install_scripts=$base/lib/movement_task

src/movement_task/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'movement_task'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[],
+    py_modules=[
+        'text.t_shape',
+    ],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'text'), glob('text/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='your_name',
+    maintainer_email='your_email@example.com',
+    description='T-shape movement task for robot',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            't_shape = text.t_shape:main',
+        ],
+    },
+)

src/movement_task/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/movement_task/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/movement_task/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/movement_task/text/t_shape.py

@@ -0,0 +1,120 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import time
+
+def main():
+    rclpy.init()
+    
+    node = Node('t_shape_movement')
+    publisher = node.create_publisher(Twist, '/cmd_vel', 10)
+    
+    node.get_logger().info("=== 开始T字形运动控制 ===")
+    time.sleep(2.0)
+    
+    cycle_count = 0
+    
+    try:
+        while True:
+            cycle_count += 1
+            node.get_logger().info(f"=== 第 {cycle_count} 次T字形循环开始 ===")
+            
+            # 1. 从起点到中心点（竖线）- 面向前
+            node.get_logger().info("竖线: 向前移动到中心点")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(2.0)
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            # 2. 左横线：左转90度前进然后返回
+            node.get_logger().info("左横线: 左转90度")
+            twist = Twist()
+            twist.angular.z = 0.5
+            publisher.publish(twist)
+            time.sleep(3.14)  # 90度，现在面向左
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("左横线: 向左前进")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 左横线前进
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            node.get_logger().info("左横线: 后退回中心")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 后退到中心，仍面向左
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            # 3. 右横线：右转180度前进然后返回
+            node.get_logger().info("右横线: 右转180度")
+            twist = Twist()
+            twist.angular.z = -0.5
+            publisher.publish(twist)
+            time.sleep(6.28)  # 180度，从面向左转到面向右
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("右横线: 向右前进")
+            twist = Twist()
+            twist.linear.x = 0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 右横线前进
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.5)
+            
+            node.get_logger().info("右横线: 后退回中心")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(1.5)  # 后退到中心，仍面向右
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            # 4. 回到起点：左转90度后退
+            node.get_logger().info("回到起点: 左转90度")
+            twist = Twist()
+            twist.angular.z = 0.5
+            publisher.publish(twist)
+            time.sleep(3.14)  # 90度，从面向右转到面向前
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(0.3)
+            
+            node.get_logger().info("回到起点: 向后移动")
+            twist = Twist()
+            twist.linear.x = -0.15
+            publisher.publish(twist)
+            time.sleep(2.0)  # 后退到起点
+            
+            node.get_logger().info(f"=== 第 {cycle_count} 次T字形完成 ===")
+            node.get_logger().info("等待3秒后开始下一次循环...")
+            twist = Twist()
+            publisher.publish(twist)
+            time.sleep(3.0)
+            
+            
+    finally:
+        twist = Twist()
+        publisher.publish(twist)
+        node.get_logger().info("机器人已停止")
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/navigation_task1/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>navigation_task1</name>
+  <version>0.0.0</version>
+  <description>Navigation task for moving in a circle</description>
+  <maintainer email="your_email@example.com">your_name</maintainer>
+  <license>Apache License 2.0</license>
+
+  <depend>rclpy</depend>
+  <depend>geometry_msgs</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/navigation_task1/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/navigation_task1
+[install]
+install_scripts=$base/lib/navigation_task1

src/navigation_task1/setup.py

@@ -0,0 +1,32 @@
+from setuptools import setup
+import os
+from glob import glob
+
+package_name = 'navigation_task1'
+
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=[],
+    py_modules=[
+        'text.circle_nav',
+    ],
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'text'), glob('text/*.py')),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='your_name',
+    maintainer_email='your_email@example.com',
+    description='Navigation task for moving in a circle',
+    license='Apache License 2.0',
+    tests_require=['pytest'],
+    entry_points={
+        'console_scripts': [
+            'circle_nav = text.circle_nav:main',
+        ],
+    },
+)

src/navigation_task1/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/navigation_task1/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/navigation_task1/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/navigation_task1/text/circle_nav.py

@@ -0,0 +1,53 @@
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import Twist
+import time
+
+def main():
+    rclpy.init()
+    
+    node = Node('circle_navigation')
+    
+    publisher = node.create_publisher(Twist, '/cmd_vel', 10)
+    
+    node.get_logger().info("=== 开始绕圈导航 ===")
+    
+
+    time.sleep(2.0)
+    
+    try:
+
+        movements = [
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+            {"name": "左转2秒", "linear": 0.0, "angular": 0.5, "duration": 2.0},
+            {"name": "前进3秒回到起点", "linear": 0.2, "angular": 0.0, "duration": 3.0},
+        ]
+        
+        for i, move in enumerate(movements):
+            node.get_logger().info(f"动作 {i+1}/{len(movements)}: {move['name']}")
+            
+            twist = Twist()
+            twist.linear.x = move['linear']
+            twist.angular.z = move['angular']
+            publisher.publish(twist)
+            
+            time.sleep(move['duration'])
+        
+        node.get_logger().info("=== 绕圈导航完成 ===")
+        
+    finally:
+        stop_twist = Twist()
+        publisher.publish(stop_twist)
+        node.get_logger().info("机器人已停止")
+        node.get_logger().info("很多没明白用AI写的呜呜呜")
+        
+        node.destroy_node()
+        rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()