def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = move_in_robot.linear_move()
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_shovel = control_srv.shovelControlSrv()
self.find_ball = find_basketball.find_basketball()
self.cmd_position = get_robot_position.robot_position_state()
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = move_in_robot.linear_move()
self.cmd_position = get_robot_position.robot_position_state()
self.cmd_shoot = control_srv.shootControlSrv()
self.cmd_shovel = control_srv.shovelControlSrv()
self.find_cylinder = find_cylinder.find_cylinder()
rospy.loginfo('The shoot is initial ok!')
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'])
self.turn_cmd = turn_an_angular.turn_an_angular()
self.cmd_position = get_robot_position.robot_position_state()
self.cmd_move_robot = move_in_robot.linear_move()
rospy.loginfo('the Shoot_Adjust_Third is initial OK!')
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'])
self.move_cmd = move_in_robot.linear_move()
rospy.loginfo("the Move Adjust is initial OK!")
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'], input_keys=['ball_x', 'ball_y', 'ball_theta'])
self.move_cmd = move_in_robot.linear_move()
rospy.loginfo("the Move Point is initial OK!")
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed'], input_keys=['column_x', 'column_theta'])
self.move_cmd = move_in_robot.linear_move()
rospy.loginfo("the Shoot Ajdust is initial OK!")
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed']
, output_keys=['ball_x', 'ball_y', 'ball_theta'])
self.move_cmd = move_in_robot.linear_move()
rospy.loginfo('Start Find the ball!!')
(ud.ball_x,ud.ball_y,ud.ball_theta) = find_volleyball.find_volleyball().find_ball_turn_cw()
=======
(ud.ball_x,ud.ball_y,ud.ball_theta) = find_volleyball.find_volleyball.find_ball_turn_cw()
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
=======
(ud.ball_x,ud.ball_y,ud.ball_theta) = find_volleyball.find_volleyball.find_ball_turn_cw()
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
return 'successed'
class Find_Ball(smach.State):
def __init__(self):
smach.State.__init__(self, outcomes=['successed', 'failed']
, output_keys=['ball_x', 'ball_y', 'ball_theta'])
<<<<<<< HEAD
<<<<<<< HEAD
self.move_cmd = move_in_robot.linear_move()
rospy.loginfo('Start Find the ball!!')
def execute(self, ud):
if self.preempt_requested():
self.service_preempt()
return 'failed'
(ball_x,ball_y,ball_theta) = find_volleyball.find_volleyball().find_volleyball()
self.move_cmd.turn_to(ball_theta)
self.move_cmd.move_to(x = math.sqrt(ball_x * ball_x + ball_y * ball_y) - 1.2)
=======
=======
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
rospy.loginfo('Start Find the ball!!')
def execute(self, ud):
# 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():
self.service_preempt()
return 'failed'