我现在正在做RRT*路径规划的作业，遇到了一个棘手的问题——没法给turtlesim3的Gazebo仿真模型发布/cmd_vel速度指令。我用rqt_graph查看了节点连接情况，显示节点之间是连通的，但用rostopic echo /cmd_vel查看话题内容时，完全没有消息输出。不过用teleop工具或者直接在终端里手动发指令是能正常控制机器人的，说明话题本身没问题。

下面是我的代码：

#!/usr/bin/env python3
import matplotlib.pyplot as plt
import random
import math
import copy
import rospy
import time
import numpy as np
from numpy import genfromtxt
from numpy import array
from sensor_msgs.msg import NavSatFix
from sensor_msgs.msg import Imu
from std_msgs.msg import String
from geometry_msgs.msg import Vector3Stamped
from geometry_msgs.msg import Twist
from geometry_msgs.msg import Pose2D
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from std_msgs.msg import Float32MultiArray

show_animation = True

class RRT():
    """
    Class for RRT Planning
    """
    
    def __init__(self, start, goal, obstacles,
                 randArea, expandDis=1.0, goalSampleRate=5, maxIter = 500):
        """
        Setting Parameter

        start: Start Position [x, y]
        goal: Goal Position [x, y]
        obstacles: Obstacle Positions [[x, y, size], ...]
        randArea: Random Sampling Area [min, max]
        """
        self.start = Node(start[0], start[1])
        self.end = Node(goal[0], goal[1])
        self.minrand = randArea[0]
        self.maxrand = randArea[1]
        self.expandDis = expandDis
        self.goalSampleRate = goalSampleRate
        self.maxIter = maxIter
        self.obstacles = obstacles

    def Planning(self, animation=True):
        """
        Path planning

        animation: flag for animation on or off
        """
        self.nodeList = [self.start]
        while True:
            # Random Sampling
            if random.randint(0, 100) > self.goalSampleRate:
                rnd = [random.uniform(self.minrand, self.maxrand), random.uniform(self.minrand, self.maxrand)]
            else:
                rnd = [self.end.x, self.end.y]

            # Find nearest node
            nind = self.GetNearestListIndex(self.nodeList, rnd)

            # Expand tree
            nearestNode = self.nodeList[nind]
            theta = math.atan2(rnd[1] - nearestNode.y, rnd[0] - nearestNode.x)

            newNode = copy.deepcopy(nearestNode)
            newNode.x += self.expandDis * math.cos(theta)
            newNode.y += self.expandDis * math.sin(theta)
            newNode.parent = nind

            if not self.__CollisionCheck(newNode, self.obstacles):
                continue

            self.nodeList.append(newNode)
            print("Node list length:", len(self.nodeList))

            # Check goal
            dx = newNode.x - self.end.x
            dy = newNode.y - self.end.y
            d = math.sqrt(dx * dx + dy * dy)
            if d <= self.expandDis:
                print("Goal reached!")
                break

            if animation:
                self.DrawGraph(rnd)

        path = [[self.end.x, self.end.y]]
        lastIndex = len(self.nodeList) - 1
        while self.nodeList[lastIndex].parent is not None:
            node = self.nodeList[lastIndex]
            path.append([node.x, node.y])
            lastIndex = node.parent
        path.append([self.start.x, self.start.y])

        return path

    def DrawGraph(self, rnd=None):
        """
        Draw Graph
        """
        plt.clf()
        if rnd is not None:
            plt.plot(rnd[0], rnd[1], "^k")
        for node in self.nodeList:
            if node.parent is not None:
                plt.plot([node.x, self.nodeList[node.parent].x], [
                         node.y, self.nodeList[node.parent].y], "-g")

        for (ox, oy, size) in self.obstacles:
            plt.plot(ox, oy, "ok", ms=30 * size)

        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.end.x, self.end.y, "xr")
        plt.axis([-10, 10, -10, 10])
        plt.grid(True)
        plt.pause(0.01)

    def GetNearestListIndex(self, nodeList, rnd):
        dlist = [(node.x - rnd[0]) ** 2 + (node.y - rnd[1]) ** 2 for node in nodeList]
        minind = dlist.index(min(dlist))
        return minind

    def __CollisionCheck(self, node, obstacles):
        for (ox, oy, size) in obstacles:
            dx = ox - node.x
            dy = oy - node.y
            d = math.sqrt(dx * dx + dy * dy)
            if d <= size:
                return False  # Collision

        return True  # Safe


class Node():
    """
    RRT Node
    """
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None


# Variable to store robot's current position
current_position = [0, 0]  # Initial position of the robot

def topic2_callback(msg):
    """
    Callback function for receiving robot's position from /topic2
    This function updates the robot's position with data from Float32MultiArray.
    """
    global current_position

    # Assuming msg.data contains the position as [x, y]
    current_position = msg.data
    print(f"Received robot position: {current_position}")


def goal_pose_callback(msg):
    """
    Callback function for receiving goal pose via /goal_pose topic.
    """
    print(f"Received new goal position: ({msg.x}, {msg.y})")
    start_position = current_position[:2]  # Start position from current robot position
    rand_area = [-15, 15]  # Define the random sampling area for the planner
    
    # Load obstacles from file
    f = open("/home/ercan/catkin_ws/src/localization_ekf/obstacles.txt", "r")
    obstacles = []
    for i in range(8):    
        x = int(f.readline())
        y = int(f.readline())
        cap = int(f.readline())
        obstacles.append((int(x), int(y), int(cap)))
    f.close()

    # Plan the path using RRT
    rrt = RRT(start=start_position, goal=[msg.x, msg.y], randArea=rand_area, obstacles=obstacles)
    path = rrt.Planning(animation=show_animation)

    # Draw the final path
    if show_animation:
        rrt.DrawGraph()
        plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
        plt.grid(True)
        plt.show()

    # Return the path
    follow_path(path, pub, rate)


def follow_path(path, pub, rate):
    """
    Function to follow the generated path using a simple control approach
    """
    # Robot speed and control parameters
    linear_speed = 0.5
    angular_speed = 1.0

    for i in range(1, len(path)):
        x1, y1 = path[i - 1]
        x2, y2 = path[i]

        # Calculate the distance and angle to the next waypoint
        distance = math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2)
        angle = math.atan2(y2 - y1, x2 - x1)

        # Initialize the Twist message
        twist = Twist()

        while distance > 0.1:  # Continue moving until the robot reaches the waypoint
            # Calculate the angle difference
            angle_to_goal = math.atan2(y2 - current_position[1], x2 - current_position[0])
            angle_diff = angle_to_goal - current_position[2]

            # Normalize the angle difference to the range [-pi, pi]
            angle_diff = (angle_diff + math.pi) % (2 * math.pi) - math.pi

            # Adjust velocities based on distance and angle difference
            if abs(angle_diff) > 0.1:
                twist.linear.x = 0.0  # Stop moving forward if we need to rotate
                twist.angular.z = angular_speed * angle_diff  # Rotate towards the goal
            else:
                twist.linear.x = linear_speed  # Move forward
                twist.angular.z = 0.0  # Stop rotating

            pub.publish(twist)
            rate.sleep()

            # Update current position (for simulation purposes, assuming we update it here)
            current_position[0] += twist.linear.x * 0.1  # Update x based on linear speed
            current_position[1] += twist.linear.x * 0.1  # Update y based on linear speed
            distance = math.sqrt((x2 - current_position[0]) ** 2 + (y2 - current_position[1]) ** 2)

    twist.linear.x = 0
    twist.angular.z = 0
    pub.publish(twist)  # Stop the robot when finished


def main():
    # Initialize the ROS node
    rospy.init_node('motion_planning_node')

    # Create a publisher to send /cmd_vel messages
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
    rate = rospy.Rate(10) # 10hz
    # Print waiting message before goal input
    print("Waiting for goal position input...")

    # Subscribe to the /goal_pose topic to get the goal position
    rospy.Subscriber('/goal_pose', Pose2D, goal_pose_callback)

    # Subscribe to the /topic2 topic to get the robot's current position
    rospy.Subscriber('/topic2', Float32MultiArray, topic2_callback)

    rospy.spin()


if __name__ == '__main__':
    main()

我感觉问题应该出在代码的位置或者执行顺序上，但找了好久都没发现哪里错了，有没有大佬能帮我看看呀？

备注：内容来源于stack exchange，提问作者Ercan İnan