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)