def get_cylinder_status(self):
self.cylinder_client.wait_for_service()
res = self.cylinder_client(True)
current_angular = start_angular = get_robot_position.robot_position_state().get_robot_current_w()
move_velocity = g_msgs.Twist()
move_velocity.angular.z = -0.13
x = res.z
theta = res.theta
iscylinder = res.has_cylinder
delta_angular = current_angular - start_angular
flag = 0
r = rospy.Rate(50)
while not rospy.is_shutdown() and not iscylinder:
current_angular = get_robot_position.robot_position_state().get_robot_current_w()
delta_angular = current_angular - start_angular
delta_upper_limit = abs(math.pi * 2 / 3) + 0.04 #误差上限
delta_lower_limit = abs(math.pi * 2 / 3) - 0.04 #误差下限
self.cmd_vel_pub.publish(move_velocity)
res = self.cylinder_client(True)
x = res.z
theta = res.theta
iscylinder = res.has_cylinder
if (abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit):
flag = 1
if iscylinder == 1 or (abs(delta_angular)<=delta_upper_limit and abs(delta_angular) >= delta_lower_limit):
self.brake()
break
r.sleep()
if flag == 1:
turn_an_angular.turn_an_angular().turn_to(-math.pi/2)
x = 2.55
return (x,theta)
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 = 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.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.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.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!")
def __init__(self):
smach.State.__init__(self, outcomes=['successed'])
self.turn_cmd = turn_an_angular.turn_an_angular()
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.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 = 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()
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'])
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()
############################################
#######shoot_ball_second and third##########
############################################
#先移动到三分线上的捡球位置，在移动到定位柱附近
class Shoot_Adjust_Second(smach.State):
def __init__(self):
smach.State.__init__(self,outcomes=['successed','failed'],input_keys=['ball_x', 'ball_y'])
self.move_cmd = linear_move.linear_move()
<<<<<<< HEAD
<<<<<<< HEAD
self.move_cmd_low = low_speed_linear_move.low_speed_linear_move()
=======
>>>>>>> c23cd36a28263fa7e748e644f0229d201acd5592
self.cmd_position = get_robot_position.robot_position_state()
self.turn_cmd = turn_an_angular.turn_an_angular()
def execute(self, ud):
<<<<<<< HEAD
if self.preempt_requested():
self.service_preempt()
return 'failed'
(current_x,current_y) = self.cmd_position.get_robot_current_x_y()
self.move_cmd.move_to(x = ud.ball_x - current_x,y = ud.ball_y - current_y)
# self.move_cmd.move_to(x = ud.ball_x - current_x - 1,y = ud.ball_y - current_y + 1)
# self.move_cmd_low.move_to(x = 1, y = -1)
(current_x,current_y,current_w) = self.cmd_position.get_robot_current_x_y_w()
self.move_cmd.move_to(x = 7.2 - current_x,y = -8.8 - current_y,yaw= -math.pi/7 -current_w)
return "successed"
# self.turn_cmd.turn_to(-math.pi / 8)
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'
(current_x,current_y) = self.cmd_position.get_robot_current_x_y()
self.move_cmd.move_to(x = 6.3 - current_x,y = -0.65 - current_y)
angular = math.atan2((-6.7 - current_y),(8.5 - current_x))
=======
self.move_cmd.move_to(x = -0.8)
