Невозможно опубликовать в /cmd_vel в ROS Gazebo. ⇐ Python

[Anonymous]

В настоящее время я работаю над домашним заданием по планированию маршрута RRT* и не могу публиковать скорости в /cmd_vel для беседки черепахи Sim3. Я вижу, что узлы связаны в rqt_graph, но когда я повторяю тему, ничего не появляется. Я могу управлять роботом с помощью телеопаса и терминала. Я вижу, что узлы соединены в rqt_graph, но когда я повторяю тему, ничего не появляется. Я могу управлять роботом с помощью телеопаса и терминала.

Код: Выделить всё

#!/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 

Подробнее здесь: [url]https://stackoverflow.com/questions/79367558/cant-publish-to-cmd-vel-in-ros-gazebo[/url]