return to_return
# def check_in_frame(pos, frame):
#
# if pos == -1:
# return True
#
# count = np.sum(frame == 255)
# print("Count: " + count)
# main
ballColor = raw_input("What color balloon do you want to go to? ")
r = robot()
r.drive(angSpeed=.1)
# list and error for pid
error_list = []
old_error = 0
rate = rospy.Rate(20)
while not rospy.is_shutdown():
# get image and convert to the mask
img = r.getImage()
img = cv2.fastNlMeansDenoisingColored(img, None, 10, 10, 7, 21)
hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
outhsv = 0
if ballColor == "red":
def __init__(self):
# make the robot and lists for tracking error
self.r = robot()
self.error_list_pos = []
self.error_list_angle = []
import rospy
from turtleAPI import robot
import cv2
COLORS = {"red":,"blue":,"green":,"pink":}
try:
turtle = robot()
while not rospy.is_shutdown():
target_color = input()
except:
rospy.loginfo("Terminating")
import rospy
from turtleAPI import robot
try:
<<<<<<< HEAD
print("creating robot")
r= robot()
r.drive(angSpeed=.4,linSpeed=.25)
while not rospy.is_shutdown():
print(r.getPositionTup())
pass
=======
print("creating robot")
r= robot()
r.drive(linSpeed=.25)
# get initial position
init_pos = r.getPositionTup()[0]
goal_pos = init_pos + 1
# checking to see how far it is
while r.getPositionTup()[0] < goal_pos:
pass
# stop
r.drive(linSpeed=0)
>>>>>>> a801abd1c91ac845e2615dc3b853361fdd5c6927
except Exception as e:
print(e)
rospy.loginto("node now terminated")
import numpy as np
import rospy
import time
import math
from turtleAPI import robot
rob = robot()
r = rospy.Rate(5)
rob.drive(linSpeed=.3)
def checkIfClose(midd):
print(midd.shape)
if len(midd.shape) == 1:
return True
#only
for col in range(0, midd.shape[1]):
count = 0
sum = 0
for row in range(0, midd.shape[0]/2): #only want lower half
if math.isnan(midd[row][col]):
continue
sum += midd[row][col]
count += 1
#avg for this col
avg = sum/count
if avg < 1.5:
return False
# calculate angle between two points
def newyaw(goal, curr):
return np.arctan2(goal[1] - curr[1], goal[0] - curr[0])
# fixes yaw error value
def yawdif(goal, curr):
yaw = goal[2] - curr[2]
if yaw > np.pi / 2 or yaw < -np.pi / 2:
yaw = 1
return yaw
if __name__ == "__main__":
# create robot
rbt = robot()
parser = argparse.ArgumentParser()
parser.add_argument(
"graph_filename",
help="Filename for graph .dot file. must be in same directory as code.",
type=str)
parser.add_argument("end",
help="Coordinates of the desired end position: [3, 4]",
type=list)
parser.add_argument(
"-s",
"--start",
help="Coordinates of the desired start position: [3, 4]",
type=list)
dist_goal = calcDistance(nodes, "goal", name)
if dist_goal < min_dist_goal:
min_dist_goal = dist_goal
min_name_goal = name
matrix[node_nums["start"]][node_nums[min_name_start]] = min_dist_start
matrix[node_nums[min_name_start]][node_nums["start"]] = min_dist_start
matrix[node_nums["goal"]][node_nums[min_name_goal]] = min_dist_goal
matrix[node_nums[min_name_goal]][node_nums["goal"]] = min_dist_goal
return matrix, nodes_list
dot_file = sys.argv[1]
r = turt.robot()
goal_coord = sys.argv[2].split(",")
goal_x = float(goal_coord[0])
goal_y = float(goal_coord[1])
if len(sys.argv) == 4:
start_coord = sys.argv[3].split(",")
start_x = float(start_coord[0])
start_y = float(start_coord[1])
else:
start_coord = r.getMCLPose()
start_x = start_coord[0]
start_y = start_coord[1]
matrix, nodes_list = parse(dot_file, start_x, start_y, goal_x, goal_y)
