HI guys , im coding a multi-robot , in ros kinetic with python and gazebo,
MY mission is make one robot find a yellow cube in scenario. then get that location and send to second robot
im trying to code my group of robot, im using ros,gazebo and qt crator, and im not expert, im a little bit stupid.
idk whats happening , i tryied few things, but im really don't know what to do.
it starts working, but when he faces the first wall, it crashes with follow message "QOBJECTS TIMMERS CANNOT BE STOPPED FROM ANOTHER THREAD"
here is my code
#!/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()
↧