def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber("/robot1/camera1/image_raw", Image,
self.camera_callback)
self.moverosbots_object = MoveRosBots()
self.pub = rospy.Publisher('lanepoint', Point, queue_size=1)
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan,
self.callback)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
self.counter = 0
class Controller(object):
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan,
self.callback)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
self.counter = 0
def trajectory_fuction(self, x, y):
return x_d, y_d
def callback(self, msg):
left = msg.ranges[541:]
front_left = msg.ranges[360:540]
front_right = msg.ranges[180:359]
right = msg.ranges[:179]
var = Twist()
# Left Turn
if left[90] > 1:
print "Turning Left..."
var.linear.x = .2
var.angular.z = .5
# Right Turn
elif (front_left[90] < 1):
print "Turning right..."
var.linear.x = .2
var.angular.z = -.5
# Straight Path
else:
print "Going Straight"
var.linear.x = .25
var.angular.z = 0
wall_distance = left[90]
#r_wall_distance = right[0]
print wall_distance
if wall_distance < 5:
var.angular.z -= .5 * (.5 - wall_distance)
#var.angular.z += .5 * (.65 - r_wall_distance)
self.pub.publish(var)
def clean_up(self):
self.moverosbots_object.clean_class()
cv2.destroyAllWindows()
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan, self.callback)
self.sub2 = rospy.Subscriber("/odom", Odometry, self.callback2)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
self.counter = 0
self.x = 0
self.y = 0
self.angle = 0
self.flush = 0
self.junctions = []
class Controller(object):
def __init__(self):
rospy.Subscriber("/lanepoint", Point, self.callback)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
def callback(self, lanepoint):
cx, cy = lanepoint.x, lanepoint.y
# Proportional controller
kp = 0.01
kd = 0.1
ki = 0.00000
ref = 0.
error = ref - cx # Error
# Control inputs
#w = kp * error # P Control for angular velocity
d_error = error - self.old_error
w = (kp * error) + (kd * d_error) + (ki * self.E)
v = 0.4 # Linear velocity
#w += -0.2
print error
self.E += error
self.old_error = error
#w += kd * d_error
#w += ki * self.E
# Message
twist_object = Twist()
twist_object.linear.x = v
twist_object.angular.z = w
# Send message
self.moverosbots_object.move_robot(twist_object)
def clean_up(self):
self.moverosbots_object.clean_class()
cv2.destroyAllWindows()
def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber("/robot1/camera1/image_raw", Image,
self.camera_callback)
self.moverosbots_object = MoveRosBots()
class LineFollower(object):
def __init__(self):
self.bridge_object = CvBridge()
self.image_sub = rospy.Subscriber("/robot1/camera1/image_raw", Image,
self.camera_callback)
self.moverosbots_object = MoveRosBots()
def camera_callback(self, data):
try:
# We select bgr8 because its the OpneCV encoding by default
cv_image = self.bridge_object.imgmsg_to_cv2(
data, desired_encoding="bgr8")
except CvBridgeError as e:
print(e)
# We get image dimensions and crop the parts of the image we don't need
# Bear in mind that because the first value of the image matrix is start and second value is down limit.
# Select the limits so that it gets the line not too close and not too far, and the minimum portion possible
# To make process faster.
height, width, channels = cv_image.shape
rows_to_watch = 100
top_trunc = 1 * height / 2 #get 3/4 of the height from the top section of the image
bot_trunc = top_trunc + rows_to_watch #next set of rows to be used
crop_img = cv_image[top_trunc:bot_trunc, 0:width]
# Convert from RGB to HSV
hsv = cv2.cvtColor(crop_img, cv2.COLOR_BGR2HSV).astype(np.float)
# Define the Yellow Colour in HSV
#RGB
#[[[222,255,0]]]
#BGR
#[[[0,255,222]]]
"""
To know which color to track in HSV, put in BGR. Use ColorZilla to get the color registered by the camera
>>> yellow = np.uint8([[[B,G,R ]]])
>>> hsv_yellow = cv2.cvtColor(yellow,cv2.COLOR_BGR2HSV)
>>> print( hsv_yellow )
[[[ 34 255 255]]
"""
lower_yellow = np.array([20, 100, 100])
upper_yellow = np.array([50, 255, 255])
# Threshold the HSV image to get only yellow colors
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
# Calculate centroid of the blob of binary image using ImageMoments
m = cv2.moments(mask, False)
try:
cx, cy = m['m10'] / m['m00'], m['m01'] / m['m00']
except ZeroDivisionError:
cy, cx = height / 2, width / 2
# Bitwise-AND mask and original image
res = cv2.bitwise_and(crop_img, crop_img, mask=mask)
# Draw the centroid in the resultut image
# cv2.circle(img, center, radius, color[, thickness[, lineType[, shift]]])
cv2.circle(res, (int(cx), int(cy)), 10, (0, 0, 255), -1)
cv2.imshow("Original", cv_image)
cv2.imshow("HSV", hsv)
cv2.imshow("MASK", mask)
cv2.imshow("RES", res)
cv2.waitKey(1)
error_x = cx - width / 2
angular_z = -error_x / 100
rospy.loginfo("ANGULAR VALUE SENT===>" + str(angular_z))
twist_object = Twist()
twist_object.linear.x = 0.2
twist_object.angular.z = -error_x / 100
# Make it start turning
self.moverosbots_object.move_robot(twist_object)
def clean_up(self):
self.moverosbots_object.clean_class()
cv2.destroyAllWindows()
def __init__(self):
rospy.Subscriber("/lanepoint", Point, self.callback)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
class Controller(object):
def __init__(self):
self.pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
self.sub = rospy.Subscriber('/kobuki/laser/scan', LaserScan, self.callback)
self.sub2 = rospy.Subscriber("/odom", Odometry, self.callback2)
self.moverosbots_object = MoveRosBots()
self.E = 0.
self.old_error = 0.
self.counter = 0
self.x = 0
self.y = 0
self.angle = 0
self.flush = 0
self.junctions = []
def callback(self, msg):
left = msg.ranges[541:]
front_left = msg.ranges[360:540]
front_right = msg.ranges[180:359]
right = msg.ranges[:179]
var = Twist()
print "Current Coords: (%.2f, %.2f)"%(self.x, self.y)
print "Distances LFR: (%.2f, %.2f, %.2f)"%(left[90], front_left[0], right[90])
print "Flush = " + str(self.flush)
distance = 3
if self.flush > 0:
self.flush -= 1
elif left[90] > distance and right[90] > distance and front_left[0] > distance:
print "Adding Junction LFR"
junc = self.find_coords()
if junc == None:
junc = Junction(self.x, self.y, self.angle, 'lfr')
self.junctions.append(junc)
# turn a direction we haven't been yet
if junc.been_to.find('l') == -1:
# go forward
print "Going left"
junc.been_to += 'l'
var.linear.x = .3
var.angular.z = -.3
self.pub.publish(var)
time.sleep(6)
elif junc.been_to.find('r') == -1:
# turn right
print "Turning Right"
junc.been_to += 'r'
var.linear.x = .3
var.angular.z = -.3
self.pub.publish(var)
time.sleep(6)
elif junc.been_to.find('f') == -1:
# go forward
print "Going straight"
junc.been_to += 'f'
var.linear.x = .5
var.angular.z = 0
self.pub.publish(var)
time.sleep(2)
self.flush = 100
elif left[90] > distance and right[90] > distance:
print "Adding Junction LR"
junc = self.find_coords()
if junc == None:
junc = Junction(self.x, self.y, self.angle, 'fr')
self.junctions.append(junc)
# turn a direction we haven't been yet
if junc.been_to.find('r') == -1:
# turn right
print "Turning Right"
junc.been_to += 'r'
var.linear.x = .3
var.angular.z = -.3
self.pub.publish(var)
time.sleep(6)
elif junc.been_to.find('l') == -1:
# go forward
print "Going left"
junc.been_to += 'l'
var.linear.x = .3
var.angular.z = .3
self.pub.publish(var)
time.sleep(3)
self.flush = 100
elif front_left[0] > distance and right[90] > distance:
print "Adding Junction FR"
junc = self.find_coords()
if junc == None:
junc = Junction(self.x, self.y, self.angle, 'fr')
self.junctions.append(junc)
# turn a direction we haven't been yet
if junc.been_to.find('r') == -1:
# turn right
print "Turning Right"
junc.been_to += 'r'
var.linear.x = .3
var.angular.z = -.3
self.pub.publish(var)
time.sleep(6)
elif junc.been_to.find('f') == -1:
# go forward
print "Going straight"
junc.been_to += 'f'
var.linear.x = .5
var.angular.z = 0
self.pub.publish(var)
time.sleep(2)
self.flush = 100
elif left[90] > distance and front_left[0] > distance:
print "Adding Junction LF"
junc = self.find_coords()
if junc == None:
junc = Junction(self.x, self.y, self.angle, 'fr')
self.junctions.append(junc)
# turn a direction we haven't been yet
if junc.been_to.find('l') == -1:
# turn right
print "Turning Left"
junc.been_to += 'l'
var.linear.x = .3
var.angular.z = .3
self.pub.publish(var)
time.sleep(6)
elif junc.been_to.find('f') == -1:
# go forward
print "Going straight"
junc.been_to += 'f'
var.linear.x = .5
var.angular.z = 0
self.pub.publish(var)
time.sleep(2)
self.flush = 100
else:
# Left Turn
if left[90] > 1:
#print "Turning Left..."
var.linear.x = .2
var.angular.z = .5
# Right Turn
elif (front_left[90] < 1):
#print "Turning right..."
var.linear.x = .2
var.angular.z = -.5
# Straight Path
else:
#print "Going Straight"
var.linear.x = .25
var.angular.z = 0
wall_distance = left[90]
#r_wall_distance = right[0]
#print wall_distance
if wall_distance < 5:
var.angular.z -= .5 * (.65 - wall_distance)
#var.angular.z += .5 * (.65 - r_wall_distance)
self.pub.publish(var)
def callback2(self, odometry):
self.x, self.y = odometry.pose.pose.position.x, odometry.pose.pose.position.y
orientation_q = odometry.pose.pose.orientation
(rot_x, rot_y, phi) = euler_from_quaternion([orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w])
self.angle = phi
def find_coords(self):
for i in self.junctions:
if (abs(self.x - i.x) < 1 and abs(self.y - i.y) < 1 and abs(self.angle - i.angle) < 30):
return i
return None
def clean_up(self):
self.moverosbots_object.clean_class()
cv2.destroyAllWindows()