def execute_fine_approach(self):
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
generatingfunction = _GoToPoint.execute(start_time,self.ball_dist_thresh)
for gf in generatingfunction:
self.kub,ballPos = gf
if not vicinity_points(ballPos,self.kub.state.ballPos,thresh=BOT_RADIUS):
self.behavior_failed = True
break
def execute_course_approach(self):
start_time = rospy.Time.now()
start_time = 1.0*start_time.secs + 1.0*start_time.nsecs/pow(10,9)
generatingfunction = _GoToPoint.execute(start_time,self.initial_target_dist_thresh,True)
for gf in generatingfunction:
self.kub,target_point = gf
self.target_point = getPointBehindTheBall(self.kub.state.ballPos,self.theta)
if not vicinity_points(self.target_point,target_point,thresh=BOT_RADIUS*3.5):
self.behavior_failed = True
break
def execute_drive(self):
start_time = rospy.Time.now()
start_time = 1.0 * start_time.secs + 1.0 * start_time.nsecs / pow(
10, 9)
generatingfunction = _GoToPoint.execute(start_time, DISTANCE_THRESH)
for gf in generatingfunction:
self.kub, target_point = gf
# print self.behavior_failed
if not vicinity_points(self.target_point, target_point):
# print
# print (self.target_point.x,self.target_point.y)
# print (target_point.x,target_point.y)
# print
self.behavior_failed = True
# print self.behavior_failed
break
self.new_point = self.kub.get_pos()