ROS Python：订阅海龟Pose数据修改后重发问题求助

问题根源 #

核心问题是回调函数中未正确声明全局变量作用域，导致回调内的变量赋值仅在局部生效，无法修改全局作用域的Pose变量；或是全局变量管理方式不严谨，引发作用域混淆。

修正方案 #

以下提供两种可行解决方式，推荐使用类封装的方式，避免全局变量带来的代码维护问题。

方式1：正确使用全局变量 #

通过global关键字明确声明回调函数中使用的变量属于全局作用域，确保Pose数据能被正确捕获并修改：

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose

# 初始化全局Pose变量
modified_pose = None

def pose_callback(msg):
    global modified_pose
    # 复制原始Pose数据
    modified_pose = Pose()
    modified_pose.x = msg.x
    modified_pose.y = msg.y
    modified_pose.theta = msg.theta
    modified_pose.linear_velocity = msg.linear_velocity
    modified_pose.angular_velocity = msg.angular_velocity
    
    # 执行自定义修改逻辑，例如偏移坐标
    modified_pose.x += 2.0
    modified_pose.y += 2.0

def main():
    rospy.init_node('pose_modifier', anonymous=True)
    # 订阅turtlesim1的Pose话题
    rospy.Subscriber('/turtlesim1/turtle1/pose', Pose, pose_callback)
    # 发布修改后的Pose到turtlesim2的自定义话题
    pub = rospy.Publisher('/turtlesim2/turtle1/modified_pose', Pose, queue_size=10)
    rate = rospy.Rate(10)  # 10Hz发布频率

    while not rospy.is_shutdown():
        if modified_pose is not None:
            pub.publish(modified_pose)
        rate.sleep()

if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass

方式2：类封装状态（推荐） #

将Pose变量作为类的实例属性，通过类方法处理订阅和发布逻辑，彻底避免全局变量的作用域问题：

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose

class PoseModifierNode:
    def __init__(self):
        self.modified_pose = None
        # 订阅turtlesim1的Pose话题
        self.pose_sub = rospy.Subscriber('/turtlesim1/turtle1/pose', Pose, self.pose_callback)
        # 发布修改后的Pose到turtlesim2
        self.pose_pub = rospy.Publisher('/turtlesim2/turtle1/modified_pose', Pose, queue_size=10)
        self.rate = rospy.Rate(10)

    def pose_callback(self, msg):
        # 复制并修改Pose数据
        self.modified_pose = msg
        self.modified_pose.x += 1.5  # 自定义修改逻辑
        self.modified_pose.y += 1.5

    def run(self):
        while not rospy.is_shutdown():
            if self.modified_pose is not None:
                self.pose_pub.publish(self.modified_pose)
            self.rate.sleep()

if __name__ == '__main__':
    rospy.init_node('pose_modifier_class')
    node = PoseModifierNode()
    try:
        node.run()
    except rospy.ROSInterruptException:
        pass

补充说明 #

如果实际需求是控制turtlesim2的turtle1移动到修改后的位置（而非单纯发布Pose话题），需要发布geometry_msgs/Twist消息到/turtlesim2/turtle1/cmd_vel话题，通过速度控制实现跟随。示例代码如下：

#!/usr/bin/env python3
import rospy
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

class TurtleFollower:
    def __init__(self):
        self.target_pose = None
        # 订阅turtlesim1的Pose作为目标
        self.target_sub = rospy.Subscriber('/turtlesim1/turtle1/pose', Pose, self.target_callback)
        # 发布速度指令到turtlesim2
        self.vel_pub = rospy.Publisher('/turtlesim2/turtle1/cmd_vel', Twist, queue_size=10)
        self.rate = rospy.Rate(10)
        # PID控制参数
        self.kp_linear = 1.2
        self.kp_angular = 4.0

    def target_callback(self, msg):
        # 修改目标位置，例如偏移3个单位
        self.target_pose = msg
        self.target_pose.x += 3.0
        self.target_pose.y += 3.0

    def normalize_angle(self, angle):
        # 将角度归一化到[-π, π]区间
        while angle > rospy.get_pi():
            angle -= 2 * rospy.get_pi()
        while angle < -rospy.get_pi():
            angle += 2 * rospy.get_pi()
        return angle

    def compute_velocity(self, current_pose):
        twist = Twist()
        # 计算位置误差
        dx = self.target_pose.x - current_pose.x
        dy = self.target_pose.y - current_pose.y
        # 计算距离和角度误差
        distance = (dx**2 + dy**2)**0.5
        target_angle = rospy.atan2(dy, dx)
        angle_error = self.normalize_angle(target_angle - current_pose.theta)

        # 计算速度指令
        twist.linear.x = self.kp_linear * distance
        twist.angular.z = self.kp_angular * angle_error
        return twist

    def run(self):
        # 先获取turtlesim2的初始Pose
        try:
            current_pose = rospy.wait_for_message('/turtlesim2/turtle1/pose', Pose, timeout=5)
        except rospy.ROSException:
            rospy.logerr("无法获取turtlesim2的Pose数据")
            return

        while not rospy.is_shutdown():
            if self.target_pose is not None:
                try:
                    current_pose = rospy.wait_for_message('/turtlesim2/turtle1/pose', Pose, timeout=0.1)
                    twist = self.compute_velocity(current_pose)
                    self.vel_pub.publish(twist)
                except rospy.ROSException:
                    pass
            self.rate.sleep()

if __name__ == '__main__':
    rospy.init_node('turtle_follower')
    follower = TurtleFollower()
    try:
        follower.run()
    except rospy.ROSInterruptException:
        pass

内容的提问来源于stack exchange，提问作者Enes