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!!')