TOLERANCE = 15
Error= 2
Error2=5
compassTolerance=5
# Init Node
rospy.init_node('moveForward', anonymous = False)
# Init publishers
# pan_angle_pub = rospy.Publisher('/act/robot/set_pan_angle', Int32, queue_size = 1)
# tilt_angle_pub = rospy.Publisher('/act/robot/set_tilt_angle', Int32, queue_size = 1)
pan_angle_pub = rospy.Publisher('/act/robot/set_pan_angle', Int32, queue_size = 1)
tilt_angle_pub = rospy.Publisher('/act/robot/set_tilt_angle', Int32, queue_size = 1)
robot_cmd_pub = rospy.Publisher('/act/robot/send_move_command', robot_cmd, queue_size = 10)
# Wait for bluetooth to connect
waitForBT()
# Init variables
angles = {"pan" :[45], "tilt" :[45]}
# sonar_data = {"distance" : 200}
compass_data={"heading":[]}
# compassHeading=[0]
#defines the current state of the robot
flags = {"turn left" : False, "turn right" : False, "walk forward" : False,"Camera track":False,"Body track":False,"follwing":False}
# Move head to default position
pan_head(45)
tilt_head(45)
moveForward()
except rospy.ROSInterruptException:
"ctSucceeded"] = True # Flag to determine whether camera track was a success of failure. True by default (Success)
# Initialize terms for PD controllers
x_prev = board["start_pan"] # previous x-pos
y_prev = board["start_tilt"] # previous y-Pos
kx = 4.5 # constant term for x direction
ky = 2.5 # constant term for y direction
kdx = 0.0008 # differential pan gain
kdy = -0.0002 # differential tilt gain
dt = 0.1 # rate of time between call back ... I guessed this one
tPrev = time.time()
# Create a behavior tree using the blackboard object board
be = behavior(board)
# call to the main tree (root)
be_tree = be.main_tree
# Wait for bluetooth to connect
waitForBT()
# Move head to default position
be.pan_head(board["start_pan"])
be.tilt_head(board["start_tilt"])
rate = rospy.Rate(100) #100hz
# rospy.spin()
while not rospy.is_shutdown():
##rospy.loginfo("Here in the loop")
be_tree.next()
rate.sleep()
##rospy.loginfo("bye")