python编写ros控制乌龟移动报错，如何解决？

Question

随机生成一个小乌龟，小乌龟在一个无限大小的 XY 网格平面上行走，从点 (0, 0) 处开始出发，面向北方。该小乌龟可以接收以下三种类型的命令 commands ：
-2 ：向左转 90 度
-1 ：向右转 90 度
1 <= x <= 9 ：向前移动 x 个单位长度
在网格上有一些格子被视为障碍物 obstacles 。第 i 个障碍物位于网格点 obstacles[i] =(xi, yi) 。障碍物通过额外生成小乌龟展示在界面上。
小乌龟无法走到障碍物上，它将会停留在障碍物的前一个网格方块上，并继续执行下一个命令。
做出小乌龟的行进路线图，并返回小乌龟距离原点的最大欧氏距离的平方。

代码如下

#! /usr/bin/env python

import math
import rospy
import time
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist
 

class Guiguirun:
    def __init__(self):
        self.pub = rospy.Publisher ('/turtle1/cmd_vel', Twist , queue_size =10)
        self.x = 0
        self.y = 0
        self.theta = math.pi/2
        self.goal_x = 0
        self.goal_y = 0
        self.goal_theta = math.pi/2
        self.error = 0
        self.aim_line = 0.035
        self.aim_angle = 0.01 
        self.twist = Twist()
        self.c = 0

# 走直线
def go_line(self):
    self.pose_get(Pose)
    self.error = math.sqrt((self.x - self.goal_x) ** 2+ (self.y - self.goal_y) ** 2)
    if self.error > self.aim_line:
        self.twist.linear.x = 1.0
    else:
        self.twist.linear.x = 0
    self.pub.publish(self.twist)

# 旋转
def rorate(self):
    self.pose_get(Pose)
    self.error = abs(self.theta - self.goal_theta)
    if self.error > self.aim_angle:
        if self.c == -1:
            self.twist.angular.z = - 1.0
        elif self.c == -2:
            self.twist.angular.z = 1.0
    else:
        self.twist.angular.z = 0
    self.pub.publish(self.twist)

# 设定目标移动
def guigui_move(self):
    commands = [4, -1 ,4, -1, 4]
    obstacles = [[2, 4]]
    dirs = [[-1, 0], [0, 1], [1, 0], [0, -1]]
    px, py, d = 0, 0, 1
    mp = set([tuple(i) for i in obstacles])
    res = 0
    for self.c in commands:
        if self.c == -1:
            d += 1
            d %= 4
            self.goal_theta -= -math.pi / 2
            self.rorate()
            while True:
                self.rorate()
                if self.twist.angular.z == 0:
                    break
        elif self.c == -2:
            d -= 1
            d %= 4
            self.goal_theta -= math.pi / 2
            while True:
                self.rorate()
                if self.twist.angular.z == 0:
                    break
        else:
            for i in range(self.c):
                if tuple([px + dirs[d][0], py + dirs[d][1]]) in mp:
                    break
                px, py = px + dirs[d][0], py + dirs[d][1]
                self.goal_x = self.goal_x + dirs[d][0]
                self.goal_y = self.goal_y + dirs[d][1]
                while True:
                   self.go_line()
                   if self.twist.angular.x == 0:
                    break
                res = max(res, px * px + py * py)
    print(res)

if __name__ == "__main__":

    rospy. init_node ('guigui_run', anonymous =True)
    guigui_run = Guiguirun()
    pose_subscriber = rospy. Subscriber ('/turtle1/pose', Pose , guigui_run.pose_get )
    
    time.sleep (2)

    guigui__run.guigui_move()

报错如下

Answer 1

该回答引用自GPT-4o, 由博主 GISer Liu 编写：

问题分析

用户在使用Python编写ROS控制乌龟移动的代码时遇到了报错。代码的主要功能是控制一个小乌龟在一个无限大小的XY网格平面上行走，并处理障碍物。代码中包含了乌龟的移动、旋转以及障碍物的检测逻辑。报错信息未提供，但我们可以通过代码分析来定位可能的问题。

代码分析

1. 初始化问题：

   • def _init_(self): 应该是 def __init__(self):，这是Python类初始化方法的标准写法。

2. 方法调用问题：

   • self.rorate() 和 self.go_line() 方法在循环中被调用，但没有更新乌龟的当前位置和方向。

3. 方向更新问题：

   • 在旋转和移动过程中，乌龟的方向和位置没有及时更新，导致目标位置和方向计算错误。

4. 线性速度和角速度的混淆：

   • self.twist.angular.x 应该是 self.twist.linear.x，因为 angular 是角速度，linear 是线性速度。

5. ROS节点初始化问题：

   • rospy. init_node ('guigui_run', anonymous =True) 应该是 rospy.init_node('guigui_run', anonymous=True)，注意空格和大小写。

解决方案

1. 修正初始化方法：

   • 将 _init_ 改为 __init__。

2. 更新乌龟位置和方向：

   • 在 go_line 和 rorate 方法中，更新乌龟的当前位置和方向。

3. 修正速度属性：

   • 将 self.twist.angular.x 改为 self.twist.linear.x。

4. 修正ROS节点初始化：

   • 将 rospy. init_node 改为 rospy.init_node。

修改后的代码

#! /usr/bin/env python

import math
import rospy
import time
from turtlesim.msg import Pose
from geometry_msgs.msg import Twist

class Guiguirun:
    def __init__(self):
        self.pub = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
        self.x = 0
        self.y = 0
        self.theta = math.pi / 2
        self.goal_x = 0
        self.goal_y = 0
        self.goal_theta = math.pi / 2
        self.error = 0
        self.aim_line = 0.035
        self.aim_angle = 0.01
        self.twist = Twist()
        self.c = 0

    # 走直线
    def go_line(self):
        self.error = math.sqrt((self.x - self.goal_x) ** 2 + (self.y - self.goal_y) ** 2)
        if self.error > self.aim_line:
            self.twist.linear.x = 1.0
        else:
            self.twist.linear.x = 0
        self.pub.publish(self.twist)

    # 旋转
    def rorate(self):
        self.error = abs(self.theta - self.goal_theta)
        if self.error > self.aim_angle:
            if self.c == -1:
                self.twist.angular.z = -1.0
            elif self.c == -2:
                self.twist.angular.z = 1.0
        else:
            self.twist.angular.z = 0
        self.pub.publish(self.twist)

    # 设定目标移动
    def guigui_move(self):
        commands = [4, -1, 4, -1, 4]
        obstacles = [[2, 4]]
        dirs = [[-1, 0], [0, 1], [1, 0], [0, -1]]
        px, py, d = 0, 0, 1
        mp = set([tuple(i) for i in obstacles])
        res = 0
        for self.c in commands:
            if self.c == -1:
                d += 1
                d %= 4
                self.goal_theta -= math.pi / 2
                self.rorate()
                while True:
                    self.rorate()
                    if self.twist.angular.z == 0:
                        break
            elif self.c == -2:
                d -= 1
                d %= 4
                self.goal_theta += math.pi / 2
                while True:
                    self.rorate()
                    if self.twist.angular.z == 0:
                        break
            else:
                for i in range(self.c):
                    if tuple([px + dirs[d][0], py + dirs[d][1]]) in mp:
                        break
                    px, py = px + dirs[d][0], py + dirs[d][1]
                    self.goal_x = self.goal_x + dirs[d][0]
                    self.goal_y = self.goal_y + dirs[d][1]
                    while True:
                        self.go_line()
                        if self.twist.linear.x == 0:
                            break
                    res = max(res, px * px + py * py)
        print(res)

if __name__ == "__main__":
    rospy.init_node('guigui_run', anonymous=True)
    guigui_run = Guiguirun()
    pose_subscriber = rospy.Subscriber('/turtle1/pose', Pose, guigui_run.pose_get)

    time.sleep(2)

    guigui_run.guigui_move()

总结

通过以上修改，代码中的初始化方法、方法调用、方向更新、速度属性以及ROS节点初始化等问题都得到了修正。用户可以尝试运行修改后的代码，观察是否解决了报错问题。如果仍有问题，请提供具体的报错信息，以便进一步分析和解决。

如果该回答解决了您的问题，请采纳！如果没有，请私信联系或评论您的疑惑