ros2乌龟跟随仿真作业

肥羊粉。

已于 2024-10-29 23:04:39 修改

阅读量855

点赞数 6

CC 4.0 BY-SA版权

文章标签： python 开发语言

于 2024-10-28 20:41:09 首次发布

本文链接：https://blog.youkuaiyun.com/cfmaker/article/details/143312718

我看我室友写了一个ros2的海龟跟随实验作业，是c++版本的，我学习了一下，借鉴了他的以及网上的一些教程，也写了一个python版本的海龟跟随功能包，但是我写代码的能力还是偏弱，最后代码相对比较冗长。

首先打开一个终端，在终端输入代码

ros2 run turtlesim turtlesim_node

可以正常运行turtlesim模拟环境。

创建工作空间，并进入

mkdir -p hw_ws/src
cd hw_ws

创建功能包，填入包名，类型选择ament_python，然后配好依赖

ros2 pkg create hw1 --build-type ament_python --dependencies rclpy tf2 tf2_ros tf2_geometry_msgs std_msgs os glob geometry_msgs turtlesim

编译文件夹并source一下，让系统能识别到这个功能包

colcon build --packages-select hw1
source install/setup.bash

下面编写功能包中的节点文件

乌龟的生成文件，命名乌龟为turtle2，因为启动turtlesim节点的时候就已经会产生一个turtle1乌龟了，文件名为turtle_spawner.py

import rclpy
from rclpy.node import Node
from turtlesim.srv import Spawn

class TurtleSpawner(Node):
    def __init__(self):
        super().__init__('turtle_spawner')
        self.spawn_turtles()

    def spawn_turtles(self):
        # 创建服务客户端
        client = self.create_client(Spawn, 'spawn')
        while not client.wait_for_service(timeout_sec=1.0):
            self.get_logger().info('服务未准备好，正在等待...')

        # 第二个乌龟
        request2 = Spawn.Request()
        request2.x = 1.0
        request2.y = 1.0
        request2.theta = 0.0
        request2.name = 'turtle2'

        future2 = client.call_async(request2)

        rclpy.spin_until_future_complete(self, future2)

def main(args=None):
    rclpy.init(args=args)
    turtle_spawner = TurtleSpawner()
    turtle_spawner.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

订阅乌龟位置信息并发布，在终端上进行显示，文件名为turtle_position_publisher.py

import rclpy
from rclpy.node import Node
from turtlesim.msg import Pose
from std_msgs.msg import String

class TurtlePositionPublisher(Node):
    def __init__(self):
        super().__init__('turtle_position_publisher')
        
        # 创建订阅者，订阅乌龟的位置信息
        self.create_subscription(Pose, '/turtle1/pose', self.pose_callback, 10)

        # 创建发布者，发布位置信息
        self.position_publisher = self.create_publisher(String, '/turtle1/coordinates', 10)

    def pose_callback(self, msg):
        # 构建位置信息字符串
        position_info = f'Turtle1 Position -> x: {msg.x}, y: {msg.y}, theta: {msg.theta}'
        
        # 发布位置信息
        self.position_publisher.publish(String(data=position_info))
        self.get_logger().info(position_info)

def main(args=None):
    rclpy.init(args=args)
    node = TurtlePositionPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

将乌龟的坐标信息转变成坐标变换并订阅发布，在跟踪节点中方便订阅并跟随,文件名为turtle_transform_publisher.py。

import rclpy
from rclpy.node import Node
from geometry_msgs.msg import TransformStamped
import tf_transformations
from tf2_ros import TransformBroadcaster
from turtlesim.msg import Pose

class TurtleTFBroadcaster(Node):

    def __init__(self, name):
        super().__init__(name)
        self.declare_parameter('turtlename', 'turtle')
        self.turtlename = self.get_parameter('turtlename').get_parameter_value().string_value
        self.tf_broadcaster = TransformBroadcaster(self)
        self.subscription = self.create_subscription(
            Pose,
            f'/{self.turtlename}/pose',
            self.turtle_pose_callback, 1)

    def turtle_pose_callback(self, msg):
        transform = TransformStamped()
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'world'
        transform.child_frame_id = self.turtlename
        transform.transform.translation.x = msg.x
        transform.transform.translation.y = msg.y
        transform.transform.translation.z = 0.0
        q = tf_transformations.quaternion_from_euler(0, 0, msg.theta)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        self.tf_broadcaster.sendTransform(transform)

def main(args=None):
    rclpy.init(args=args)
    node = TurtleTFBroadcaster("turtle_transform_publisher")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

turtle2跟踪turtle1的跟随代码，这里用的是我网上看的比较多的tf跟随控制，就是把距离和角度差乘以一个系数就发布成控制指令，但是会让turtle2快一阵慢一阵的，放到真实情况实现不了，后面还有用PID和模糊控制方法控制的。
这里给turtle2发布速度指令，文件名为turtle_follower.py

import math
import rclpy
from rclpy.node import Node
import tf_transformations
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from geometry_msgs.msg import Twist
from turtlesim.srv import Spawn

class TurtleFollowing(Node):

    def __init__(self, name):
        super().__init__(name)
        self.declare_parameter('source_frame', 'turtle1')
        self.source_frame = self.get_parameter('source_frame').get_parameter_value().string_value
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.spawner = self.create_client(Spawn, 'spawn')
        self.turtle_spawning_service_ready = False
        self.turtle_spawned = False
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
        self.timer = self.create_timer(1.0, self.on_timer)

    def on_timer(self):
        from_frame_rel = self.source_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

                msg = Twist()
                scale_rotation_rate = 1.0
                msg.angular.z = scale_rotation_rate * math.atan2(
                    trans.transform.translation.y,
                    trans.transform.translation.x)

                scale_forward_speed = 0.5
                msg.linear.x = scale_forward_speed * math.sqrt(
                    trans.transform.translation.x ** 2 +
                    trans.transform.translation.y ** 2)

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)

                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                self.get_logger().info('Service is not ready')

def main(args=None):
    rclpy.init(args=args)
    node = TurtleFollowing("turtle_follower")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

在src文件夹中新建一个launch文件夹，再编写launch文件，以此启动多个节点。文件名为follow.launch.py

from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node

def generate_launch_description():
    return LaunchDescription([
     
        Node(
            package='turtlesim',
            executable='turtlesim_node',
            name='turtlesim',
            output='screen'
        ),
        Node(
            package='hw1',
            executable='turtle_spawner',
            name = 'turtle2_spawn'
        ),

        Node(
            package='hw1',
            executable='turtle_transform_publisher',
            name='publisher1',
            parameters=[
                {'turtlename': 'turtle1'}
            ]
        ),
        DeclareLaunchArgument(
            'target_frame', default_value='turtle1',
            description='Target frame name.'
        ),
        Node(
            package='hw1',
            executable='turtle_transform_publisher',
            name='publisher2',
            parameters=[
                {'turtlename': 'turtle2'}
            ]
        ),
        Node(
            package='hw1',
            executable='turtle_follower',
            name='follower1',
            parameters=[
                {'target_frame': LaunchConfiguration('target_frame')}
            ]
        ), 
    ])

将setup.py文件进行修改，让系统识别节点名称对应的节点文件

from setuptools import find_packages, setup
import os
from glob import glob

package_name = 'hw1'

setup(
    name=package_name,
    version='0.0.0',
    packages=find_packages(exclude=['test']),
    data_files=[
        ('share/ament_index/resource_index/packages',
            ['resource/' + package_name]),
        ('share/' + package_name, ['package.xml']),
        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
    ],
    install_requires=['setuptools'],
    zip_safe=True,
    maintainer='cf',
    maintainer_email='cf@todo.todo',
    description='TODO: Package description',
    license='TODO: License declaration',
    tests_require=['pytest'],
    entry_points={
        'console_scripts': [
             'turtle_position_publisher = hw1.turtle_position_publisher:main',
             'turtle_follower = hw1.turtle_follower:main',
             'turtle_transform_publisher = hw1.turtle_transform_publisher:main',
             'turtle_spawner = hw1.turtle_spawner:main',           
        ],
    },
)

检查一遍package.xml文件中的依赖是否缺少

<?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>hw1</name>
  <version>0.0.0</version>
  <description>TODO: Package description</description>
  <maintainer email="cf@todo.todo">cf</maintainer>
  <license>TODO: License declaration</license>

  <depend>rclpy</depend>
  <depend>tf2</depend>
  <depend>tf2_ros</depend>
  <depend>tf2_geometry_msgs</depend>
  <depend>std_msgs</depend>
  <depend>geometry_msgs</depend>
  <depend>turtlesim</depend>
  <depend>pynput</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>

一切就绪就可以对功能包进行编译运行了，打开一个终端输入

cd hw_ws #进入工作空间
colcon build --packages-select hw1 #编译功能包
source install/setup.bash #source一下

运行launch文件

ros2 launch hw1 follow.launch.py

然后就能看见turtle2跟踪到turtle1

在这里插入图片描述

这时候我们再打开一个终端，输入

ros2 run turtlesim turtle_teleop_key

就可以控制小乌龟turtle1的运动，使用方向键盘操纵即可，可以看见turtle2依然在跟踪turtle1

PID控制实现

修改turtle_follower.py文件即可,代码如下，这里由于系统本身能够在稳态时保持较小的误差，不需要积分环节来消除稳态误差，所以没有加入ki。

import math
import rclpy
from rclpy.node import Node
import tf_transformations
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from geometry_msgs.msg import Twist
from turtlesim.srv import Spawn

class TurtleFollowing(Node):

    def __init__(self, name):
        super().__init__(name)
        self.declare_parameter('source_frame', 'turtle1')
        self.source_frame = self.get_parameter('source_frame').get_parameter_value().string_value
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.spawner = self.create_client(Spawn, 'spawn')
        self.turtle_spawning_service_ready = False
        self.turtle_spawned = False
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
        self.timer = self.create_timer(1.0, self.on_timer)
        self.kp_linear = 0.5
        self.kd_linear = 0.1
        self.kp_angular = 1.0
        self.kd_angular = 0.1
        self.prev_distance = 0.0
        self.prev_angle = 0.0

    def on_timer(self):
        from_frame_rel = self.source_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

                msg = Twist()
                distance = math.sqrt(trans.transform.translation.x ** 2 +
                                     trans.transform.translation.y ** 2)
                angle = math.atan2(trans.transform.translation.y, trans.transform.translation.x)

                linear_speed = self.kp_linear * distance + self.kd_linear * (distance - self.prev_distance)
                angular_speed = self.kp_angular * angle + self.kd_angular * (angle - self.prev_angle)

                msg.linear.x = max(0.0, linear_speed)
                msg.angular.z = angular_speed

                self.publisher.publish(msg)
                self.prev_distance = distance
                self.prev_angle = angle
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)

                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                self.get_logger().info('Service is not ready')

def main(args=None):
    rclpy.init(args=args)
    node = TurtleFollowing("turtle_follower")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

结果如下：
在这里插入图片描述

模糊控制实现

将修改turtle_follower.py文件即可,代码如下

import math
import rclpy
from rclpy.node import Node
import tf_transformations
from tf2_ros import TransformException
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
from geometry_msgs.msg import Twist
from turtlesim.srv import Spawn

class FuzzyController:
    def __init__(self):
        pass

    def fuzzy_control(self, distance, angle):
        linear_speed = 0.0
        angular_speed = 0.0

        if distance < 0.5:
            linear_speed = 0.0
        elif distance < 1.0:
            linear_speed = 0.3
        elif distance < 2.0:
            linear_speed = 0.5
        else:
            linear_speed = 1.0

        if angle < -0.3:
            angular_speed = -1.0
        elif angle < 0.0:
            angular_speed = -0.5
        elif angle > 0.3:
            angular_speed = 1.0
        elif angle > 0.0:
            angular_speed = 0.5
        else:
            angular_speed = 0.0

        return linear_speed, angular_speed

class TurtleFollowing(Node):

    def __init__(self, name):
        super().__init__(name)
        self.declare_parameter('source_frame', 'turtle1')
        self.source_frame = self.get_parameter('source_frame').get_parameter_value().string_value
        self.tf_buffer = Buffer()
        self.tf_listener = TransformListener(self.tf_buffer, self)
        self.spawner = self.create_client(Spawn, 'spawn')
        self.turtle_spawning_service_ready = False
        self.turtle_spawned = False
        self.publisher = self.create_publisher(Twist, 'turtle2/cmd_vel', 1)
        self.timer = self.create_timer(0.1, self.on_timer)  # Increase frequency
        self.fuzzy_controller = FuzzyController()

    def on_timer(self):
        from_frame_rel = self.source_frame
        to_frame_rel = 'turtle2'

        if self.turtle_spawning_service_ready:
            if self.turtle_spawned:
                try:
                    now = rclpy.time.Time()
                    trans = self.tf_buffer.lookup_transform(
                        to_frame_rel,
                        from_frame_rel,
                        now)
                except TransformException as ex:
                    self.get_logger().info(
                        f'Could not transform {to_frame_rel} to {from_frame_rel}: {ex}')
                    return

                msg = Twist()
                distance = math.sqrt(trans.transform.translation.x ** 2 +
                                     trans.transform.translation.y ** 2)
                angle = math.atan2(trans.transform.translation.y, trans.transform.translation.x)

                linear_speed, angular_speed = self.fuzzy_controller.fuzzy_control(distance, angle)

                msg.linear.x = max(0.0, linear_speed)
                msg.angular.z = angular_speed

                self.publisher.publish(msg)
            else:
                if self.result.done():
                    self.get_logger().info(
                        f'Successfully spawned {self.result.result().name}')
                    self.turtle_spawned = True
                else:
                    self.get_logger().info('Spawn is not finished')
        else:
            if self.spawner.service_is_ready():
                request = Spawn.Request()
                request.name = 'turtle2'
                request.x = float(4)
                request.y = float(2)
                request.theta = float(0)

                self.result = self.spawner.call_async(request)
                self.turtle_spawning_service_ready = True
            else:
                self.get_logger().info('Service is not ready')

def main(args=None):
    rclpy.init(args=args)
    node = TurtleFollowing("turtle_follower")
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()