From e8700f287b08c29fbdd911e04d6850acd09b4741 Mon Sep 17 00:00:00 2001 From: Kaden King Date: Thu, 29 Mar 2018 18:24:28 -0400 Subject: [PATCH 1/3] VISION: updated cascade path to be set for the nuc --- bog/nodes/seeker.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/bog/nodes/seeker.py b/bog/nodes/seeker.py index 419849e..6bfa032 100755 --- a/bog/nodes/seeker.py +++ b/bog/nodes/seeker.py @@ -6,7 +6,7 @@ from std_msgs.msg import String from std_msgs.msg import Float32 -CASCADE_PATH = "/home/kaden/Documents/button/src/buttonseek/src/cascade_19.xml" #absolute path to the cascade +CASCADE_PATH = "/home/uf-ieee/Documents/ws/src/IEEE2018/bog/nodes/cascade_19.xml" #absolute path to the cascade DEBUG_DISPLAY = 0 # if true, shows the camera and its interpretations on the picture def vision(): @@ -17,7 +17,7 @@ def vision(): button_cascade = cv2.CascadeClassifier(CASCADE_PATH) # uses the haar cascade in the path to find the button - cap = cv2.VideoCapture(1) # cap is the image that the camera captured + cap = cv2.VideoCapture(0) # cap is the image that the camera captured while not rospy.is_shutdown(): From d150630485ae6e42b5be9755410b79f5373e8f54 Mon Sep 17 00:00:00 2001 From: Kaden King Date: Thu, 5 Apr 2018 19:35:21 -0400 Subject: [PATCH 2/3] VISION: vision_control now alters motor speeds --- bog/nodes/vision_control.py | 73 ++++++++++++++++++++++++++++++------- 1 file changed, 59 insertions(+), 14 deletions(-) diff --git a/bog/nodes/vision_control.py b/bog/nodes/vision_control.py index 973d194..d065c72 100755 --- a/bog/nodes/vision_control.py +++ b/bog/nodes/vision_control.py @@ -1,48 +1,93 @@ #!/usr/bin/env python import rospy +import time from std_msgs.msg import String from std_msgs.msg import Float32 from bog.msg import SetWheelSpeeds #constants DISTANCE_RATIO = 0.25 #this constant will be used to determine how much to move the robot left or right. The farther away we are, the less we want to move left or right. -GOAL_DISTANCE_LOW = 9 # the lower bound of the goal distance from the button +GOAL_DISTANCE_LOW = 5 # the lower bound of the goal distance from the button GOAL_DISTANCE_HIGH = 11 # the upper bound of the goal distance from the button - +WHEEL_SPEED = 75 #state variables current_direction = "" current_distance = -1 - +updated2 = False #move method will move the robot left, right, or forward depending on the distance and direction passed to it. #we will move toward the button until certain parameters are met + +wheels = SetWheelSpeeds() + +class status: + updated = False + + +def forward(): + pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10) + wheels.wheel1 = WHEEL_SPEED + wheels.wheel2 = WHEEL_SPEED + wheels.wheel3 = WHEEL_SPEED + wheels.wheel4 = WHEEL_SPEED + pub.publish(wheels) + time.sleep(2) + wheels.wheel1 = 0 + wheels.wheel2 = 0 + wheels.wheel3 = 0 + wheels.wheel4 = 0 + pub.publish(wheels) + + + + def move(direction, distance): - if current_distance < GOAL_DISTANCE_LOW: - rospy.loginfo("test") + + rospy.loginfo("Direction: %s", current_direction) # write the information to the console + rospy.loginfo("Distance: %s", current_distance) # write the information to the console + + if current_distance > GOAL_DISTANCE_LOW: #move forward + forward() + + + + def distance_callback(data): + global current_distance current_distance = data.data # sets current distance to the updated value of the distance topic - rospy.loginfo("Distance: %s", current_distance) # write the information to the console - updated = True + #rospy.loginfo("Distance: %s", current_distance) # write the information to the console + status.updated = True def direction_callback(data): + global current_direction current_direction = data.data # sets the current direction variable to the updated value of the topic - rospy.loginfo("Direction: %s", current_direction) # write the information to the console - updated = True + #rospy.loginfo("Direction: %s", current_direction) # write the information to the console + status.updated = True def listen(): - updated = False rospy.init_node('vision_subscriber', anonymous=True) rospy.Subscriber('direction', String, direction_callback) rospy.Subscriber('distance', Float32, distance_callback) + + rate = rospy.Rate(10) + + while not rospy.is_shutdown(): + if status.updated: + rospy.loginfo("MOVING FORWARD") + move(current_direction,current_distance) + status.updated = False # changed updated to false so that we do not move again based on old information + rate.sleep() + rate.sleep() + + + + + - if updated: #only call the move function if we are working with new information - move(direction,distance) - updated = False # changed updated to false so that we do not move again based on old information - rospy.spin() if __name__ == '__main__': listen() \ No newline at end of file From c112286c2b568c2b6e0fd038c6ae1be44ab00a6f Mon Sep 17 00:00:00 2001 From: Kaden King Date: Sat, 7 Apr 2018 15:48:58 -0400 Subject: [PATCH 3/3] VISION: motors moving in their proper directions based on where the button is working --- bog/nodes/seeker.py | 4 +-- bog/nodes/vision_control.py | 60 +++++++++++++++++++++++++++++++++++-- 2 files changed, 59 insertions(+), 5 deletions(-) diff --git a/bog/nodes/seeker.py b/bog/nodes/seeker.py index 6bfa032..375d026 100755 --- a/bog/nodes/seeker.py +++ b/bog/nodes/seeker.py @@ -7,7 +7,7 @@ from std_msgs.msg import Float32 CASCADE_PATH = "/home/uf-ieee/Documents/ws/src/IEEE2018/bog/nodes/cascade_19.xml" #absolute path to the cascade -DEBUG_DISPLAY = 0 # if true, shows the camera and its interpretations on the picture +DEBUG_DISPLAY = 1 # if true, shows the camera and its interpretations on the picture def vision(): direct = rospy.Publisher('direction', String, queue_size=10) #handles the publishing of the direction of the button @@ -33,7 +33,7 @@ def vision(): # add this # image, reject levels level weights. #buttons = button_cascade.detectMultiScale(gray, scaleFactor=2 ,minNeighbors=30) - buttons = button_cascade.detectMultiScale(gray, scaleFactor=1.2 ,minNeighbors=30) # adjusts how sensitive the cascade is to saying that there is a button in the frame + buttons = button_cascade.detectMultiScale(gray, scaleFactor=1.1 ,minNeighbors=30) # adjusts how sensitive the cascade is to saying that there is a button in the frame # add this for (x,y,w,h) in buttons: diff --git a/bog/nodes/vision_control.py b/bog/nodes/vision_control.py index d065c72..ff8c1db 100755 --- a/bog/nodes/vision_control.py +++ b/bog/nodes/vision_control.py @@ -9,7 +9,7 @@ DISTANCE_RATIO = 0.25 #this constant will be used to determine how much to move the robot left or right. The farther away we are, the less we want to move left or right. GOAL_DISTANCE_LOW = 5 # the lower bound of the goal distance from the button GOAL_DISTANCE_HIGH = 11 # the upper bound of the goal distance from the button -WHEEL_SPEED = 75 +WHEEL_SPEED = 50 #state variables current_direction = "" @@ -32,12 +32,59 @@ def forward(): wheels.wheel3 = WHEEL_SPEED wheels.wheel4 = WHEEL_SPEED pub.publish(wheels) - time.sleep(2) + time.sleep(0.5) wheels.wheel1 = 0 wheels.wheel2 = 0 wheels.wheel3 = 0 wheels.wheel4 = 0 pub.publish(wheels) + time.sleep(0.5) + +def backward(): + pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10) + wheels.wheel1 = -1*WHEEL_SPEED + wheels.wheel2 = -1*WHEEL_SPEED + wheels.wheel3 = -1*WHEEL_SPEED + wheels.wheel4 = -1*WHEEL_SPEED + pub.publish(wheels) + time.sleep(0.5) + wheels.wheel1 = 0 + wheels.wheel2 = 0 + wheels.wheel3 = 0 + wheels.wheel4 = 0 + pub.publish(wheels) + time.sleep(0.5) + + +def left(): + pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10) + wheels.wheel1 = 1*WHEEL_SPEED + wheels.wheel2 = -1*WHEEL_SPEED + wheels.wheel3 = 1*WHEEL_SPEED + wheels.wheel4 = -1*WHEEL_SPEED + pub.publish(wheels) + time.sleep(0.5) + wheels.wheel1 = 0 + wheels.wheel2 = 0 + wheels.wheel3 = 0 + wheels.wheel4 = 0 + pub.publish(wheels) + time.sleep(0.5) + +def right(): + pub = rospy.Publisher('Set_Motors', SetWheelSpeeds, queue_size=10) + wheels.wheel1 = -1*WHEEL_SPEED + wheels.wheel2 = 1*WHEEL_SPEED + wheels.wheel3 = -1*WHEEL_SPEED + wheels.wheel4 = 1*WHEEL_SPEED + pub.publish(wheels) + time.sleep(0.5) + wheels.wheel1 = 0 + wheels.wheel2 = 0 + wheels.wheel3 = 0 + wheels.wheel4 = 0 + pub.publish(wheels) + time.sleep(0.5) @@ -47,8 +94,15 @@ def move(direction, distance): rospy.loginfo("Direction: %s", current_direction) # write the information to the console rospy.loginfo("Distance: %s", current_distance) # write the information to the console - if current_distance > GOAL_DISTANCE_LOW: #move forward + if direction == "left": + left() + elif direction == "right": + right() + elif current_distance > GOAL_DISTANCE_LOW: #move forward forward() + elif current_distance > GOAL_DISTANCE_HIGH: + backward() +