def goToBall(self):
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
msg = point_SF()
msg.bot_id = int(self.textBotId.text())
pub.publish(msg)
if (not self.t1 == None):
self.t1.end()
self.t1 = multiprocessing.Process(target=self.goToPoint)
self.t1.start()
def sendParams(self):
stepSize = float(self.stepSizeText.text())
biasParam = float(self.biasParamText.text())
maxIterations = float(self.maxIterationsText.text())
msg = point_SF()
msg.s_x = points_home[1][0]
msg.s_y = points_home[1][1]
msg.f_x = 0
msg.f_y = 0
print(" here step_size ", stepSize)
msg.step_size = stepSize
msg.bias_param = biasParam
msg.max_iteration = maxIterations
# pub.publish(msg)
pass
def sendParams(self):
stepSize = float(self.stepSizeText.text())
biasParam = float(self.biasParamText.text())
maxIterations = float(self.maxIterationsText.text())
self.start = literal_eval(str(self.startPointText.text()))
self.goal = literal_eval(str(self.endPointText.text()))
print("in send params start = ",self.start," goal = ",self.goal)
# self.start[1]-=20
# self.goal[1]-=20
msg=point_SF()
msg.s_x=self.start[0]
msg.s_y=self.start[1]
msg.f_x=self.goal[0]
msg.f_y=self.goal[1]
print(" here step_size ",stepSize)
msg.step_size=stepSize
msg.bias_param=biasParam
msg.max_iteration=maxIterations
pub.publish(msg)
pass
def goToBall(self):
global BState,kubs_pub
import signal
signal.signal(signal.SIGINT, signal.SIG_DFL)
msg=point_SF()
kubs_id = int(self.textBotId.text())
# msg.bot_id = kubs_id
# pub.publish(msg)
print "in goToBall",BState
if BState:
print "belief_state Subscriber"
kub = kubs.kubs(kubs_id,BState,kubs_pub)
g_fsm = GoToBall.GoToBall()
g_fsm.add_kub(kub)
g_fsm.add_theta(theta=normalize_angle(pi+atan2(BState.ballPos.y,BState.ballPos.y-3000)))
g_fsm.spin()
if not (self.t1 == None):
self.t1.terminate()
self.t1 = multiprocessing.Process(target=g_fsm.spin)
self.t1.start()