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 = 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.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):
rospy.loginfo('[robot_move_pkg]->move_an_angular is initial')
self.robot_state = robot_state.robot_position_state()
self.speed = config.high_turn_angular_speed
self.stop_tolerance = config.high_turn_angular_stop_tolerance
# self.turn_scale = config.turn_angular_scale
self.cmd_vel_pub = rospy.Publisher('/cmd_move' , g_msgs.Twist , queue_size=100)
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):
self.find_ball_client = rospy.ServiceProxy('oneball_data',CatchOneBall)
self.cmd_vel_pub = rospy.Publisher('/cmd_move' , g_msgs.Twist , queue_size=100)
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('waiting for the find ball..')
self.find_ball_client.wait_for_service()
rospy.loginfo('connect to the find ball!!!')
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, Type):
self.cmd_angular_pub = rospy.Publisher('/robot_move/world_velocity',
g_msgs.Twist,
queue_size=100)
self.find_ball_client = rospy.ServiceProxy('visionDate', visionDate)
self.rotate_client = rospy.ServiceProxy('/robot_move/rotate_cmd_srv',
robot_rotate)
self.current_position = get_robot_position.robot_position_state()
self.requiredType = Type
def __init__(self):
rospy.loginfo('[robot_move_pkg]->move_in_robot is initial')
# 通过这个模块获取机器人当前姿态
self.robot_state = robot_state.robot_position_state()
self.cmd_move_pub = rospy.Publisher('/cmd_move_robot', g_msgs.Twist, queue_size = 100)
self.rate = rospy.Rate(100)
# 设置机器人直线移动阈值
self.linear_move_stop_tolerance = config.low_speed_move_stop_tolerance
self.turn_move_stop_tolerance = config.low_turn_angular_stop_tolerance
self.x_speed = 0.0
self.y_speed = 0.0
self.w_speed = config.low_turn_angular_speed
# 设置移动过程中的速度
self.speed = config.low_linear_speed
def __init__(self):
rospy.loginfo('[robot_move_pkg]->low_speed_linear_move is initial')
#通过这个模块获取机器人当前姿态
self.robot_state = robot_state.robot_position_state()
self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100)
self.rate = rospy.Rate(100)
#设置机器人移动阈值
self.stop_tolerance = config.low_speed_move_stop_tolerance
#通过这个模块修正最终姿态角
self.accurate_turn_an_angular = turn_an_angular.turn_an_angular()
self.x_speed = 0.0
self.y_speed = 0.0
self.w_speed = 0.0
#设置移动过程中的速度
self.speed = config.low_linear_speed
def __init__(self):
self.find_ball_client = rospy.ServiceProxy('volleyball_data',CatchVolleyball)
self.cmd_vel_pub = rospy.Publisher('cmd_move_robot' , g_msgs.Twist , queue_size=100)
self.cmd_position = get_robot_position.robot_position_state()
self.move_speed = 0.21
#注册停止回调函数
rospy.on_shutdown(self.brake)
#用来决定走弧线方向的参数
self.MODE = { 1:(-1, 1),
2:( 1,-1),
3:( 1, 1),
4:(-1,-1)}
rospy.loginfo('waiting for the find ball..')
self.find_ball_client.wait_for_service()
rospy.loginfo('connect to the find ball!!!')
def __init__(self):
rospy.loginfo('[robot_move_pkg]->linear_move is initial')
#通过这个模块获取机器人当前姿态
self.robot_state = robot_state.robot_position_state()
self.cmd_move_pub = rospy.Publisher('/cmd_move', g_msgs.Twist, queue_size = 100)
self.rate = rospy.Rate(100)
#设置机器人直线移动阈值
self.stop_tolerance = config.high_speed_stop_tolerance
self.angular_tolerance = config.high_turn_angular_stop_tolerance
#通过这个模块修正最终姿态角
self.accurate_turn_an_angular = turn_an_angular.turn_an_angular()
self.x_speed = 0.0
self.y_speed = 0.0
self.w_speed = config.high_turn_angular_speed
#进行线速度插值
self.linear_sp = spline_func.growth_curve()
def __init__(self):
#向机器人坐标系发速度 m/s
self.move_cmd_pub = rospy.Publisher('cmd_move_robot',g_msgs.Twist,queue_size=100)
self.move_speed = config.go_along_circle_speed
self.get_position = robot_state.robot_position_state()
#设置停止时角度阈值 rad
self.stop_tolerance = config.go_along_circle_angular_tolerance
#注册停止回调函数
rospy.on_shutdown(self.brake)
# 设置sleep 频率， 这将影响圆弧的近似程度
self.rate = 100.0
self.R = rospy.Rate(int(self.rate))
self.MODE = { 1:(-1, 1),
2:( 1,-1),
3:( 1, 1),
4:(-1,-1)}
def __init__(self):
#通过这个模块获取机器人当前姿态
self.robot_state = robot_state_pkg_path.robot_position_state()
self.cmd_move_pub = rospy.Publisher('/cmd_move_robot',
g_msgs.Twist,
queue_size=100)
self.rate = rospy.Rate(10)
#设置机器人直线移动阈值
self.stop_tolerance = 0.015
#通过这个模块修正最终姿态角
self.accurate_turn_an_angular = turn_an_angular.turn_an_angular()
self.x_speed = 0.0
self.y_speed = 0.0
self.w_speed = 0.0
#进行线速度插值
self.linear_sp = growth_curve()
#amend_speed是单一方向运动时矫正速度
self.amend_speed = 0.06
self.x_y_amend_speed = 0.05
def __init__(self):
rospy.loginfo('[robot_move_pkg]->move_an_angular is initial')
self.robot_state = robot_state.robot_position_state()
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.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('We are ready to go home!!!')
def __init__(self):
self.robot_state = robot_state_pkg_path.robot_position_state()
self.cmd_move_pub = rospy.Publisher('/world_locate', g_msgs.Twist, queue_size = 100)
self.rate = rospy.Rate(10)
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'],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()
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('the Move To Another Ball 2 is initial OK!')
############################################
#######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)
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):
rospy.loginfo('[robot_move_pkg]->move_a_distance is initial')
self.robot_state = robot_state.robot_position_state()
self.stop_tolerance = config.high_speed_stop_tolerance
self.cmd_move_pub = rospy.Publisher('/cmd_move',g_msgs.Twist , queue_size=100)
self.vel_sp = sp_func.growth_curve()
def __init__(self):
self.robot_location = robot_state.robot_position_state()
self.move_cmd = world_move.world_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'])
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']
, output_keys=['ball_x', 'ball_y'])
self.cmd_position = get_robot_position.robot_position_state()
rospy.loginfo('Start search the ball!!')
