def __init__(self, sim=False):
print("Initialized strategy core.py")
rospy.init_node('core', anonymous=False)
self.rate = rospy.Rate(200)
self.sm = MyStateMachine(sim)
# self.sm.mir = MIR(HOST)
self.dclient = dynamic_reconfigure.client.Client(
"core", timeout=30, config_callback=None)
print("Initialized OK")
self.main()
def __init__(self, sim=False):
rospy.init_node('core', anonymous=True)
self.rate = rospy.Rate(200)
self.robot = MyStateMachine(sim)
self.main()
class Strategy(object):
def __init__(self, sim=False):
print("Initialized strategy core.py")
rospy.init_node('core', anonymous=False)
self.rate = rospy.Rate(200)
self.sm = MyStateMachine(sim)
# self.sm.mir = MIR(HOST)
self.dclient = dynamic_reconfigure.client.Client(
"core", timeout=30, config_callback=None)
print("Initialized OK")
self.main()
def main(self):
while not rospy.is_shutdown():
# s = self.sm.get_mir_status()
# print(s['mir_state'])
# print(self.sm.current_state)
if self.sm.go_home and not self.sm.is_home:
self.sm.toHome()
if self.sm.is_home and self.sm.mir.arrived_position("HOME"):
self.sm.toIdle()
if not self.sm.is_idle and not self.sm.start \
and not self.sm.is_home:
self.sm.toIdle()
if self.sm.is_idle:
if self.sm.start:
# self.sm.toMove("TKU_ToHOME")
self.sm.toMove("TKU_ToSHELF")
# self.sm.mir.relative_move(dx=0.03)
# time.sleep(1)
if self.sm.is_move:
if self.sm.mir.mission_queue_is_empty:
print("Arrived")
# self.sm.toArm()
if self.sm.is_arm:
if self.sm.arm_result is not None:
if self.sm.arm_result.finish:
print("Arm task finished, Back to home!!!")
self.sm.toHome()
else:
print("Arm task doesn't finished...")
self.sm.toHome()
if rospy.is_shutdown():
break
self.rate.sleep()
class Strategy(object):
def __init__(self, sim=False):
rospy.init_node('core', anonymous=True)
self.rate = rospy.Rate(200)
self.robot = MyStateMachine(sim)
self.main()
def main(self):
while not rospy.is_shutdown():
print(self.robot.current_state)
targets = self.robot.GetObjectInfo()
position = self.robot.GetRobotInfo()
# Can not find ball when starting
if targets is None or targets['ball'][
'ang'] == 999 and self.robot.game_start:
print("Can not find ball")
self.robot.toIdle()
else:
if not self.robot.is_idle and not self.robot.game_start:
self.robot.toIdle()
if self.robot.is_idle:
if self.robot.game_start:
self.robot.toChase()
if self.robot.is_chase:
if self.robot.CheckBallHandle():
self.robot.toAttack()
if self.robot.is_attack:
if not self.robot.CheckBallHandle():
self.robot.toChase()
elif abs(targets[self.robot.opp_side]['ang']) < self.robot.atk_shoot_ang and \
abs(targets[self.robot.opp_side]['dis']) < self.robot.atk_shoot_dis:
self.robot.toShoot(100)
if self.robot.is_shoot:
self.robot.toAttack()
if rospy.is_shutdown():
break
## Keep Current State Running
keepState = 'to' + self.robot.current_state.name
getattr(self.robot, keepState)()
self.rate.sleep()