def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = linear_move.linear_move()
# self.move_cmd = move_in_robot.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo("the Move Point_Pro is initial OK!")
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
self.cmd_shovel = control_srv.shovelControlSrv()
rospy.loginfo('The Shoot_Adjust is initial ok!!!!')
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.cmd_position = get_robot_position.robot_position_state()
self.cmd_turn = turn_an_angular.turn_an_angular()
self.cmd_move = linear_move.linear_move()
self.cmd_return = go_close_line.move_to_home()
rospy.loginfo('The Return is initial ok!!!!')
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.turn_cmd = turn_an_angular.turn_an_angular()
self.move_cmd = linear_move.linear_move()
self.cmd_position = get_robot_position.robot_position_state()
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('The Shoot_Adjust is initial ok!!!!')
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('the Move To Another Ball 2 is initial OK!')
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
rospy.loginfo("the Move_To_Three_Point_Line is initial OK!")
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_shovel = control_srv.shovelControlSrv()
rospy.loginfo("the Move To Find BAll is initial Ok!")
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'],input_keys=['ball_x', 'ball_y'])
self.move_cmd = linear_move.linear_move()
self.move_cmd_low = low_speed_linear_move.low_speed_linear_move()
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
self.cmd_position = get_robot_position.robot_position_state()
self.turn_cmd = turn_an_angular.turn_an_angular()
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'],output_keys=['ball_x', 'ball_y'])
self.move_cmd = linear_move.linear_move()
self.move_cmd_low = low_speed_linear_move.low_speed_linear_move()
self.cmd_position = get_robot_position.robot_position_state()
self.turn_cmd = turn_an_angular.turn_an_angular()
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.move_cmd = linear_move.linear_move()
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'],input_keys=['ball_x', 'ball_y'])
self.move_cmd = linear_move.linear_move()
self.cmd_position = get_robot_position.robot_position_state()
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = linear_move.linear_move()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('We are ready to go home!!!')
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.cmd_position = get_robot_position.robot_position_state()
self.move_cmd = linear_move.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
rospy.loginfo("the Move_To_Three_Point_Line is initial OK!")
(current_x,current_y) = self.cmd_position.get_robot_current_x_y()
self.cmd_move.move_to(x = 2.5-current_x,y = -2-current_y)
# self.cmd_move.move_to(y = -current_y)
self.cmd_turn.turn_to(math.pi)
self.cmd_return.go_close_line()
# self.cmd_move.move_to(x = -2.6)
return 'successed'
#铲球后调整机器姿态，使其正对传球目标区域中心
class Shoot_Adjust(smach.State):
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
<<<<<<< HEAD
<<<<<<< HEAD
self.move_cmd = linear_move.linear_move()
=======
self.move_cmd = move_in_robot.linear_move()
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
=======
self.move_cmd = move_in_robot.linear_move()
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('The Shoot_Adjust is initial ok!!!!')
def execute(self, ud):
rospy.loginfo('Start Shoot Adjust')
<<<<<<< HEAD
<<<<<<< HEAD
if self.preempt_requested():
