idk发生了什么，我尝试了几件事，但是我真的不知道该怎么办。
它开始工作，但是当他面对第一堵墙时，它崩溃并显示以下消息：“无法从另一个线程停止qobjects调光器”
这是我的代码
#!/usr/bin/env python
import rospy, time , cv2, cv_bridge, numpy
from nav_msgs.msg import Odometry
from tf.transformations import euler_from_quaternion
from geometry_msgs.msg import Point, Twist
from sensor_msgs.msg import LaserScan, Image, CameraInfo
from math import atan2
class Follower:
def __init__(self):
self.bridge = cv_bridge.CvBridge()
#cv2.namedWindow("window", 1)
self.image_sub = rospy.Subscriber('/tb3_0/camera/rgb/image_raw',Image, self.image_callback)
#self.cmd_vel_pub = rospy.Publisher('/cmd_vel',Twist, queue_size=1)
#self.twist = Twist()
def image_callback(self, msg):
image = self.bridge.imgmsg_to_cv2(msg,desired_encoding='bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_yellow = numpy.array([ 10, 10, 10])
upper_yellow = numpy.array([255, 255, 250])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
h, w, d = image.shape
search_top = 3*h/4
search_bot = 3*h/4 + 20
mask[0:search_top, 0:w] = 0
mask[search_bot:h, 0:w] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
#cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
# BEGIN CONTROL
print "achei amarelo"
#color = 1
goal.x = x
goal.y = y
# END CONTROL
# cv2.imshow("mask",mask)
cv2.imshow("output", image)
cv2.waitKey(3)
#Odometria para identificar posicao
x = 0.0
y = 0.0
theta = 0.0
def newOdom(msg):
global x
global y
global theta
x = msg.pose.pose.position.x
y = msg.pose.pose.position.y
rot_q = msg.pose.pose.orientation
(roll, pitch, theta) = euler_from_quaternion([rot_q.x, rot_q.y, rot_q.z, rot_q.w])
rospy.init_node("speed_controller")
odom_sub_tb3_0 = rospy.Subscriber("/tb3_0/odom", Odometry, newOdom)
#pub = rospy.Publisher("/tb3_0/cmd_vel", Twist, queue_size = 1)
r = rospy.Rate(4)
goal = Point()
goal.x = 0
goal.y = 0
def scan_callback(msg):
global range_front
global range_right
global range_left
global ranges
global min_front,i_front, min_right,i_right, min_left ,i_left
ranges = msg.ranges
# get the range of a few points
# in front of the robot (between 5 to -5 degrees)
range_front[:5] = msg.ranges[5:0:-1]
range_front[5:] = msg.ranges[-1:-5:-1]
# to the right (between 300 to 345 degrees)
range_right = msg.ranges[300:345]
# to the left (between 15 to 60 degrees)
range_left = msg.ranges[60:15:-1]
# get the minimum values of each range
# minimum value means the shortest obstacle from the robot
min_range,i_range = min( (ranges[i_range],i_range) for i_range in xrange(len(ranges)) )
min_front,i_front = min( (range_front[i_front],i_front) for i_front in xrange(len(range_front)) )
min_right,i_right = min( (range_right[i_right],i_right) for i_right in xrange(len(range_right)) )
min_left ,i_left = min( (range_left [i_left ],i_left ) for i_left in xrange(len(range_left )) )
# Initialize all variables
range_front = []
range_right = []
range_left = []
min_front = 0
i_front = 0
min_right = 0
i_right = 0
min_left = 0
i_left = 0
# Node para tb3_0 -- leader
cmd_vel_pub_tb3_0 = rospy.Publisher('/tb3_0/cmd_vel', Twist, queue_size = 1) # to move the robot
scan_sub_tb3_0 = rospy.Subscriber('/tb3_0/scan', LaserScan, scan_callback) # to read the laser scanner
command = Twist()
command.linear.x = 0.0
command.angular.z = 0.0
rate = rospy.Rate(10)
time.sleep(1) # wait for node to initialize
near_wall = 0 # start with 0, when we get to a wall, change to 1
follower = Follower()
time.sleep(2)
#rospy.spin()
# Turn the robot right at the start
# to avoid the 'looping wall'
print("Turning...")
command.angular.z = -0.5
command.linear.x = 0.1
cmd_vel_pub_tb3_0.publish(command)
time.sleep(2)
inc_x = goal.x -x
inc_y = goal.y -y
angle_to_goal = atan2(inc_y, inc_x)
# The algorithm:
# 1. Robot moves forward to be close to a wall
# 2. Start following left wall.
# 3. If too close to the left wall, reverse a bit to get away
# 4. Otherwise, follow wall by zig-zagging along the wall
# 5. If front is close to a wall, turn until clear
while(near_wall == 0 and not rospy.is_shutdown()): #1
print("Moving towards a wall.")
if(min_front > 0.2 and min_right > 0.2 and min_left > 0.2):
command.angular.z = -0.1 # if nothing near, go forward
command.linear.x = 0.15
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
print "C"
elif(min_left < 0.2): # if wall on left, start tracking
near_wall = 1
print "A"
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
else:
command.angular.z = -0.25 # if not on left, turn right
command.linear.x = 0.0
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
cmd_vel_pub_tb3_0.publish(command)
else: # left wall detected
if(min_front > 0.2): #2
if(min_left < 0.12): #3
print("Range: {:.2f}m - Too close. Backing up.".format(min_left))
command.angular.z = -1.2
command.linear.x = -0.1
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
elif(min_left > 0.15): #4
print("Range: {:.2f}m - Wall-following; turn left.".format(min_left))
command.angular.z = 1.2
command.linear.x = 0.15
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
else:
print("Range: {:.2f}m - Wall-following; turn right.".format(min_left))
command.angular.z = -1.2
command.linear.x = 0.15
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
else: #5
print("Front obstacle detected. Turning away.")
command.angular.z = -1.0
command.linear.x = 0.0
cmd_vel_pub_tb3_0.publish(command)
print("Posicao x",x)
print("Posicao y",y)
print("Posicao theta",theta)
while(min_front < 0.3 and not rospy.is_shutdown()):
pass
# publish command
cmd_vel_pub_tb3_0.publish(command)
# wait for the loop
rate.sleep()