我目前正在做 RRT* 路径规划作业，我无法将turtlesim3 Gazebo 的速度发布到 /cmd_vel。我可以看到节点已在 rqt_graph 中连接，但当我回显该主题时，没有弹出任何内容。我可以使用 teleop 和终端控制机器人。我可以看到节点已在 rqt_graph 中连接，但当我回显该主题时，没有任何内容弹出。我可以使用 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()
我认为问题是由代码的放置或顺序引起的，但我无法找到解决方案。 谢谢
这种使用变量的方法不起作用，而且实际上会导致错误。如果您这样做，您的
pub#!/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
pub = rospy.Publisher('/cmd_vel', Twist, queue_size=10)
class RRT():
....
另一个问题是这会导致运行时错误。因此，如果上述节点没有死亡，则意味着您还有另一个问题。您的
/goal_pose